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

Kalman Filter 是处理连续变化的动态不确定系统的理想方法，并且由于内存占用小(不需要记录历史状态)，运行速度快，被广泛应用在机器人实时多传感器融合系统中。

What can we do with a Kalman filter

首先看一个简单的例子: 假设有一个可以在树林中自由漫步的机器人，这个机器人配备了一个精度为 10m 的 GPS 传感器和自身状态的测量设备(轮速记等)。

对于机器人而言，除了能够通过 GPS 获取位置信息外，它还准确知道自己下达的所有指令，比如向前前进 10m，向右前进 5m 等等。但是由于受到外部环境的影响(风向、地面打滑，测量误差等)，机器人自身获得的测量数据与实际行驶的距离并不完全吻合。

树林中沟壑、悬崖遍布，不准确的定位信息使得机器人时时都有坠落悬崖的危险。

GPS 的测量信息和机器人自身的测量信息都不准确，如何利用这些不确定的信息获取更加确定的、更加准确的信息。Kalman Filter 可以用来解决这个问题。

Describing the problem with matrices

机器人在 k 时刻的 State 的矩阵形式如下:

假设已知机器人在 k-1 时刻的 State，要预测 k 时刻的 State。

预测的过程可以表述为矩阵变换的过程，变化矩阵为

。

根据基础的动力学知识:

用矩阵表示:

随机变量的乘以矩阵之后，对协方差矩阵的影响如下：

因此:

External influence

在机器人运动过程中，外力会对系统的 State 产生影响。比如系统会发出指令进行加速、减速等。这些外力是明确已知的，如何对系统产生影响也是明确的。这些信息统统被放进

中。

假设我们已知系统发出的加速的指令，产生的加速度为

，基于基础的动力学知识:

写成矩阵形式:

被称为 Control Matrix，

被称为 Control Vector。

External uncertainty

在运动过程中，除了机器人自身的属性(位置、速度)和已知的外力作用之外，还有一些未知的外部环境因素影响带来新的 uncertainty。

这些 Untracked Influence 可以用协方差为

的 Noise 来表达。

机器人 State 中的所有随机变量的 Noise 均服从均值相同、方差不同的正态分布。

增加 External uncertainty 之后的 Prediction 方程如下：

可以看出:

New Best Estimate = Previous Best Estimate + Known External Influences

New Uncertainty = Old Uncertainty + Additional Uncertainty From The Environment

Refining Estimate With Measurements

传感器可以产生一系列的测量结果，这些测量数据用来对 Estimate State 进行校准。

传感器读数和 Trace State 的 Unit 和 Scale 可能不同，所以需要用矩阵

进行变换。

传感器读数的分布如下:

Kalman Filter 的一个强大之处就在于，它可以处理传感器噪声(Sensor Noise)。如下图所示，传感器的读数是不准确的，在一定范围内波动，服从正态分布。

至此，我们得到两个高斯分布，一个是我们预测的值(Predicted Measurement)，另外一个是从传感器设备读取的值(Observed Measurement).

我们记传感器的噪声的协方差为:

，均值为:

。两个高斯分布如下图所示:

将两个分布相乘就得到两种情况同时发生的概率。如下图重叠区域所示，事实上，重叠区域仍然服从高斯分布。

高斯分布融合

一维高斯分布的概率密度函数如下:

两个高斯函数的乘积仍然服从高斯分布:

其中:

同样的，对于多维高斯分布，有：

被称为 Kalman Gain.

Putting all together

现在我们有两个高斯分布的测量结果:Predicted Measurement 和 Observed Measurement。

1） Predicted Measurement

2） Observed Measurement

根据多维高斯分布融合理论:

其中 Kalman gain 如下:

我们对上述方程进行化简，去除头部的

和尾部的

，得到如下的更新方程:

Kalman Filter 的运行流程图

Kalman Filter 与 Recursive Least Square

Least Square 解决的是静态参数估计的问题，Kalman Filter 可以解决动态变化的状态的估计和更新问题。

对比 KF 与 RLS 的过程:

KF:

预测

测量更新

RLS：

可以看出，KF 比 RLS 相比，增加了基于 Motion Model 的 Prediction 过程，用于跟踪 State 是如何随时间变化的。

参考链接

https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/