卡尔曼滤波详解（1）-编程知识

1. 核心思想

2. 五个公式的解读

2.1 预测部分

2.2 更新部分

3. 公式的实际应用

4. 调参方法

1. 核心思想

首先，卡尔曼滤波器可以用来估计系统的状态，这个状态是时间序列上的，利用上一时刻的状态可以预测当前时刻的状态，利用当前时刻的观测可以更新和修正当前时刻的预测。这么说可能有点绕，看下图。

绿色的x表示系统的状态，y表示对系统状态的观测，蓝色的x表示修正后的状态。卡尔曼滤波的核心思想，就是用利用蓝色 $\hat{x_{1}}$ 进行预测，得到下一时刻绿色的 $x_{2}$ 。在利用下一时刻的观测 $y_{2}$ 和预测得到的x2得到下一时刻修正后的状态，即蓝色的 $\hat{x_{2}}$ 。

由上面的过程可以发现，卡尔曼滤波由一个明显的特点，即当前时刻的状态，只和上一时刻的状态有关，这叫做马尔可夫性。对于实际应用来说，内存中只需要存储上一时刻的状态，占用内存小，速度快，对于实时问题有很好的响应。

2. 五个公式的解读

下面来直接解读五个公式

2.1 预测部分

首先是预测部分，预测就是根据当前上一时刻的融合状态，推测当前时刻的状态，而这个状态不能完全代表当前时刻的最终状态，所以看到预测部分的两个公式都是带了“-”号的。

$\hat{x_{t}}$ ：表示t时刻系统的状态

F：表示状态转移矩阵，即如何从 $x_{t-1}$ 变换到 $x_{t}$ ，通常和理论公式有关。

u: t 时刻外界对系统的作用（现在不理解没关系，实际应用中会具象化）

B：控制矩阵，将外界的作用转化为对状态的影响

P: 系统的不确定度，这个非常的重要，以协方差矩阵的形式展现，也可以表征状态量之间的关系，随着卡尔曼过程的迭代，这个P应该逐渐减小。

Q：预测过程增加进系统的不确定度，可以视作从上一时刻得到当前时刻状态量的可靠程度，也以协方差的形式呈现，有时也将外部的影响放入Q中一起考虑。

对于预测部分，第一个公式也有在最后加上一个噪声 $w_{t}$ 的，如果有些噪声可以被表示清楚，写进公式可以增加预测结果的可信程度，Q也就随之减小（我个人的理解）。

第二个公式关于系统不确定度P，一定要认识到其实以协方差矩阵的形式展现的，能够表征系统各状态量之间的关系。同时最重要的是P是表征整个系统的不确定度的，所以随着迭代的进行，P也是要更新的，这就是有第二个公式的原因。而这个公式的来源就是将上一时刻的状态预测为当前时刻，用协方差的数学特性也变换到当前时刻，再加上这么变换的误差噪声Q。

$cov(x_{t-1}) = P_{t-1}$

$cov(x_{t}) = cov(Fx_{t-1}) = FP_{t-1}F^{T}$

2.2 更新部分

更新部分的3个公式，卡尔曼增益K是一个过程量，最终会得到修正后的状态量x及系统的不确定度P。这其中

H：是变换矩阵，表示从观测值到状态量之间的关系，因为有些状态量是无法直接观测的

Z：是观测值

R: 是观测的噪声，也是以协方差的形式表示的

K：卡尔曼增益，这个增益其实就是为了表达简便，认为定义的一个公式。

更新部分的三个公式来源，实际上是由两个正态分布的公式相乘的结果得到的。根据预测部分的结果，我们得到了预测的当前时刻的状态量，这个状态量其实服从高斯分布。同时，在当前时刻，我们有当前时刻的观测量，这个量也服从高斯分布。更新部分的主要操作，就是将这两个高斯分布的量结合，得到一个方差更小的高斯分布。图示如下（这个图是来自两个高斯分布乘积的理论推导_两个高斯分布相乘-CSDN博客

讲两个高斯分布如何融合起来，讲的很好）

而这三个公式，实际上就是两个高斯分布相乘得到新高斯分布的公式套用：

$\mu = \mu _{0} + \frac{\sigma _{0}^{2}(\mu _{1}-\mu _{0})}{\sigma _{0}^{2}+\sigma _{1}^{2}}$

$\sigma^{2} = \sigma_{0}^{2} - \frac{ \sigma_{0}^{4}}{ \sigma_{0}^{2} + \sigma_{1}^{2}}$

简化一下表达，定义K为：

$K = \frac{ \sigma_{0}^{2}}{ \sigma_{0}^{2} + \sigma_{1}^{2}}$

则公式可以写为：

$\mu = \mu _{0} + K(\mu _{1}-\mu _{0})$

$\sigma^{2} = \sigma_{0}^{2} - K\sigma_{0}^{2}$

再对应到上面的更新部分的三个公式，是不是如出一辙。

3. 公式的实际应用

现在我们假设一个物体匀加速直线运动，状态量X是[x, y, vx, vy],由于是匀加速直线运动，我们很容易得到前一时刻各个状态量和当前时刻各个状态量之间的关系，y方向以此类推。

$x_{t} = x_{t-1} + vx_{t-1} + 1/2a_{x}t^{2}$

$vx_{t} = vx_{t-1} + a_{x}t$

写成矩阵的形式为：

$\begin{bmatrix} x\\ y\\ vx\\ vy \end{bmatrix}=\begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0& 1& 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} x_{t-1}\\ y_{t-1}\\ vx_{t-1}\\ vy_{t-1} \end{bmatrix} + \begin{bmatrix} 1/2\Delta t^{2}\\ 1/2\Delta t^{2}\\ \Delta t\\ \Delta t \end{bmatrix}a$