首先这里非常感谢drcan提供的非常详细且通俗的教程。这篇文章也记录一下我学习卡尔曼滤波的过程。主要是记录一下公式的推到过程。

均值的增量式写法

最后整理为：

可以看到随着测量次数的增加，测量结果就不再重要了，此时这个公式变成的1/k被提炼成一个系数，也就表示：

当前的估计值等于上次的估计值，加上一个系数乘以测量值与当前估计值之差。

OK，可以看到，当这个系数越大，越采信测量值；当这个系数越小，越采信估计值。

当这个系数为

的时候，就是刚刚说到的求均值：

当这个系数为一个固定值

的时候，这个式子就变成了一阶低通滤波：

而当这个系数为卡尔曼增益

的时候，就变成了一维的卡尔曼滤波：
那么，如何在动态过程中计算出一个最合适的卡尔曼增益，就是卡尔曼滤波的精髓所在。

OK，接下来引入几个概念。

估计值与当前真实值之差称为估计误差（Estimation Error）：

测量值与当前真实值之差称为测量误差（Measurement Error）：

通常我们并不直接关心某一次误差，而是关心它们的统计大小，因此定义：

估计误差协方差：

测量误差协方差：

卡尔曼增益的公式就可以表示为：

可以发现：

当估计误差 $P_k$ 变大时，说明当前预测越来越不可靠，因此卡尔曼增益变大，更加采信测量值。
当测量误差 $R$ 变大时，说明传感器测量越来越不可靠，因此卡尔曼增益变小，更加采信估计值。所以卡尔曼滤波本质上就是在做一个数据融合。

它利用一个存在误差的预测值和一个存在误差的测量值，根据双方误差大小动态计算融合比例（卡尔曼增益），最终得到一个尽可能接近真实值的最优估计。
也就表示：
当前的估计值等于上次的估计值，加上一个系数乘以测量值与当前估计值之差。

数据融合

（标准差：向左向右各一个标准差就覆盖了68.4%）
这里的话就以Drcan举的例子来记录吧

假设现在有这样一个问题：

假设现在有两个传感器，都在测量同一个真实值。

测量结果如下：

第一个传感器测量的标准差为2，第二个传感器测量的标准差为4
那么，如何，用这两个传感器的测量值来估计出尽可能准确的估计值呢。

因为两个测量值都存在误差，所以最好的办法就是把二者融合起来，得到一个新的估计值。

解法如下：
我们令最终估计值为：

由于真实值未知，我们无法直接最小化估计误差。
但是我们知道，每个传感器的误差大小可以用方差表示，因此刚刚的问题就变为：如何找到一个合适的K让数据融合后的方差最小。

即

接下来首先对方差公式进行化简，这里的话由于两个传感器相互独立，方差最终化简为：

其中

分别表示两个传感器的测量方差。

接下来转变为一个求最小值的问题。

为了使融合后的方差最小，对 $K$ 求导：

展开得到

整理可得

这就是最优融合系数。

那么刚刚给的例子中：

于是

可以发现：

由于第二个传感器误差更大，因此它只占 20% 的权重；

而第一个传感器更加准确，因此占 80% 的权重。

也就是说：

误差越小的数据，权重越大；误差越大的数据，权重越小。

这也是数据融合最重要的原则，那么接下来如何把他和卡尔曼滤波联系在一起呢

我们回头看卡尔曼滤波的更新公式：

和刚刚的数据融合好像有点相似。

进一步：

把当前预测值 $\hat{x}_{k-1}$ 看成第一个”传感器”

把当前测量值 $z_k$ 看成第二个”传感器”

那卡尔曼增益 $K_k$ 就是刚刚推导出来的最优融合系数。

唯一不同的是：

普通数据融合只计算一次，而卡尔曼滤波会在每一个时刻根据预测误差和测量误差重新计算一个新的融合系数，因此能够实现实时、自适应的数据融合。

协方差矩阵

ok那么接下来的话，还有一个问题，刚刚所说的两个传感器，一个方差2，一个方差是4.方差描述一个变量的不确定性大小。但是在实际问题中，往往需要同时估计多个变量，例如机器人的位置、速度、姿态等。此时，仅用一个方差已经无法描述所有变量之间的关系，因此需要引入协方差矩阵（Covariance Matrix）。

设有两个随机变量 $X$ 和 $Y$，它们的协方差定义为

对于一组样本，其协方差可以计算为

注意这里是n-1是因为是引入了Bessel Correction（贝塞尔校正），这是因为在统计学 中如果用样本来估计整体的话，对于n个数据，当前面n-1个数据都被固定住之后，为了保证均值稳定，那最后一个数也就被限制住了，真正能变化的其实也就是n-1个数了。自由度变成了n-1。
这里的话如果单纯算一个组给定数据的协方差的话，用n

可以发现，协方差实际上就是同时观察两个变量偏离均值的情况,有关协方差的定义、计算、性质这些这些就不多说了，具体的看概率论教材吧。
值得说明一点的是矩阵计算协方差的形式

假设共有 $n$ 个样本，每个样本包含 $m$ 个变量。

将所有样本组成矩阵

则协方差矩阵可以表示为

协方差矩阵说明了什么？

对于协方差矩阵

可以发现：

主对角线上的元素表示各个变量自身的方差；
非对角线元素表示不同变量之间的协方差。

因此，一个协方差矩阵同时描述了：
每个变量自身的不确定性；
各个变量之间是否存在相关性。

若

表示两个变量呈正相关。

即：

当 $X$ 增大时，$Y$ 往往也会增大。

并且数值越大，说明这种同向变化越明显。

若

表示两个变量呈负相关。

即：

当 $X$ 增大时，$Y$ 往往会减小。

数值越小（绝对值越大），说明这种反向变化越明显。

若

说明两个变量之间几乎没有线性相关关系。

需要注意的是：

协方差等于 0 并不一定表示两个变量完全独立，只能说明它们没有明显的线性相关性。

为什么卡尔曼滤波要用协方差矩阵？

卡尔曼滤波通常需要同时估计多个状态，例如

或者

因此，不仅需要知道每个状态变量各自的误差大小，还需要知道它们之间是否相互影响。

这也是卡尔曼滤波中使用协方差矩阵 $P$ 来描述状态估计误差，而不是仅使用一个方差的原因。

卡尔曼增益推导

ok接下来是对卡尔曼增益的推导，其中符号以矩阵表示适配多维情况。

首先

假设系统在第 $k$ 个时刻的状态向量为

我们定义系统的状态空间方程和测量方程为（也就是现代控制里面的ABCD矩阵，记得离散化）：

状态方程

系统的状态方程写为

其中：
$w_{k-1}$：过程噪声， $v_k$：测量噪声，表示传感器本身引入的误差。

对于过程噪声

我们假设其服从正态分布

其中Q为协方差矩阵(这里的计算推导省略)：

对于测量噪声

我们同样假设其服从正态分布

其中R为协方差矩阵：

然而在实际过程中我们无法知道模型中的噪声项$w_{k-1}$，所以我们只能通过系统的动力学模型（状态空间方程）来预测下一时刻的状态，这个时候我们把它称作为状态变量的估计值(先验)：

然后另一边，我们也能够通过传感器测量出一个数据，来反推我们的状态变量。我们把它称作测量值：

上述两个获取状态变量的方式就是刚刚所谓的“传感器A”和传感器“B”.所以卡尔曼滤波接下来就是要根据这两个状态信息的来源去估计一个尽可能准确的状态值

这里如果

公式变成

展开后就能得到标准的卡尔曼滤波更新公式

接下来的任务就是去找到目标的$K_k$：

量化误差:

估计误差的协方差矩阵(2维为例)：

所以接下来目标是让误差的方差最小，这个时候误差越接近于期望值0。要注意：
卡尔曼滤波通常在无偏估计的前提下，通过最小化估计误差的方差（多维情况下为误差协方差矩阵），使误差尽可能集中在零附近，从而得到更加稳定、准确的状态估计。如果系统存在其他误差，也就是过程噪声的均值非0，则需要采用其他方法，比如把系统误差也放到状态变量里面。

这里就以无偏估计为前提，目标是让误差协方差矩阵的迹最小：

那这个时候就又有个疑惑，目标是让矩阵的迹最小，那非对角线上的元素会不会很大，或者说有没有被约束。
其实也是被约束了的：

任何合法的协方差矩阵都必须是半正定矩阵

则有(以2维为例):

所以，如果通过最小化迹让两个对角线元素都很小，那么非对角线元素也不可能任意增大

继续推导

误差协方差

接下来计算：

定义先验误差

最后带入到之前误差协方差计算式中得到:

继续展开：

由于先验估计误差与测量噪声相互独立，因此中间亮项为0，同时提取常数，合并为：

接下来根据定义，先验误差的协方差矩阵为:

观测噪声协方差矩阵为:

代换得到:

这个也叫做Joseph Form（Joseph稳定形式）

展开：

接下来对 $P_k$ 求迹：

由于迹满足：

并且 $P_k^-$ 为对称矩阵，因此：

所以中间两个一次项可以合并：

接着让矩阵的迹对$K_K$矩阵求导

（这里本来想展开讲一下但是全堆到这里有点乱，所以直接给结果了，关于矩阵求导的部分到时候再另作总结）

展开得到以下结果：

协方差矩阵对称，继续化简得到：

解出卡尔曼增益：

至此卡尔曼增益推导完毕，这个时候会发现，如果我们要计算卡尔曼增益，那首先要线算出先验误差的协方差矩阵。

先验协方差矩阵推导

OK接下来是对先验误差协方差矩阵的推导：

首先根据定义：

然后把真实值和真实值的先验估计带入，得到

消去控制输入项,化简合并后，得到一个递推公式：

进而代入 $P_k^- $的定义:

展开后:

因为 $e{k-1}$ 是上一次的估计误差，也就是 $x{k-1}-\hat{x}{k-1}$；$w{k-1}$ 是这一次的过程噪声。所以它们相互独立，中间两项都是0

所以：

带入定义，得到先验协方差的计算公式：

至此，所以卡尔曼滤波所有公式全部推导完成

总结

总结卡尔曼滤波步骤：

初始化：

​ 系统模型：

​ 其中：

​ 设置:

​ Q：过程噪声协方差、R：测量噪声协方差、$P_0$：初始误差协方差、$\hat{x}_0$：初始误差协方差、A,B,H：模型矩阵

计算预测值（模型预测的先验值）：

计算先验误差协方差矩阵：

计算卡尔曼增益：

计算后验估计（卡尔曼滤波的输出值）：

更新上一次先验误差协方差矩阵：

Q 和 R 的设置

在实际使用中，$Q$ 和 $R$ 常常先设为对角矩阵，这是因为我们通常先假设不同状态分量或不同传感器测量噪声之间相互独立。

若噪声之间相互独立，则非对角线协方差为 0：

但是严格来说，$Q$ 和 $R$ 不一定是对角矩阵。只要不同噪声分量之间存在相关性，就需要在非对角线位置加入协方差项。

尤其是过程噪声协方差 $Q$，在运动模型中经常不是对角矩阵。例如未知加速度会同时影响位置和速度，因此会导致位置误差和速度误差相关。