【学习笔记】卡尔曼滤波 Kalman Filter

Part.I Introduction

笔者在上大学的时候，曾经有两个词语如同梦魇一般萦绕在耳边，经久不散。一个是『卡尔曼滤波』，另一个是『傅里叶变换』，这两个词被说的很玄乎，但是笔者感觉虽然很多人经常爱说这些词语，但是真正学的超级超级透的人肯定不多。

卡尔曼滤波到底是说的啥呢，这段时间笔者决定系统的学习一番，此篇博客就简单记录一下学习笔记。

Chap.I 传送门

因为不想把一篇博客搞得太长，因此开了下面一些子博客。

• 前导：【卡尔曼滤波】连续&离散线性系统的数学模型
• 核心：【卡尔曼滤波】离散线性系统的卡尔曼滤波
• 实例：【卡尔曼滤波】卡尔曼滤波简单实例

Part.II 卡尔曼滤波简介

卡尔曼滤波是以卡尔曼的名字命名的。卡尔曼全名 Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。

卡尔曼滤波 是一种线性最小方差估计，它具有如下特点：

• 1）算法是递推的，且使用状态空间法在时域内设计滤波器，所以卡尔曼滤波适用于对多维随机过程的估计。
• 2）采用动力学方程即状态方程描述被估计量的动态变化规律，被估计量的动态统计信息由激励白噪声的统计信息和动力学方程确定。由于激励白噪声是平稳过程，动力学方程已知，所以被估计量既可以是平稳的，也可以是非平稳的，即卡尔曼滤波也适用于非平稳过程。
• 3）卡尔曼滤波具有连续型和离散型两类算法，离散型算法可直接在数字计算机上实现。

正由于上述特点，卡尔曼滤波理论一经提出立即受到了工程应用的重视，阿波罗登月飞船和C-5A飞机导航系统的设计是早期应用中的最成功者。

Chap.I 卡尔曼滤波的要素

卡尔曼滤波有五个核心要素：状态向量及其协方差、系统模型、观测向量及其协方差、观测模型和滤波算法。

• 状态向量（state vector）：是一组描述系统的参数，又称为状态，是卡尔曼滤波估计的对象。状态可能是常量也可能是时变量。在大多数导航应用中，状态量一般包括位置或者位置误差，速度、姿态及导航传感器的误差也可作为被估状态。注意有些学者用“状态”来表示整个状态向量，而非个别组成部分。与状态向量相关联的是误差协方差矩阵( error covariance matrix)。该矩阵描述了卡尔曼滤波状态估计的不确定度及估计误差之间的相关程度。误差协方差矩阵中的相关信息非常重要，原因有三：①它可完全表示状态估计的误差分布。当不考虑相关性时，会导致一个方向过高估计而另一个方向过低估计。②观测量中往往没有足够的信息来独立地估计卡尔曼滤波的状态，相关信息使得在等待新的观测信息时，可估计那些需保持的状态间的线性关系。③误差间的相关信息可以建立起观测量之间的联系。通过建立模型可使得某些状态可由其他状态确定(例如从一系列的位置中确定速度)。卡尔曼滤波是一个迭代过程，故状态向量和协方差矩阵的初始值需由用户设定或由其他过程确定。
• 系统模型(system model )，也被称为过程模型或者时间传播模型，描述了卡尔曼滤波的状态及其误差协方差矩阵随时间的变化特性。例如，位置状态作为速度状态的积分，将会随时间发生变化；位置不确定度会因为速度不确定度的积分而随时间积累；并且，位置估计误差与速度估计误差之间的相关性会越来越强。系统模型对状态来说是确定的，因为它是基于系统的已知特性建立起来的。
  在缺少新的观测信息的情况下，系统中的未知变化会导致状态估计过时，因此状态的不确定度也会随时间增加。这些变化可能是未被测量的动态变化量或仪器输出的随机噪声。例如，在加速度未知的情况下，速度的不确定度必然会随时间而增大。这种与状态真值间的偏差被称作系统噪声( system noise)或过程噪声，其统计特性一般由卡尔曼滤波设计者定义。
• 观测向量( measurement vector)是一组针对同一时刻的系统特性的观测值，是状态向量的函数。例如，观测量包括无线电导航系统中的距离测量值，还有被校准的惯性导航系统与参考导航系统之间导航结果的差值等。系统初始化后，所有状态估计值便可从这些信息中获得。与观测向量相对应的是观测噪声协方差(measurement noise covariance)矩阵，它描述了观测噪声的统计特性。对许多应用来说，新的观测信息按照固定的时间间隔输人到卡尔曼滤波。在某些其他应用中，观测量的时间间隔也可能不是固定的。
• 观测模型( measurement model)描述了在没有观测噪声的情况下，观测向量作为真实状态向量(区别于状态向量估计值)的函数是怎样变化的。例如，被校准的惯性导航系统与参考系统之间速度观测值的差异，与惯性导航系统速度误差直接成正比。就像系统模型一样，观测模型也是确定的，是基于已知的系统特性建立起来的。
• 卡尔曼滤波算法(Kalman filter algorithm)使用观测向量、观测模型和系统模型来获得状态向量的最优估计。

Chap.II 卡尔曼滤波的流程

卡尔曼滤波算法由两个流程构成：系统传播流程和观测更新流程，两个流程每次迭代由10个步骤组成，如下图所示。步骤1~4是系统传播流程，步骤5~10是卡尔曼滤波的观测更新流程。每一次卡尔曼滤波迭代的完成，都是相对于特定的时间点而言的，一次迭代称为一个“节拍”。

• 系统传播(或称为时间传播)流程的目的，是根据已知的系统特性，从上一组观测量修正后的时刻推进到当前一组观测量的时刻，前向预测状态向量估计与误差协方差矩阵。举例来说，位置估计是利用相应的速度估计来预测的。当前时刻，在没有最新观测信息的情况下，系统传播提供了状态向量的卡尔曼滤波最优估计。前两个步骤计算了系统模型的确定性部分和噪声部分。第三步为状态传播( state propagation)，利用该步实现状态向量估计的更新。第四步为协方差传播( covariance propagation)，根据系统噪声引起的状态不确定度的增加，更新误差协方差矩阵。
• 在观测更新(或修正)流程中，用最新观测信息更新状态向量估计和误差协方差矩阵。第五步和第六步计算观测模型的确定性部分和噪声部分。第七步，增益计算( gain computation) ，计算卡尔曼滤波的增益矩阵。此步根据当前状态估计的不确定度和观测噪声情况，对状态的修正量进行优化加权。第八步构建观测向量。第九步，观测更新( measurement update)，用卡尔曼增益加权的观测数据更新状态估计。最后，协方差更新( covariance update)，用已包含在状态向量估计值中的观测数据新信息来更新误差协方差矩阵。

Chap.III 卡尔曼滤波的变种

卡尔曼滤波的变体有：

1. 扩展卡尔曼滤波（Extended Kalman Filter，EKF）： 适用于非线性系统。通过在卡尔曼滤波中引入雅可比矩阵来近似非线性函数，从而处理非线性动态模型和测量方程。
2. 无迹卡尔曼滤波（Unscented Kalman Filter，UKF）：通过选取一组称为“sigma点”的采样点，避免了对非线性函数进行线性化所带来的误差。在处理高度非线性系统时，相对于EKF，UKF通常更稳健。
3. 平均卡尔曼滤波（Averaging Kalman Filter）：通过在时间步长上对估计进行平均，可以降低系统噪声的影响。特别适用于具有较大过程噪声的系统。
4. 粒子滤波（Particle Filter）：通过随机粒子的集合来近似系统状态的后验分布。相对于卡尔曼滤波，粒子滤波适用于非高斯和非线性系统。
5. 时滞卡尔曼滤波（Time Delayed Kalman Filter）：用于处理系统测量值的时滞。适用于那些由于传感器延迟而导致的时滞问题。
6. 变系数卡尔曼滤波（Time-varying Coefficient Kalman Filter）：允许卡尔曼滤波的状态转移矩阵和观测矩阵是时间变化的。适用于系统的参数随时间变化的情况。
7. 平方根协方差滤波（Square Root Covariance Filter）：它在计算过程中使用了协方差矩阵的平方根，以避免负数的问题。这主要是为了提高数值稳定性。在卡尔曼滤波中，协方差矩阵 $P$ 需要保持对称正定，以确保它是一个有效的协方差矩阵，然而在实际数值计算中，因为数值误差等原因，协方差矩阵不再满足这些条件，导致算法的数值不稳定性。平方根协方差滤波通过使用平方根矩阵，可以确保产生的协方差矩阵始终是对称正定的，这提高的数值稳定性，但也增加了一些计算复杂性。

Part.III 卡尔曼滤波算法和模型

因为一些详细的描述放在了子博文中，下面就贴几个流程图吧！

Reference

1. 秦永元, 张洪钺, 王叔华. 卡尔曼滤波与组合导航原理[M]. 西北工业大学出版社, 2012.
2. Paul D .Groves. GNSS与惯性及多传感器组合导航系统原理.第2版[M]. 国防工业出版社, 2015.
3. 崔希璋, 於宗俦, 陶本藻,等. 广义测量平差(新版)[M]. 武汉测绘科技大学出版社, 2001.
4. 通俗理解卡尔曼滤波及其算法实现（实例解析）
5. 一个应用实例详解卡尔曼滤波及其算法实现