自动驾驶定位系统-卡尔曼滤波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的矩阵形式如下:

$$ \begin{aligned} \mathbf{\hat{x}}_k &= \begin{bmatrix} \text{position}\\ \text{velocity} \\ \end{bmatrix}\\ \mathbf{P}_k &= \begin{bmatrix} \Sigma_{pp} & \Sigma_{pv} \\ \Sigma_{vp} & \Sigma_{vv} \\ \end{bmatrix} \end{aligned} $$

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

预测的过程可以表述为矩阵变换的过程，变化矩阵为$\mathbf{F_k}$。

根据基础的动力学知识:

$$ \begin{aligned} {p_k} &= {p_{k-1}} + \Delta t {v_{k-1}} \\ {v_k} &= {v_{k-1}} \end{aligned} $$

用矩阵表示:

$$ \begin{aligned} {\mathbf{\hat{x}}_k} &= \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} {\mathbf{\hat{x}}_{k-1}} \\ &= \mathbf{F}_k {\mathbf{\hat{x}}_{k-1}} \end{aligned} $$

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

$$ \begin{aligned} Cov(x) &= \Sigma \\ Cov({\mathbf{A}}x) &= {\mathbf{A}} \Sigma {\mathbf{A}}^T \end{aligned} $$

因此:

$$ \begin{aligned} {\mathbf{\hat{x}}_k} &= \mathbf{F}_k {\mathbf{\hat{x}}_{k-1}} \\ {\mathbf{P}_k} &= \mathbf{F_k} {\mathbf{P}_{k-1}} \mathbf{F}_k^T \end{aligned} $$

External influence

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

假设我们已知系统发出的加速的指令，产生的加速度为${a}$，基于基础的动力学知识:

$$ \begin{aligned} {p_k} &= {p_{k-1}} + {\Delta t} {v_{k-1}} + \frac{1}{2} {a} {\Delta t}^2 \\ {v_k} &= {v_{k-1}} + {a} {\Delta t} \end{aligned} $$

写成矩阵形式:

$$ \begin{aligned} {\mathbf{\hat{x}}_k} &= \mathbf{F}_k {\mathbf{\hat{x}}_{k-1}} + \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} {a} \\ &= \mathbf{F}_k {\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k {\vec{\mathbf{u}}_k} \end{aligned} $$

$\mathbf{B}_k$ 被称为Control Matrix，${\vec{\mathbf{u}}_k}$被称为Control Vector。

External uncertainty

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

这些Untracked Influence可以用协方差为${\mathbf{Q}_k}$的Noise来表达。

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

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

$$ \begin{aligned} {\mathbf{\hat{x}}_k} &= \mathbf{F}_k {\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k {\vec{\mathbf{u}_k}} \\ {\mathbf{P}_k} &= \mathbf{F_k}{\mathbf{P}_{k-1}} \mathbf{F}_k^T + {\mathbf{Q}_k} \end{aligned} $$

可以看出:

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可能不同，所以需要用矩阵$\mathbf{H}_k$进行变换。

传感器读数的分布如下:

$$ \begin{aligned} \vec{\mu}_{\text{expected}} &= \mathbf{H}_k {\mathbf{\hat{x}}_k} \\ \mathbf{\Sigma}_{\text{expected}} &= \mathbf{H}_k {\mathbf{P}_k} \mathbf{H}_k^T \end{aligned} $$

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

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

我们记传感器的噪声的协方差为:${\mathbf{R}_k}$，均值为:${\vec{\mathbf{z}}_k}$。两个高斯分布如下图所示:

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

高斯分布融合

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

$$ \mathcal{N}(x, \mu,\sigma) = \frac{1}{ \sigma \sqrt{ 2\pi } } e^{ -\frac{ (x – \mu)^2 }{ 2\sigma^2 } } $$

Screenshot from 2020-02-15 21-40-25.png

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

$$ \mathcal{N}(x, {\mu_0}, {\sigma_0}) \cdot \mathcal{N}(x, {\mu_1}, {\sigma_1}) \stackrel{?}{=} \mathcal{N}(x, {\mu’}, {\sigma’}) $$

其中:

$$ {\mathbf{k}} = \frac{\sigma_0^2}{\sigma_0^2 + \sigma_1^2} $$

$$ \begin{aligned} {\mu’} &= \mu_0 + {\mathbf{k}} (\mu_1 – \mu_0)\\ {\sigma’}^2 &= \sigma_0^2 – {\mathbf{k}} \sigma_0^2 \\ \end{aligned} $$

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

$$ {\mathbf{K}} = \Sigma_0 (\Sigma_0 + \Sigma_1)^{-1} $$

$$ \begin{aligned} {\vec{\mu}’} &= \vec{\mu_0} + {\mathbf{K}} (\vec{\mu_1} – \vec{\mu_0})\\ {\Sigma’} &= \Sigma_0 – {\mathbf{K}} \Sigma_0 \end{aligned} $$

${\mathbf{K}}$被称为Kalman Gain.

Putting all together

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

1） Predicted Measurement

$$ ({\mu_0}, {\Sigma_0}) = ({\mathbf{H}_k \mathbf{\hat{x}}_k},{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T}) $$

2） Observed Measurement

$$ {\mu_1}, {\Sigma_1}) = ({\vec{\mathbf{z}}_k}, {\mathbf{R}_k}) $$

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

$$ \begin{aligned} \mathbf{H}_k {\mathbf{\hat{x}}_k’} &= {\mathbf{H}_k \mathbf{\hat{x}}_k} + {\mathbf{K}} ( {\vec{\mathbf{z}_k}} – {\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\ \mathbf{H}_k {\mathbf{P}_k’} \mathbf{H}_k^T &= {\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} –{\mathbf{K}} {\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} \end{aligned} $$

其中Kalman gain如下:

$$ {\mathbf{K}} = {\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} ( {\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + {\mathbf{R}_k})^{-1} $$

我们对上述方程进行化简，去除头部的$H_K$和尾部的$H_k^T$，得到如下的更新方程:

$$ \begin{aligned} {\mathbf{\hat{x}}_k’} &= {\mathbf{\hat{x}}_k} + {\mathbf{K}’} ( {\vec{\mathbf{z}_k}} – {\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\ {\mathbf{P}_k’} &= {\mathbf{P}_k} – {\mathbf{K}’} {\mathbf{H}_k \mathbf{P}_k} \end{aligned} $$

$$ {\mathbf{K}’} = {\mathbf{P}_k \mathbf{H}_k^T} ( {\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + {\mathbf{R}_k})^{-1} $$

Kalman Filter的运行流程图

Kalman Filter与Recursive Least Square

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

对比KF与RLS的过程:

KF:

预测

$$ \begin{aligned} {\mathbf{\hat{x}}_k} &= \mathbf{F}_k {\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k {\vec{\mathbf{u}_k}} \\ {\mathbf{P}_k} &= \mathbf{F_k} {\mathbf{P}_{k-1}} \mathbf{F}_k^T + {\mathbf{Q}_k} \end{aligned} $$

测量更新

$$ {\mathbf{K}’} = {\mathbf{P}_k \mathbf{H}_k^T} ( {\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + {\mathbf{R}_k})^{-1} $$

$$ \begin{aligned} {\mathbf{\hat{x}}_k’} &= {\mathbf{\hat{x}}_k}+{\mathbf{K}’} ( {\vec{\mathbf{z}_k}} – {\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\ {\mathbf{P}_k’} &= {\mathbf{P}_k}–{\mathbf{K}’} {\mathbf{H}_k \mathbf{P}_k} \end{aligned} $$

RLS：

$$ \begin{aligned} K_k=&P_{k-1}H_k^T(R_k + H_kP_{k-1}H_k^T)^{-1} \end{aligned} $$

$$ \hat{x}_{k} = \hat{x}_{k-1} + K_k(y_k - H_k \hat{x}_{k-1}) $$

$$ \begin{aligned} P_k=(I-K_kH_k)P_{k-1} \end{aligned} $$

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