卡尔曼滤波 c++代码

以下是一个简单的卡尔曼滤波器的C代码实现：

#include <stdio.h>

#define R 0.1
#define Q 0.0001

float x_hat = 0.0;
float P = 1.0;

float kalman_filter(float z) {
    // Prediction step
    float x_hat_minus = x_hat;
    float P_minus = P + Q;

    // Update step
    float K = P_minus / (P_minus + R);
    x_hat = x_hat_minus + K * (z - x_hat_minus);
    P = (1 - K) * P_minus;

    return x_hat;
}

int main() {
    float measurements[] = {1.1, 1.5, 2.0, 1.7, 1.9, 2.2};
    int num_measurements = sizeof(measurements) / sizeof(measurements[0]);

    for (int i = 0; i < num_measurements; i++) {
        float z = measurements[i];
        float x = kalman_filter(z);
        printf("Measurement: %f, Filtered: %f\n", z, x);
    }

    return 0;
}

在这个例子中，我们使用卡尔曼滤波器来对一系列测量值进行平滑处理。我们首先定义了一些常量，包括测量噪声方差R和过程噪声方差Q。在函数`kalman_filter`中，我们执行了卡尔曼滤波器的预测步骤和更新步骤，并返回滤波后的值。在主函数中，我们遍历测量值数组，并对每个值进行滤波。最后，我们打印出每个测量值和对应的滤波值。

请注意，这只是一个简单的卡尔曼滤波器实现，您可以根据需要调整常量和函数实现，以满足您的特定应用程序的需求。