【算法】卡尔曼滤波器进行障碍物跟踪

前言

最近看了障碍物跟踪的一些资料，大都提到了卡尔曼滤波器，想着动手去试试。手上的源码是Python的，Python有numpy库进行矩阵运算，对应C++，就需要Eigen库。因此学习卡尔曼滤波器基础知识，跟Eigen库的使用就是第一部分内容了。
卡尔曼滤波器干货学习文章传送门
C++实现—主要多看几个程序例程借助别人的思想自己理解一些
Eigen库的练习有很多，随便百度一下，主要是矩阵的初始化和赋值。

1.环境

1. Ubuntu18.04
2. C++
3. ROS
4. RViz
5. GAZEBO
6. Clion
7. Eigen

2.算法思路

3. 过程总结

1. Clion debug Eigen参数不是很方便可以使用gdb调试

(gdb) p *A.data()@4@4
$2 = {{1, 0, 0, 0}, {0, 1, 0, 0}, {1, 0, 1, 0}, {0, 1, 0, 1}}

其中@4@4表示4行4列，p表示打印。

2. 注意int 和 float 变量之间的运算，最好统一为float型
3. 单位矩阵初始化最好这样

A <<1,0,1,0,
        0,1,0,1,
        0,0,1,0,
        0,0,0,1;
static MatrixXf H(4,4);
H <<1,0,0,0,
        0,1,0,0,
        0,0,1,0,
        0,0,0,1;
static MatrixXf Q(4,4);
Q = MatrixXf::Identity(4,4)*0.1；

其中

Q.Identity()是正则化，不是初始化，这个地方太坑了，调了半天bug，怪自己不熟悉，不认真。

4. 激光雷达数据聚类我采用DBSCAN算法

4.最终效果

视频链接