从零开始学习自动驾驶系统(五)-扩展卡尔曼滤波Extend Kalman Filter

1.泰勒级数展开

如果函数

在点

具有任意阶导数,则

称为

在点

处的泰勒级数。我们可以使用泰勒级数来逼近非线性函数。

以

为例，它的泰勒级数如下:

假定在

处展开:

称为

在

处的二阶泰勒展开。可以看到距离

越近，二者的差别就越小，随着距离的增加，误差不断增加。

非线性函数线性化是将nonlinear function在展开点

附近进行一阶泰勒展开。

下图是sin(x)的函数图像以及在x=0处的一阶泰勒的函数图像，可以看到，在x=0附近，二者非常接近，一阶泰勒展开可以很好的逼近sin(x)。

图片来源: Coursera:State Estimation - Linear and Nonlinear Kalman Filters

2. 线性卡尔曼滤波 (linearized Kalman filter)

线性卡尔曼滤波通过一阶泰勒级数将非线性系统(nonlinear system)线性化，从而满足标准卡尔曼滤波对于线性化的要求。

2.1 非线性系统模型

假设非线性系统的模型如下:

Nonlinear Motion/Process Model：

其中

是Current State，

是Previous State，

是Input，

是Process Noise.

Nonlinear Measurement Model:

是Measurement，

是State，

是Measurement Noise。

Process Noise和Measurement Noise均服从0均值的正态分布：

其中函数

和h(.)都是非线性函数。

2.2 线性展开

Nonlinear Motion Model展开:

Nonlinear Measurement Model展开:

其中

被称为Jacobian Matrix。

于是在展开点附近，非线性系统就可以当做线性系统来处理.

根据标准卡尔曼滤波理论:

Predection

Measurement Update

EKF定位系统

以车辆定位为例，如下图所示，已知LandMark的位置参数

，车辆加速度

，车辆安装了角度测量的设备，能够测量与LandMark的视角。

图片来源: Coursera:State Estimation - Linear and Nonlinear Kalman Filters

已知车辆在k-1时刻的State的矩阵形式如下:

车辆的Motion Model：

车辆的Measurement Model:

误差模型服从正态分布：

计算Jacobian Matrix:

。

给定系统的初始值:

测量的时间间隔

，车辆的加速度，

，LandMark的位置信息

。

把上述信息代入Extend Kalman Filter:

可以看出，Motion Model给出的车辆位置为2.50,速度为4.0m/s;Measurement Model给出的车辆位置为2.51,速度为4.02m/s。

参考链接

https://www.embedded.com/design/connectivity/4025693/Using-nonlinear-Kalman-filtering-to-estimate-signals

Coursera : State Estimation and Localization for Self-Driving Cars

《Probabilistic Robotics》，中文翻译版本:《概率机器人》

《State Estimation for Robotics》，中文翻译版：《机器人学的状态估计》