核心思想

取自知乎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} + Δ t v_{k - 1} + \frac{1}{2} a Δ t^{2} \\ v_{k} = v_{k - 1} + a Δ t \end{array}$

卡尔曼增益

首先，卡尔曼增益可以看成是一个权重。根据上述式子，我们得到了两个值，预测值和观测值，简单来说，要想得到最后的估计值，那我们就可以把两者进行加权平均。因此，卡尔曼增益的作用，就是分配模型预测的状态和传感器测量的状态之间的权重。 接下来，让我们来算一算卡尔曼增益，在算卡尔曼增益之前，我们先算算估计值的误差协方差是怎么样一个形态。
卡尔曼滤波Kalman Filter

以上，证明完毕……

理清思路

1、我们拥有上一时刻的估计值，上一时刻估计值的误差协方差矩阵。 2、通过数学模型计算预测值、以及预测值和真实值之间的误差协方差矩阵。 3、计算卡尔曼增益，并得到估计值。 4、计算估计值与标准值之间的误差协方差矩阵。

扩展卡尔曼滤波

扩展卡尔曼滤波和卡尔曼滤波的不同之处就在于，他的状态转移方程和观测方程是非线性的，我们只需要对两方程进行泰勒展开即可。参考文献：证明过程参考此处 https://www.jianshu.com/p/9214a94b26ca https://blog.csdn.net/victor_zy/article/details/82862904 https://www.cnblogs.com/bonelee/p/9210821.html https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2