卡尔曼滤波Kalman Filter

年少不知记笔记，老来方恨看不懂……本文从三方面进行，首先是卡尔曼滤波的思想，然后对卡尔曼滤波进行公式推导，最后是对卡尔曼滤波进行总结。

核心思想

取自知乎Kent Zeng的回答：点击链接，我对他的回答进行了例子的补充。
1、假设你有两个传感器，测的是同一个信号。可是它们每次的读数都不太一样，怎么办？
取平均。
2、再假设你知道其中贵的那个传感器应该准一些，便宜的那个应该差一些。那有比取平均更好的办法吗？
加权平均。
怎么加权？假设两个传感器的误差都符合正态分布，假设你知道这两个正态分布的方差，用这两个方差值，（此处省略若干数学公式），你可以得到一个“最优”的权重。

Example：比如GPS和IMU测量值，GPS可以测量你走过的路，IMU也可以，它们的误差都满足正态分布，那我们也可以通过它们的误差协方差矩阵调整权重进行融合。

3、接下来，重点来了：假设你只有一个传感器，但是你还有一个数学模型。模型可以帮你算出一个值，但也不是那么准。怎么办？
把模型算出来的值，和传感器测出的值，（就像两个传感器那样），取加权平均。

Example：一个机器人当前状态是5米每秒，做匀速运动，且搭载雷达可测距，当前机器人距离障碍物20米，下一秒我们可以通过数学模型预测得到机器人距离障碍物还有15米（估计值），而此时雷达测距17米（观测值），然后我们就要看估计值和观测值的准确度，其实就是方差（多元的话是协方差矩阵），方差大权重就低，反之则高。

痛苦的证明

又到了痛苦的时候。
首先搞清楚几个概念：
预测值：通过预测方程得到的值，有噪声
观测值：通过观测方程得到的值，有噪声
估计值：最后得到的值，有噪声
标准值：无噪声，不可能得到的值
误差协方差：描述这个值的可信度，越小越好

我们要得到什么？
K时刻刚开始到来的时候，我们拥有K-1时刻的估计值以及它的误差协方差（可信度）。
那么K时刻结束时，我们要求拥有K时刻的估计值以及它的误差协方差（可信度）。

预测

状态转移方程&预测值

假设一台车的状态可以表示为位置和速度，假设有一台车，正在做加速运动（也就是有加速度），那么他的运动方程可以写成这种形式：
$$\begin{array}{l}{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{array}$$
写成矩阵的形式，有：

这个又称为k-1时刻到k时刻的状态转移方程，${\hat{x}}_k^{\prime }$指的是预测值。
$B_k$称为控制矩阵，$\overrightarrow{{\mathbf{u}}_k}$称为控制向量，这个项（$B_k\overrightarrow{{\mathbf{u}}_k}$）可有可无（比如说小车做匀速运动时就没有），${\hat{x}}_{k-1}$指的是上一时刻的卡尔曼滤波的估计值。

预测值的误差协方差矩阵

来看一下预测值${\hat{x}}_k^{\prime }$的误差协方差矩阵，也就是预测值的可信度。请注意，有一些外力是我们无法预测的，比如在运动的过程中，道路有坑洼，导致它的速度或者加速度并不是预想的那样，仅使用预测方程去预测是有误差的，该时刻的预测噪声$w_k$服从$N(0,Q_k)$（又称为过程噪声），同时，这个预测值还会受到上一时刻的估计值误差协方差的影响。
由：$${\hat{x}}_k^{\prime }=F_k{\hat{x}}_{k-1}+B_k{\overrightarrow{u}}_k+w_k$$
算得预测状态的误差协方差：（交叉项为0，因为互相独立）
$$\begin{gathered} {\hat{P}}_k^{\prime }=E[{{{\hat{e}}_k}^{\prime }}^T{\hat{e}}_k^{\prime }] \\ =E[(x_k-{\hat{x}}_k^{\prime })(x_k-{\hat{x}}_k \end{gathered}$$