一文搞懂 SLAM 中的Extension Kalman Filter 算法编程

©作者 | Doreen

01 问题描述

预先知道事物未来的状态总是很有价值的！

√ 预知台风的路线可以避免或减轻重大自然灾害的损失。

√ 敌国打过来的导弹，如果能够高精度预测轨迹，就能有效拦截。

√ 操控无人机，需要知道下一刻飞机的方向、速度不断修正，才能精准控制、回避各种风险。

这是一个状态估计问题

如下图所描绘的，在 k 个（一个或多个）时间点上，

基于初始的状态信息

一系列观测数据

给定控制输入

以及系统的运动和观测模型，力求预测系统在每一时刻的真实状态

图 1.1: 状态估计问题示意图

以自动驾驶为例，图中符号的含义如下：

● xˇ: 设计的轨迹，比如预先计算得出的理想轨迹。

● w: 驾驶过程中各种操作引入的噪声，称为过程噪声。

● x: 在理想轨迹之上混入了过程噪声的真实轨迹。

● t 下标表示时间。

● n: 观测噪声。

● y: 观测数据：对真实轨迹的观测，其中包含观测噪声。

每一个 t − 1 时刻，系统处于

状态，输入控制信号

状态变为

这一过程用运动模型描述

此时观测到

这一个过程用观测模型描述

可以看出 t 时刻状态，在时间序列上只与 t − 1 时刻的状态有关，即具有一阶马尔科夫性。

02 EKF 算法

最开始人们对这一问题做了简化，假设模型是线性的，噪声符合高斯分布，提出了 Kalman Filter。然而总存在某些问题不符合线性与高斯假设，人们又继续探索。

2.1、Bayes Filter

由于系统状态与观测之间的因果关系，人们首先想到了贝叶斯定理。对于状态x, 由先验p(x), 后验概率 p(x|y), 推到观测对象发生的概率。

考虑到系统的马尔科夫性， t 时刻状态与 t − 1 时刻之前的状态和控制 输入都没有关系，忽略无关项，改写后如下。

2.2、算法改进

上述 Bayes