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

前言

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

扩展卡尔曼滤波实例

这里以无人驾驶的测量障碍物的实际案例为例子展开，如下图所示，毫米波雷达能够测量障碍物在极坐标下离雷达的距离

、方向角

以及距离的变化率（径向速度）

，如下图所示，

图1

• 预测更新

以二维匀速运动为例，这里的状态量

为：

根据牛顿运动定理，经过

时间后的预测状态量为：

对于二维的匀速运动模型，加速度为0，并不会对预测后的状态造成影响，因此令

由此可得状态转移矩阵为

毫米波雷达测量障碍物在径向上的位置和速度相对准确，不确定度较低，因此可以对状态协方差矩阵P进行如下初始化：

由于Q对整个系统存在影响，但又不能太确定对系统的影响有多大。对于简单的模型来说&#