How a Kalman filter works, in pictures学习笔记

参考https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
卡尔曼滤波器是组合不确定信息的强大的工具。

什么是卡尔曼滤波器?

在猜测动态系统将会发生的事情时，卡尔曼滤波器是一个很好的选择。卡尔曼滤波器很适合一直在变化的系统。它消耗的内存小，而且速度很快，适合嵌入式设备。而且实际上卡尔曼滤波器是一个十分简单和容易理解的算法，只需要掌握一些基本的概率和矩阵的知识即可以理解了，

我们可以用卡尔曼滤波器做什么?

举例说明：假设你需要一个机器人自行在丛林中行走，那么你必须知道机器人的运动状态，它包括了位置和速度。



你现在可以知道的信息，一个是GPS的信息，但是GPS的定位精度只有10米，另外一个是你可以知道机器人轮椅的控制信息，但是机器人可能受到风、复杂的道路所影响而导致控制信息并不能没有误差地控制机器人。如果要更好地估计机器人的状态，你就可以使用卡尔曼滤波器结合GPS和控制信息来给出一个更加精准的机器人运动状态估计。

怎样用卡尔曼滤波器解决你的问题？

如何用卡尔曼滤波器来表示你的问题？
接着上面那种情形，我们来估计机器人的运动状态

我们不知道准确的位置和速度，但是我们知道某些组合的可能性更大，某些组合可能性更小

卡尔曼滤波器假设两个向量都具有高斯噪声，这样每一个变量都有平均值和方差

上图位置和速度是没有关联的，但是实际过程中位置和速度是相互关联的，因为如果速度很快，位置可能就更远，因此实际上是这样的

为了表示这样关联，就使用了协方差矩阵，它表示速度和位置的关联

如何用矩阵描述问题？

使用矩阵描述问题
状态向量和协方差

当然这里只有位置和速度两个向量，实际上可以扩展到任意多个向量。
接下来我们将用当前状态(k-1)预测接下来的状态(k)，我们不不关心真实状态到底是怎样的，我们预测是所有可能的状态

我们可以用一个矩阵F_k来描述所有状态。

实际上预测是如何做的，我们使用的是基本运动学公式

写成矩阵形式就是

但是协方差矩阵是如何变化的呢？
实际上根据概率学知识，对于一个变换矩阵A有

这样我们就知道了实际上的状态变化和协方差矩阵变化

外力影响

考虑更多的因素
现实情况中可能更加复杂，例如导航软件可能给轮椅发了加速指令或者停止指令，因此我们需要更加复杂的模型来进行预测，我们加入变量加速度来表示控制指令造成的影响。

写成矩阵形式就是

其中B_k就是控制矩阵，u_k就是控制向量

外部不确定性

考虑更多的不确定性，如果状态一直基于它自身的性质演变或者基于已知的外力演变，我们都可以很好地估计状态。但是有一些力我们是无法测量的，如对于无人机来说的风，对于轮椅来说它可能会滑倒或者受到障碍物阻挡而减慢速度，对于这些外力的影响我们并不能进行测量。

我们可以这样做，就是对于每一次的预测加上一个新的不确定项用来表示这些外部不确定因素的影响。

每一个$x$_k-1将被移动到一个带有协方差为Q_k的高斯斑块当中，这样所有的$x$_k-1就会得到如下的变化

最终总体上的状态分布会变成协方差和之前不同，但是均值和之前一样的分布

这样，之前的预测公式就相应地变成如下形式

这样我们就得到了系统的粗略估计$x$_k和P_k。

用测量修正估计量

我们有一些测量设备，它们可以间接反映系统的状态，可以认为这些传感器根据系统状态得到了一系列的读数。

注意到读数的单位和尺度和系统状态的单位和尺度是不一样的，因此我们需要用一个测量矩阵H_k来反映它们之间的关系。如下图所示：

写成公式，按照一般的写法我们可以这样写

KF还考虑到了传感器的噪声，对于一个原始的估计值由于噪声的影响可能产生一系列读数中的一个。如下图所示：

更详细的描述如下图所示

我们可以用协方差R_k来表示这些噪声，同时这些分布和测量值的均值是相等的，也就是$z$_k。所以现在我们可以得到两个高斯斑块，一个是由转化矩阵预测得到的，用紫色表示，一个是由传感器测量值得到的，用绿色表示。如下图所示：

我们需要相互协调这两个高斯斑块，我们做的就是将这两个分布相乘来得到它们之间重叠的部分，这部分的可能性是最大的

这样我们就得到了一个新的高斯分布

组合高斯分布

我们想要得到两个高斯分布相乘之后的分布，我们先来看下一维高斯分布的公式：

那么两个一维的高斯分布相乘用图像显示如下：

用公式描述如下：

那么新的高斯分布的均值和方差和原来的高斯分布的均值和方差有什么关系呢？通过将(9)代入(10)，我们可以得到如下结果：

我们稍微化简下形式

那如果写成矩阵形式呢？假设∑表示高斯分布的协方差矩阵，$\mu$表示高斯分布的均值向量，那么矩阵形式如下所示：

这其中的K就是卡尔曼增益。

整合

将之前的预测和观测修正写在一起。对于预测测量我们有：

对于观测修正我们有

现在我们将上两式带入(15)，可以得到

带入(14)可以得到

对于(16)和(17)我们左乘H_k^-1（K中隐含，也就是说K’ = H_k^-1K）,而对于(16)中的第二个式子右乘H^T_k^-1,可以得到如下式子

这样我们就得到了完整的更新步骤，而
就是我们 的最优估计，它的协方差是

总体的KF信息流动图如下：

扩展

你需要用代码实现的就是式(7)(18)(19)
卡尔曼滤波器可以进行非线性扩展得到扩展卡尔曼滤波器