卡尔曼滤波

前言

之前看Fast-LIO的时候，了解过卡尔曼滤波的知识，重新整理下。同时推荐一个很棒的Kalman Filter讲解网站：https://www.kalmanfilter.net，这里有关于网站的速览：https://mp.weixin.qq.com/s/QGmX4ygxqpVXjpkl40MBiA。

一、初印象

卡尔曼滤波是最重要和最常见的估计算法，基于不精确、不确定的观测来估计和预测隐藏变量（或称为状态）；它基于马尔科夫假设，假设当前状态只与上一时刻状态有关。根据上时刻状态量、输入以及观测量，推导出当前时刻状态和协方差矩阵。

显然，从卡尔曼滤波的描述上看，它符合我们在SLAM系统中根据传感器数据估计机器人位姿的需求。

二、基本概念

记录一些描述的很清晰的概念。

1. 估计、准确度和精度
   估计（Estimate ）：对系统隐藏状态的估计。
   准确度（accuracy）：描述了测量值和真值的接近程度。
   精度（precision）：描述了同一参数的多次测量的变化情况。
   

2. 关于测量值
   测量值（measurement）：随机变量，使用概率密度函数描述。
   期望：测量值均值。
   准确度：测量值均值和真值的偏差，也称作偏差（bias）或系统测量误差。
   精度：变量分布的离散度，也称为测量噪声/随机测量误差/测量不确定度。
   

3. 噪声
   过程噪声（process noise）：动态模型误差，w（Q）。
   测量噪声（Measurement Noise）：测量误差，v（R）。

三、公式及参数

卡尔曼滤波一般分为两个步骤，状态预测和更新。有五大公式：1.状态外推方程；2.协方差外推公式；3.状态更新方程；4.协方差更新方程；5.卡尔曼增益，如下图所示。感觉图中第四列的别名对方程的描述很有助于理解。

• 预测方程由状态空间方程推导出来。

• 卡尔曼增益Kn，也成为权重更新方程。描述了在状态更新时对测量值的信任度，取值在0-1之间，相当于一个对观测量得权重值。测量值不确定度低时，Kn接近1，表示更加信任测量量提供的信息来调整状态量；测量值不确定度高时，Kn接近0表示更希望保持当前状态。
  

• 状态更新方程，也称为滤波方程，根据观测量来调整预测的当前状态。

• 协方差更新方程，协方差表示了对状态的估计的不确定度。从一维卡尔曼滤波器推导的公式结果来认识一下协方差更新方程：
  
  由于0<=Kn<=1，随着更新迭代这里的p_n_n会越来越小，意味着状态估计结果会越来越准确。当测量的不确定度高时，Kn值小，协方差收敛得慢；反之，协方差收敛得快。

四、其他

1. 滞后误差（lag error）
   可能的原因：1.模型构建不合理；2.过程噪声过小。
   那么对应的解决方式：1.重新构建模型，考虑更多的变化量；2.调大噪声以表明模型本身的不确定度较高。
   好的卡尔曼滤波器应该尽可能真实地描述实际模型，但无法得到较准确的模型时，考虑调整过程噪声也是折中的选择。
2. 过程噪声方差
   过程噪声方差Q越小，对模型信任度越高，系统越容易收敛，但太小容易发散。而Q越大，则代表模型不确定度高，其实和卡尔曼滤波器初衷不相符。
3. 测量噪声方差
   测量噪声方差R太大，对新的测量值信任度低，卡尔曼滤波响应会变慢；越小系统收敛越快，但容易出现震荡。

总结

本文简要介绍了卡尔曼滤波相关的概念和理解。