用C++实现卡尔曼滤波

卡尔曼滤波是一种常用的状态估计方法，用于从带有噪声的测量数据中估计出一个未知的状态变量。下面是一个简单的 C++ 实现：

#include <iostream>
using namespace std;

class KalmanFilter {
public:
    KalmanFilter(float _x, float _p, float _q, float _r) {
        x = _x; // 初始状态值
        p = _p; // 初始协方差矩阵
        q = _q; // 过程噪声协方差
        r = _r; // 测量噪声协方差
    }

    float update(float z, float u) {
        // 预测步骤
        x = x + u;
        p = p + q;

        // 更新步骤
        float k = p / (p + r);
        x = x + k * (z - x);
        p = (1 - k) * p;

        return x;
    }

private:
    float x; // 状态变量
    float p; // 协方差矩阵
    float q; // 过程噪声协方差
    float r; // 测量噪声协方差
};

int main() {
    float z = 10; // 测量值
    float u = 1; // 输入值

    KalmanFilter kf(0, 1, 0.1, 1); // 初始化滤波器

    for (int i = 0; i < 10; i++) {
        float x = kf.update(z, u); // 更新状态
        cout << "x = " << x << endl;
    }

    return 0;
}

在上面的代码中，KalmanFilter 类表示一个卡尔曼滤波器，它有四个参数：初始状态值 x、初始协方差矩阵 p、过程噪声协方差 q 和测量噪声协方差 r。update 函数用于更新状态，它接收两个参数：测量值 z 和输入值 u。在 update 函数中，首先进行预测步骤，然后进行更新步骤。最后，update 函数返回更新后的状态值 x。

在主函数中，我们创建了一个 KalmanFilter 对象 kf，并进行了 10 次状态更新。每次更新都传入测量值和输入值，然后打印更新后的状态值 x。在实际应用中，我们可以根据需要调整 KalmanFilter 的参数，以达到更好的滤波效果。