看了一下卡尔曼滤波的教程，做个总结。

通常大家都熟悉一阶滤波的公式。

这个公式体现的就是一种数据融合的思想。历史数据和采样数据的融合，不同数据按照权重合成。之所以需要融合，是因为所有的数据源都是有误差的。传感器有误差，计算模型出来的数据也有误差。把不同的数据源合成，期望获得更优的结果，就是数据融合的目的。

卡尔曼滤波也一样，需要在计算值（估计值）和测量值之间进行数据融合，形成一个误差更优于两者的校正值，使得这个后验的估计值尽量靠近真实值。它采用的就是下面的公式。

变量上面的尖头表示这是个估计值，也就是靠模型算出来的。上面的小横线表示这是个先验值，没有被测量校正过。X是系统状态值（过程值），Y是系统测量值。下标k是离散的递归计算中的第k次。C是个矩阵，当作常数。

和一阶滤波一样，它也需要一个可以在一定范围（0 ~ C的逆）变化的系数，就是公式中的K(k)（称作卡尔曼增益）。这个系数的作用，是在测量值与先验估计值的差异范围内游走，来修正先验估计值，使本次计算出来的后验估计值更准确一些。

卡尔曼滤波算法的核心思想就是求得合适的K值，让真实值和后验估计值之间的误差最小。

通常过程噪声和测量噪声（也就是误差）都被认为是正态分布的。误差的范围由误差的协方差矩阵来决定。其中包含了方差，也包含协方差。它反应合成空间中的变化因素的关联。

协方差矩阵的对角线元素都是方差，这些方差之和称作迹。方差是用来衡量一个量对真实值而言的准确性和稳定性的。

误差如果要最小，也就是这个协方差矩阵的迹（对角形之和）要最小。方差的最小就表明了计算值的最可靠性，也就是最稳定的一个很窄的真实值正态分布。

 所以就是要把这个与真实值误差的协方差矩阵的迹求出来，然后对其公式求导。导数为0的极点，就约束了K的数学形式。推导有些麻烦，要是看过详细的步骤，其实也还可以。

最后推导出来的K值计算公式如下。

这个公式就能产生误差下的数据融合最优解。R是测量误差的协方差矩阵。P就是后验估计值和真实值之间误差的协方差矩阵，其中包含了过程误差的影响，这个矩阵需要单独计算。

误差是数据可信度的负面。但在一个两者合成的约束关系中，对方的误差和自己的可信度是成正比的，这就是用系数来权衡两个数据源的意义所在。对方误差越大，越要忽略对方的不同所带来的差异，保持自我。反过来，如果自身误差占比越大，那就越要向对方的差异靠拢，以校正自己。这就是融合。

关于过程误差和测量误差，在控制理论关于一个系统的状态空间的离散表示方法中有具体描述如下

一个系统从状态空间的角度，是按照它的输入、输出和内部状态来分角度理解的。X代表系统状态，Y代表测量，也就是系统的输出。U代表输入。每种量都是被矩阵修饰的。W是过程（模型计算）误差，V是测量误差。

当前的系统状态，由上一次的系统状态、系统的控制输入、和状态误差的线性合成决定。

当前的测量，由当前的系统状态、和测量误差的线性合成决定。

因为现实中W和V是不确定的，所以能存在于模型中的东西就简化为如下

因为误差被去掉了，所以表示状态和测量的量就都成了估计值，都带个尖头。

卡尔曼滤波的计算分为5个步骤

1、首先按照上面的离散状态描述，根据上一次的系统状态计算当前系统状态，都是估计值。在程序初始的时候，这个值要自己给。

2、算出了本次估计值，就要去求卡尔曼增益系数。但是求这个系数要用到真实值和经测量校正过的估计值之间误差的协方差矩阵，所以要先把这个矩阵求出来。就是下面这个公式。Q是过程误差的协方差矩阵。

3.有了这个矩阵，带入K的公式，就得到了本次的卡尔曼增益系数。

4、有了卡尔曼系数，可以把先验估计值和测量值进行数据融合了，得到本次经过测量校正的后验估计值。

5、在第2步中计算本次的误差协方差矩阵的时候，用到了上一次的误差协方差矩阵的值，所以此处必须更新这个矩阵，供下一次使用。在程序初始的时候，矩阵值要自己给。

卡尔曼滤波有很多变种，因为基本算法中有一些基于线性的假设，在不同环境下，非线性条件下都要扩展。