C++实现卡尔曼滤波（完整代码）

最新推荐文章于 2024-03-05 11:06:37 发布

码农农的啊豪

最新推荐文章于 2024-03-05 11:06:37 发布

阅读量605

点赞数 2

分类专栏： slam 文章标签： c++ 开发语言

本文链接：https://blog.csdn.net/weixin_46226429/article/details/133797122

版权

slam 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

悄悄话：

找了半天，发现都没有人发布C++实现的卡尔曼滤波的文章，故我琢磨了一会，自己写一遍，提供给大家参考。

话不多说，相信大家都是了解过卡尔曼滤波，这里就不多介绍了，直接上公式！

实现：

1.1 前提

以小车做匀加速直线运动为例，那么便可以写出预测模型和测量模型，便可以知道F、B、H的对应的值。

1.2 代码

#include "matplotlibcpp.h"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <time.h> 
using namespace std;

namespace plt = matplotlibcpp;

int main()
{
    // 造数据，平面小车以匀速加速度行驶，向前行驶
    double initX = 100;            // 初始位置
    double initAcc = 1.0;          // 初始加速度
    double initVel = 0;            // 初始速度
    vector<double> real_pos, sys_time; // 真实的小车路径（P—t）
    for (double t = 1; t < 40; t += 1)
    {
        real_pos.push_back(initX + initVel * t + 0.5 * initAcc * t * t);
        sys_time.push_back(t);
    }

    // 模拟噪声
    srand((unsigned)time(NULL)); 
    vector<double> detect_pos;
    for (double data : real_pos) {
        detect_pos.push_back((data + (rand() % (101)) - 50));
    }

    // 卡尔曼滤波
    double cutPos = 95, // 观测到当前位置
            cutAcc = 1, // 观察到当前加速度, 高斯分部
            cutVel = 0; // 观测到当前速度
    
    Eigen::Matrix2d F; // 预测模型的F
    Eigen::Vector2d B; // 预测模型的B
    Eigen::Matrix2d H; // 测量模型的H
    Eigen::Matrix2d Q,R; // Q与R
    Eigen::Matrix2d covP; // 协方差
    Eigen::Vector2d initState;
    vector<double> optimi_pos;
    double VarTime = sys_time[1] - sys_time[0];
    H << 1,0,0,1; // 转移方程
    Q << 1e-5,0,0,1e-5; 
    R << 0.001,0,0,0;
    covP << 1,0,0,1; //协方差一开始为1，后续会迭代收敛
    initState << detect_pos[0], 0; // 初始状态值， 第一个为位移、第二个为速度
    F << 1,VarTime,0,1;
    B << 0.5 * VarTime * VarTime, VarTime;
    for (int i=0;i<detect_pos.size() ;i++) {// 迭代
        Eigen::Vector2d z; // 设置观测状态向量，位移、速度
        z << detect_pos[i], i*cutAcc;
        Eigen::Matrix2d I;
        I.setIdentity(); // 矩阵单位化

        // 预测
        initState = F * initState + B * cutAcc; // 估计位移
        covP = F * covP * F.transpose() + Q; // 协方差
        
        // 更新
        Eigen::Matrix2d K = covP * H.transpose() * ( H * covP * H.transpose() + R).inverse(); // 卡尔曼增益
        initState = initState + K*(z - H.transpose() * initState); // 优化后的数据
        covP = (I - K) * covP; // 更新协方差
        optimi_pos.push_back(initState(0,0));
    }


    // 可视化
    plt::xlabel("time");
    plt::ylabel("Position");
    plt::named_plot("real_pos", sys_time, real_pos);
    plt::named_plot("detect_pos", sys_time, detect_pos);
    plt::named_plot("opti_pos", sys_time, optimi_pos);
    plt::title("KalmanFilter");
    plt::legend();
    plt::show();

    return 0;
}

注：matplotlib的使用，请看文章最后的参考3