说不尽的卡尔曼 | 详解扩展卡尔曼滤波器

作者 | 火山 编辑 | 空中机器人前沿

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【目标跟踪】技术交流群

后台回复【卡尔曼滤波】获取卡尔曼滤波学习资料！

1  前言

在上一篇文章(卡尔曼滤波器实例与推导)中，我们介绍了线性卡尔曼滤波器的来源、推导及实例。但在很多实际工程问题当中，非线性系统占大多数，而卡尔曼提出来的卡尔曼滤波器是一种针对线性系统的估计算法[1]。 为了解决这一问题，Schmidt学者分析了卡尔曼滤波器中的系统模型部分，并提出将卡尔曼理论应用到非线性系统的扩展卡尔曼算法 Extended Kalman Filter（EKF）[2]。相比于线性卡尔曼滤波器，扩展卡尔曼滤波器有更广的适用范围[3, 4]，更高的状态估计精度，可以处理任意更新频率的系统。 有了上文的基础，这里，我们将通过一个二维小车的例子并来介绍扩展卡尔曼滤波的原理。

2  扩展卡尔曼滤波算法介绍

在介绍扩展卡尔曼滤波算法之前，我们先回忆一下之前介绍的卡尔曼滤波算法：

 

其中前两个公式为系统状态预测部分，后三个公式为系统的修正部分。其中整个系统最重要的是要通过公式将系统的模型和测量与状态之间的关系表示出来：

 

式中为系统矩阵，为控制矩阵，为观测矩阵。为目标在时刻的状态，为控制量，和分别为系统误差和测量误差。在线性卡尔曼滤波算法中，和是常值矩阵，其对应的系统就是线性系统。如果想将卡尔曼理论应用到非线性系统中，则需要对非线性系统进行线性化处理。

2.1  扩展卡尔曼滤波算法的不同

非线性系统方程及其观测方程通常可以表示为：

 

扩展卡尔曼滤波算法可以通过将非线性系统在其参考点处做泰勒级数展开，从而使非线性系统可以线性化。展开后，我们可以得到：

 

其中和是非线性系统函数在时刻求偏导数后的雅可比矩阵，和为非线性测量函数求偏导数后的雅克比矩阵。那么，扩展卡尔曼滤波算法可以写成：

 

介绍完公式，接下来我们将通过2维平面内的小车来对扩展卡尔曼滤波算法进行讲解。

3  二维小车实例

如果要估计一个在二维平面内运动的小车，首先要根据要求选择一个较为合适的系统模型。在二维平面内，小车的运动可能是直线也可能是曲线，那么在一维平面用的线性匀速模型constant velocity model(CV) 将不适用。这里将采用一个简单的非线性模型匀转速、速度模型 constant turning rate and velocity magnitude model(CTRV)。

3.1  非线性模型CTRV

ctrv模型是cv模型的一般形式。这里我们假设观测到的状态变量为：：
分别对应目标在,方向上的位置，速度，角度和角速度，变量之间的关系如图1所示。

图 1: 二维平面ctrv模型状态量关系

从图中可以看出，各变量之间的相互关系。那么，从时刻到时刻，状态的变化应该是： 

根据积分，我们可以得到下式：

 

这里，我们对系统函数求偏导得到其雅克比矩阵为

这里为了将系统误差为：

 
后续再套入到卡尔曼滤波器中，我们就可以得到一个CTRV的跟踪模型：

 
3.2  仿真

这里我们假设二维小车系统可以观测到目标的位置：

那么观测方程为：

 
仿真总时间10s，时间间隔，目标小车绕原点，半径以速度做匀速圆周运动。观测信息为目标的位置，测量误差。实时跟踪如下视频所示。

视频: 跟踪匀速圆周运动小车二维仿真视频

从视频中我们可以看到，不同的模型即使在当前估计状态相差不大的情况下，其估计目标未来的状态也是有很大差别的。在这个例子中，目标做的是匀速绕圈运动，CTRV模型更能描述系统的运动状态，但该模型是非线性系统。因此，需要结合EKF，来实现对非线性系统的状态跟踪和预测。

仿真结果如图2所示，红色轨迹为小车真实轨迹，红点为测量小车位置，蓝色圆圈为CV模型估计的小车当前位置，绿色圆圈为CTRV模型估计的小车当前位置，绿色实线为CTRV模型估计小车轨迹，绿色虚线为CTRV模型预测未来一段时间内的小车轨迹，蓝色实线为CV模型估计小车轨迹，蓝色虚线为CV模型预测未来一段时间内的小车轨迹。可以看出CTRV模型的预测比CV模型的预测在绕圈运动中更为准确。

4  总结

在很多实际工程问题中，系统的描述总存在不同程度的非线性，如平方、三角函数等。针对这一问题，EKF的思路是将非线性系统线性化转换为近似的线性系统，然后应用卡尔曼滤波算法完成状态估计。

参考文献

[1] Rudolph Emil Kalman. A new approach to linear filtering and prediction problems. 1960.

[2] Leonard A McGee, Stanley F Schmidt, Leonard A Mcgee, and Stanley F Sc. Discovery of the kalman filter as a practical tool for aerospace and. In Industry,” National Aeronautics and Space Administration, Ames Research. Citeseer, 1985.

[3] Jakub Simanek, Michal Reinstein, and Vladimir Kubelka. Evaluation of the ekf-based estimation architectures for data fusion in mobile robots. IEEE/ASME Transactions on Mechatronics, 20(2):985–990, 2014.

[4] Yibing Wang and Markos Papageorgiou. Real-time freeway traffic state estimation based on extended kalman filter: a general approach. Transportation Research Part B: Methodological, 39(2):141–167, 2005.

国内首个自动驾驶学习社区

近1000人的交流社区，和20+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向；

添加汽车人助理微信邀请入群

备注：学校/公司+方向+昵称