卡尔曼滤波C++实现

卡尔曼滤波是一种用于估计系统状态的优化算法，它可以通过融合传感器测量值和系统模型来提高状态估计的准确性。在C++中，可以使用Eigen库来实现卡尔曼滤波算法。

以下是一个简单的卡尔曼滤波C++实现的示例：

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

using namespace Eigen;

class KalmanFilter {
public:
    KalmanFilter() {
        // 初始化状态向量
        x = Vector2d(0, 0);
        
        // 初始化状态协方差矩阵
        P = Matrix2d::Identity();
        
        // 初始化过程噪声协方差矩阵
        Q = Matrix2d::Identity();
        
        // 初始化观测噪声协方差矩阵
        R = Matrix2d::Identity();
        
        // 初始化状态转移矩阵
        A = Matrix2d::Identity();
        
        // 初始化观测矩阵
        H = Matrix2d::Identity();
    }
    
    void predict() {
        // 预测步骤
        x = A * x;
        P = A * P * A.transpose() + Q;
    }
    
    void update(const Vector2d& z) {
        // 更新步骤
        Vector2d y = z - H * x;
        Matrix2d S = H * P * H.transpose() + R;
        Matrix2d K = P * H.transpose() * S.inverse();
        
        x = x + K * y;
        P = (Matrix2d::Identity() - K * H) * P;
    }
    
private:
    Vector2d x;  // 状态向量
    Matrix2d P;  // 状态协方差矩阵
    Matrix2d Q;  // 过程噪声协方差矩阵
    Matrix2d R;  // 观测噪声协方差矩阵
    Matrix2d A;  // 状态转移矩阵
    Matrix2d H;  // 观测矩阵
};

int main() {
    KalmanFilter kf;
    
    // 测量值
    Vector2d z(1, 1);
    
    // 预测步骤
    kf.predict();
    
    // 更新步骤
    kf.update(z);
    
    // 输出估计的状态向量
    std::cout << "Estimated state: " << kf.x.transpose() << std::endl;
    
    return 0;
}

这个示例实现了一个简单的二维卡尔曼滤波器。在`KalmanFilter`类中，`predict`函数用于进行预测步骤，`update`函数用于进行更新步骤。在`main`函数中，首先创建了一个`KalmanFilter`对象，然后设置了一个测量值`z`，接着进行预测和更新步骤，并输出估计的状态向量。