Apollo卡尔曼滤波与EKF

原因

传感器存在误差——需要跟踪——卡尔曼滤波

由于传感器本身的特性，任何测量结果都是有误差的。

以障碍物检测为例，如果直接使用传感器的测量结果，在车辆颠簸时，可能会造成障碍物测量结果的突变，这对无人车的感知来说是不可接受的。

在传感器测量结果的基础上，进行跟踪，以此来保证障碍物的位置、速度等信息不会发生突变。

应用

障碍物跟踪、车道线跟踪、障碍物预测以及定位等领域；

原理

线性系统，且参数是符合高斯分布，即完全可以被均值和协方差参数化:X∼N(μ, σ²)。

1556226-20190213103029198-760523576.png

                                                        $ x_2=w_1*x_{pre}+w_2*z_2 $

预测状态（prediction）与观测状态(measurement)分别加权求和作为新时刻的状态向量。

• 预测根据前一周期的状态和运动学方程求得；
• 观测通过传感器测得的当前周期参数得到；
• 预测与观测均成高斯分布，两个高斯分布的乘积就是加权后的结果。新的高斯分布方差最小，确定性也最高。（两个不那么确定的分布，最终得到了一个相对确定的分布，这是卡尔曼滤波的一直被推崇的原因）

7大公式

prediction：

(状态协方差矩阵，表示系统不确定度)

measurement update:

（实际观测到的测量值 z 与预测值 x' 之间差值 y）

（当前状态向量 x 的更新）

（根据卡尔曼增益，更新了系统的不确定度 P）

其中，
F（state transition matrix）：状态转移矩阵(运动学推导)；

B：控制输入矩阵；

u：引起运动的输入向量（如油门、刹车、转角）；

w：过程向量中的噪声因素，符合高斯分布，w∼N(0,Q)；

P（state covariance matrix）：在卡尔曼滤波器初始化时会很大，随着越来越多的数据注入滤波器中，不确定程度会变小；

Q（process covariance matrix）：过程噪声，即无法用 x'=Fx+u 表示的噪声，比如车辆运动时突然到了上坡，这个影响是无法用之前的状态转移估计的；（由于 Q 对整个系统存在影响，但又不能太确定对系统的影响有多大。工程上，我们一般将 Q 设置为单位矩阵参与运算）

H（Measurement Matrix）：测量矩阵，由于传感器的测量值不一定就是直接的状态向量，如毫米波雷达是角度和位置信息，这里H就是转换矩阵，将预测向量转为测量数据；

R （measurement covariance matrix）：测量噪声矩阵，表示的是测量值与真值之间的差值。一般情况下，传感器的厂家会提供该值；

S 只是为了简化公式，写的一个临时变量；

K（Kalman Gain）：卡尔曼增益，也即y的权重。

问题：针对线性系统，且符合高斯分布。

EKF扩展卡尔曼滤波

当状态向量与测量量之间映射关系是非线性时，这将使得卡尔曼滤波的过程和测量符合高斯分布的假设无效。

如毫米波雷达，返回数据是基于极坐标系，包含：

• ρ /Range (从原点到此的距离)；

• ϕ /bearing (ρ 和 x 的夹角)；
• ρ˙：接近率 / 距离变化率；

此时，H转移矩阵（状态向量向测量数据转换），也即笛卡尔坐标向极坐标映射：

这个映射就是非线性函数。

EKF使用局部线性逼近非线性模型，通过计算当前状态估计的一阶泰勒展开得出。一阶的逼近也叫雅克比矩阵。

如对上泰勒展开（在均值u处，本例为0），
求出雅克比矩阵如下：

对比公式

image

问题：非线性系统线性化省略高阶项，导致误差；
另外当状态向量复杂时候，求取雅克比矩阵难度也会上升。

参考

1. Apollo 开发者社区 - 开发者说 | 手把手教你写卡尔曼滤波器

2.自动驾驶中的传感器融合算法：第一部分 - 卡尔曼滤波器和扩展卡尔曼滤波器

3. 使用扩展卡尔曼滤波器完成传感器融合和目标跟踪

4. sensor-fusion-ekf-reference.pdf

5.详解卡尔曼滤波原理