c++ 卡尔曼滤波 代码

以下是一个简单的卡尔曼滤波代码示例，用于预测机器人的位置和速度：

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

using namespace Eigen;

int main()
{
    // 初始化卡尔曼滤波器
    int n = 2; // 状态向量的大小
    int m = 1; // 观测向量的大小
    MatrixXd A(n, n); // 状态转移矩阵
    MatrixXd C(m, n); // 观测矩阵
    MatrixXd Q(n, n); // 过程噪声协方差矩阵
    MatrixXd R(m, m); // 观测噪声协方差矩阵
    MatrixXd P(n, n); // 估计误差协方差矩阵
    VectorXd x_hat(n); // 状态向量的估计值
    VectorXd z(m); // 观测向量

    // 初始化参数
    A << 1, 1, 0, 1;
    C << 1, 0;
    Q << 0.05, 0.05, 0.05, 0.05;
    R << 5;
    P << 1, 0, 0, 1;
    x_hat << 0, 0;
    z << 0;

    // 模拟机器人运动
    double dt = 0.1; // 时间步长
    double v = 1; // 速度
    double w = 0.1; // 角速度
    double x = 0; // 位置
    for (int i = 0; i < 100; i++) {
        x = x + v * cos(w * i * dt);
        z << x + 0.1 * randn();
        // 预测下一时刻状态和估计误差协方差
        MatrixXd x_hat_new = A * x_hat;
        MatrixXd P_new = A * P * A.transpose() + Q;
        // 计算卡尔曼增益
        MatrixXd K = P_new * C.transpose() * (C * P_new * C.transpose() + R).inverse();
        // 更新状态估计值和估计误差协方差
        x_hat = x_hat_new + K * (z - C * x_hat_new);
        P = (MatrixXd::Identity(n, n) - K * C) * P_new;
        // 输出结果
        std::cout << "x = " << x << ", x_hat = " << x_hat(0) << ", P = " << P << std::endl;
    }

    return 0;
}

这段代码使用Eigen库实现了卡尔曼滤波器。它根据机器人的位置和速度，以及观测噪声和过程噪声的协方差矩阵，预测机器人的下一个位置，并输出估计值和估计误差协方差。