卡尔曼滤波算法_导航系统设计专题（五）—扩展卡尔曼滤波（2）

0 前言

       上一篇围绕卡尔曼滤波算法的参数选取问题展开，针对非线性对象的状态估计问题，阐述扩展卡尔曼滤波(EKF)与卡尔曼滤波的区别以及扩展卡尔曼滤波算法的核心步骤。本篇将结合实际案例进行详解，同时，提供一种扩展卡尔曼滤波算法的C++代码实现方案。

1 扩展卡尔曼滤波实例

      这里以无人驾驶的测量障碍物的实际案例为例子展开，如下图所示，毫米波雷达能够测量障碍物在极坐标下离雷达的距离 、方向角 以及距离的变化率(径向速度)，如下图所示。

预测更新

量测更新

此时，我们需要对H矩阵进行泰勒展开(线性化)，得到其Jacobian矩阵，求解方法详解上一篇，这里不再赘述。

2 扩展卡尔曼滤波的C++实现方案

      完成以上推导后，我们将推导的过程写成代码。这里采用矩阵运算库Eigen进行矩阵运算，其运算效率较高，与自定义的矩阵运算库相比也更省心。

ExtendedKalmanFilter类的实现

扩展卡尔曼滤波器的调用

3 总结

      本篇围绕一个简单的案例展开，详解扩展卡尔曼滤波算法的实现，同时，提供了一种采用Eigen矩阵运算库的c++实现方案。下篇中，我们将围绕多传感器状态估计的最优估计问题展开论述。