了解CV和RoboMaster视觉组（五）滤波器、观测器和预测方法：卡尔曼滤波器

--neozng1@hnu.edu.cn

Kalman Filter（卡尔曼滤波器，时域）

KF家族从被提出以来可谓是运用的最多最广的滤波器了，并且其发明者Kalman教授以他的一己之力将控制领域的研究从频域转换到时域（以状态空间设计为基础的现代控制理论）上来，虽然他在后半生一直热衷于频域分析的研究。Kalman滤波器最出名的轶事就是在Kalman博士访问NASA后，那里的工程师认为这是一个极佳的时域修正方法，并且直接在阿波罗6登月飞行器的轨道矫正系统上使用了扩展卡尔曼滤波器。

相比于传统的时域滤波器和频域滤波器，KF运用马尔卡夫假设（从概率和信息观测的角度），只需要保留前一个状态的信息，占用内存小且速度非常快。

这一小节主要推荐一些非常棒的教程。关于KF的工程应用和直观理解，Matlab官方出品的这个系列视频很好地讲解了运用过程：Kalman Filter。另外若想从概率统计和数学角度初步理解KF，可以进一步参照这篇可可爱爱的教程：How a Kalman Filter works。

若是结合控制和概率论解释，并且完整地进行数学推导，请看bilibili up主 DR_CAN的：卡尔曼滤波器系列视频，另外，他从工程的角度讲解了状态空间法中的观测器设计，这能让你更好地从控制角度理解KF，请戳：状态观测器的设计。

若你学习过自动控制原理（准确的说是现代控制理论，主要用到的是关于稳定性分析和线性代数的知识），想从李雅普诺夫稳定性角度分析为什么KF是高斯噪声影响下的最优线性滤波器和其收敛性、抗扰能力，请戳卡尔曼滤波器原理介绍。

如果想进一步了解状态估计和机器人学中的概率论相关知识，可以参阅《state esstimation of robotics》和《probabilitics robotics》这两本经典的作品中相关的内容，里面详尽地介绍了KF为什么是高斯意义下地最优滤波器以及作为一个贝叶斯估计器的扩展，KF是怎样演化成这种形式的。

 KF从多个信息来源获取数据后的融合过程图示

​

• 由KF的构成我们可以发现，它能够综合多个数据来源(观测模型)和系统状态转移的模型并根据他们置信度（由协方差的大小来表示）来对各个信息来源赋予不同的权重最终融合得到预测结果。而EKF、UKF都是对非线性系统作近似线性化而产生的滤波器，对于工作点附近非线性程度较小、容易线性化近似的系统（容易指的是求导生成的Jacobian矩阵较好计算，不会包含很多非线性函数）有着很好的效果。显然在比赛中大部分的控制数据和传感器数据都是能够比较好地用近似线性化来拟合的，比如在传感器帧率高、处理速度快的时候我们常常假设对手机器人的运动是匀速直线运动，因此KF系列便顺利成章地作为我们使用滤波器的首选。

• 若我们想用KF来修正之前的数据（如t-1，t-2时刻），那么卡尔曼滤波器相当于一个插值或平滑数据的装置；若想通过KF来融合当前采集的 数据并且消除噪声，那么它是一个估计器，和前者一样都是贝叶斯方法，即利用后验信息来修正概率模型；若想通过它来预测t+1时刻的值即通过修改状态转移矩阵用历史数据来推测将来值，这时候就是一个预测器（单纯从名词直译来理解）。

• 有了以上教程的铺垫，我们就可以通过修改KF的状态转移矩阵来预测将来的数据了：以装甲板的运动为例，我们一般对解算得到的装甲板的中点进行运动学建模并假设装甲板的运动模型（一般假设为匀速直线或匀加速直线）和观测模型（也就是相机的成像模型，可以使用小孔成像）的非线性程度较小。这样我们便能选用EKF来对其进行线性化。我们可以把当前帧和之前若干帧的装甲板位置的世界坐标系坐标相减并除以相机的帧率的倒数得到其运动速度，或者进一步求差分以得到加速度信息。在得到这些信息后，把他们填入状态转移矩阵并设置一个合理的时间间隔（曝光时间+单帧处理时间+数据发送到电控的通信时间+电机响应时间+子弹飞行时间等，一般需要实车调试进行修改），EKF便能根据转移矩阵和置信度计算出此确定的时间间隔后装甲板的位置了。我们会在 5.3.2 进一步介绍运动预测方法，在 6.4 也会详细讲解如何利用KF对装甲板的运动进行预测。

• UKF则是EKF的改进版，全称为Unscented KF。当系统模型的非线性程度较大的时候，我们就很难用局部线性化的方法去得到贴近实际的近似分布，因此我们会在参数未进入系统前随机生成服从高斯分布的一组点同时给它们赋予不同的权重（为什么要这么做？），然后将他们一起投入到此非线性变换模型中，并在变换结束后统计这些点的均值和方差，利用这些数据重建一个高斯分布（点参数估计）用于近似此模型的分布。显然，使用的随机点越多则近似越准确（大数定律），但是消耗的资源也越大，这点和下一个要介绍的PF是完全一样的。不过，在实践中的经验来看，UKF对高维模型的近似并不是很好，一来是高维空间的分布更加复杂难以通过线性系统（在这里我们使用高斯分布）近似，另一方面是计算开销也随着维度增加而迅速上涨，维度爆炸是这两种方法（UKF和PF）共同的、最大的问题。

  好在我们的系统并不复杂，最多只有速度、位置、加速度三个维度（加上加加速度和加加加速度也是没有任何问题的，如果你觉得者能够提升你模型的精确度并且这些数值的噪声足够小的话，或者你能够找到其他能够增加置信度的先验信息和其他可用传感器数据）。

  

  ​UKF的流程图解；图源知乎-"fishmarch"的专栏'概率机器人'
  ​
  ​

  根据上图我们可以把UKF的迭代过程分为以下四步：

  1. 选取服从Gauss分布的随机点

  2. 和数据一起投入变换

  3. 根据权重和新的均值方差得到新的Gauss分布，并假设此高斯分布就是经过变换后状态的近似分布

• 此外，卡尔曼滤波器还有各式各样的变种，如SCKF（状态约束）、ESKF（误差状态）、AUKF（增强无迹）等，有兴趣的读者可以自行了解相关的内容，无非是从不同状态变量的关系之间入手增加额外信息或约束以进一步逼近真值，或是通过更合理的方法来近似非线性部分。