对卡尔曼滤波5个公式的非量化理解及若干Tricks

先把KF５个公式放在这里，下面２种表述皆可。本文主要学习和参考了这篇文章，因此以左侧第一种表示为主。

　                       

在Predict Step的2个公式中，可以将  与  视为等价的运算。 是F推动状态x的演化， 是F对x的不确定度的影响。，其中，FP计算F对P的列的影响，计算F对FP的行的影响。因此，可以将刚初始化的 转换为的形式，即纳入了不同状态变量间的相关关系。

是非常常见的矩阵计算形式，表示P向F做投影。

在建模时考虑加入隐变量（不可以直接观测的变量），可以提高模型的准确度和KF的效果，如追踪的效果一般要优于只追踪 [location]。因为隐变量与原状态变量是相关的，如果不相关，加入隐变量对滤波没有影响。

卡尔曼增益K是矩阵，其中有对应每个状态变量的系数。原状态变量的系数主要与其方差有关，隐变量的系数与协方差有关。这样，隐变量的不确定度、隐变量与原状态变量的相关性、原状态变量的不确定度，都被考虑了进来。

根据上图第2、3个公式，可以理解为通过F或H将P映射到不同的空间。

第6个公式，。 将残差y（测量空间）映射到状态空间。K表示测量在更新时所占的比重，体现了估计更相信测量还是更相信预测。

最后一个公式，若K较大，相信测量大于预测，信息增多，P会比小很多；若K较小，相信测量小于预测，信息减少，P会与接近。

 

Little tricks

采用数值方法更新后验协方差矩阵时，可能会因为float类型的舍入误差导致P不对称，而协方差矩阵一定是对称的，就会引起KF发散。有一个计算的小技巧是，把改为。具体推导参考。

滤波器的阶数和系统方程的阶数匹配，往往可以取得较好的滤波效果。当然，低阶滤波器也可以追踪高阶系统，只要设定足够高的系统噪声（process noise）和足够小的采样周期。

马氏距离（mahalanobis distance）描述一个采样点到一个分布的标准偏差距离。结合3σ法则，若一个点到某分布的马氏距离为3.6，则该点落在以均值为中心、以3.6σ为半径的椭圆上。椭圆的长短轴长度取决于分布的状态变量的标准偏差。即，马氏距离的量纲可以理解为σ，σ是矩阵。马氏距离的定义如下：。如果协方差矩阵是对角阵，即状态变量的协方差都是0，马氏距离可以看作是归一化的欧式距离，尺度因子就是各状态变量的方差。

对于测量值的异常检测，可以使用基于3σ法则的双门限方法，第一个gate使用矩形门限，第二个gate使用椭圆形门限（即马氏距离，计算耗时）。参考资料