在机器人定位中，KF或者EKF是常用的传感器融合算法，之前也总结过很多关于EKF的用法：
如何理解卡尔曼滤波(Kalman Filter)实现数据融合
通俗易懂理解扩展卡尔曼滤波EKF用于多源传感器融合
大部分都是基于MATLAB写的，所以正好有空简单的写一下基于C++的计算方法，并且和 bfl 进行对比。
简单的来说，EKF 分为两个过程，预测和更新，预测的部分一般使用的是数据频率比较高的传感器，例如IMU或者Odom数据，这里为了对比效果，使用了 bfl 例子中的仿真传感器数据，首先先对bfl中的例子，做一个简单的解析：
for (time_step = 0;
time_step < NUM_TIME_STEPS-1;
time_step++)
{
auto start = std::chrono::steady_clock::now();
// DO ONE STEP WITH MOBILE ROBOT
mobile_robot.Move(input);
// DO ONE MEASUREMEN
ColumnVector measurement = mobile_robot.Measure();
// UPDATE FILTER
// filter.Update(&sys_model,input,&meas_model,measurement);
filter.Update(&sys_model,input);
filter.Update(&meas_model,measurement);
measurement = mobile_robot.Measure();
x = filter.PostGet()->ExpectedValueGet();
covarLabelMeas = filter.PostGet()->CovarianceGet();
cout << " ExpectedValueGet = " <具体的代码如上，后面会继续更新基于C++的非线性卡尔曼滤波，也就是考虑了转向角的融合。
推荐阅读
- 自动驾驶|手握全球最大ADAS激光雷达订单（这家公司股价却急速“跌落”）
- 激光雷达和三维点云PCL|自动驾驶中激光雷达如何检测障碍物
- 神经网络|信息瓶颈提出者Naftali Tishby生前指导，129页博士论文「神经网络中的信息流」公布...
- 算法|js算法学习——选择排序
- 算法|[JS][dfs]题解 | #迷宫问题#
- leetcode|leetcode:词典中最长的单词
- LeetCode笔记|力扣4.寻找两个正序数组的中位数(java)
- 数据库|30 道 MySQL 面试题全放送！
- C++|UE4智能指针TSharedPtr的几种初始化方式