C++实现卡尔曼滤波

#include <iostream>

using namespace std;

// 定义卡尔曼滤波参数
float Q = 0.1; // 系统过程噪声协方差  如果这个值设置得很小，那么滤波器将会更加相信模型的预测；而如果设置得很大，滤波器就会更加相信观测到的数据。
float R = 0.1; // 观测噪声协方差      如果这个值设置得很小，滤波器将会更加相信传感器的测量值；而如果设置得很大，滤波器就会更加相信模型的预测。
float X = 1;   // 状态估计            最终结果
float P = 1;   // 状态估计协方差      如果这个值设置得很小，滤波器将会更加相信状态估计值；而如果设置得很大，滤波器就会更加相信观测到的数据。

// 卡尔曼滤波函数
float kalmanFilter(float Z) {
    // 预测
    float X_pred = X;
    float P_pred = P + Q;
    // 更新
    float K = P_pred / (P_pred + R);
    X = X_pred + K * (Z - X_pred);
    P = (1 - K) * P_pred;
    return X;
}

int main() {
    // 给定的组数据
    float num_arr[] = {1,1.05,1.1,1,2,1.1,0.89,1.2,1,0.77,1,0.77};
    int n = sizeof(num_arr) / sizeof(float);
    // 对数据进行滤波并输出结果
    for(int i = 0; i < n; ++i) {
        float filtered_value = kalmanFilter(num_arr[i]);
        cout << "Filtered value at time " << i+1 << ": " << filtered_value << endl;
    }
    return 0;
}