卡尔曼滤波算法C++代码

以下是一个简单的卡尔曼滤波算法的C++代码示例：

#include <iostream>
#include <vector>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

// 卡尔曼滤波类
class KalmanFilter {
public:
    KalmanFilter(const VectorXd& x0, const MatrixXd& P0, const MatrixXd& A,
                 const MatrixXd& Q, const MatrixXd& H, const MatrixXd& R)
        : x(x0), P(P0), A(A), Q(Q), H(H), R(R) {}

    void predict() {
        x = A * x;
        P = A * P * A.transpose() + Q;
    }

    void update(const VectorXd& z) {
        MatrixXd S = H * P * H.transpose() + R;
        MatrixXd K = P * H.transpose() * S.inverse();
        VectorXd y = z - H * x;
        x = x + K * y;
        P = (MatrixXd::Identity(P.rows(), P.cols()) - K * H) * P;
    }

    VectorXd getState() const { return x; }

private:
    VectorXd x; // 状态向量
    MatrixXd P; // 状态协方差矩阵
    MatrixXd A; // 状态转移矩阵
    MatrixXd Q; // 过程噪声协方差矩阵
    MatrixXd H; // 观测矩阵
    MatrixXd R; // 观测噪声协方差矩阵
};

int main() {
    // 初始化卡尔曼滤波器参数
    VectorXd x0(2); // 初始状态向量
    x0 << 0, 0;
    MatrixXd P0(2, 2); // 初始状态协方差矩阵
    P0 << 1, 0, 0, 1;
    MatrixXd A(2, 2); // 状态转移矩阵
    A << 1, 1, 0, 1;
    MatrixXd Q(2, 2); // 过程噪声协方差矩阵
    Q << 0.1, 0, 0, 0.1;
    MatrixXd H(1, 2); // 观测矩阵
    H << 1, 0;
    MatrixXd R(1, 1); // 观测噪声协方差矩阵
    R << 1;

    // 构造卡尔曼滤波器
    KalmanFilter kf(x0, P0, A, Q, H, R);

    // 模拟观测数据
    vector<double> measurements = {1.1, 2.0, 3.5, 4.9};
    vector<double> filteredResults;

    // 运行卡尔曼滤波器
    for (double z : measurements) {
        kf.predict();
        kf.update(VectorXd(1) << z);
        filteredResults.push_back(kf.getState()(0));
    }

    // 打印滤波结果
    cout << "滤波结果：" << endl;
    for (double result : filteredResults) {
        cout << result << endl;
    }

    return 0;
}

这段代码实现了一个简单