卡尔曼滤波算法

了解过导航、雷达数据处理的必然听过卡尔曼滤波，因为最近有项目需求，要验证一下卡尔曼滤波对结果的优化程度，所以入门学习一下卡尔曼滤波器。毕竟是经典的滤波器，网上关于卡尔曼滤波的代码和文章有很多，一维滤波的代码也很简单，就几条代码，拿来用很容易。但为了致敬卡神，则花时间潜心拜读了一些有关其背后思想的文章，看一轮文献下来，总的感受就以下两点：

难见把卡尔曼滤波讲得特别通透的博客（参考文献有比较好的外文文献及其翻译）；

同一个专业名词各种翻译并且混合使用，无故增加了阅读障碍；

不同的文献使用的不同数学表达式，各种各样很久没见过的统计学符号会吓退你；

我数学思维一般，统计学的知识更是忘得差不多了。

基于以上几点，我断断续续花了几天时间，理解和消化了其中的部分基本思想，由于能力有限，不能够把卡尔曼滤波器从核心思想到实际运用讲的特别透彻，再加上写本博客的初衷是梳理总结。因此，在看这篇博客之前，更建议你先阅读博客后面列举的参考文献。

1. 简介
以下是关于卡尔曼滤波的简介，来自于英文文献的直译：

卡尔曼滤波器是一种基于估计算法的递归状态空间模型。换句话说，它是一种最优的递归数据处理算法，故卡尔曼滤波器也称为Predictor-Corrector算法。
该滤波器以Rudolph E. Kalman的名字命名，是他于1960年发表的著名论文，描述了用一种递归方法来解决离散数据线性滤波器的问题。卡尔曼滤波器会处理所有可用的测量值，无论其精度如何，都可以使用来估计相关变量的当前值。

通过简介，卡尔曼滤波器的特点可以用三个关键词描述：线性、估计以及递归。

2. 应用简述
一句话简述卡尔曼滤波器就是：解决估计值和测量值之间的权重关系，从而得到一个更接近真实值的结果，这个结果我们叫最优估计。

实际运用中，我们往往是离散的获取一个个的观测量（要么就是间歇性的测量，要么就是AD采样的离线数据），比如雷达观测飞机的运动轨迹、传感器捕获被测物的某个物理量变化，那么首先建立测量系统模型，运用初始值（当前值）来估计下一状态/值，同时比较下一状态的实际测量值，我们知道估计值可能不准确（被测物状态不可能一成不变），测量值也未必有完美的精度（噪声存在），因此究竟相信哪一个更多一些，是估计值还是测量值？那么卡尔曼滤波就是解决两者的权重问题（不确定性问题），计算一个结果，使其更接近于被测物真实的状态，依次递归下去。

卡尔曼滤波器广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如人脸识别，图像分割，图像边缘检测等等。

话说卡尔曼滤波器用到实际中，是要有效的把上面的概念“具体化”，这也就是很多时候我们知道一些知识的概念，但不知道如何更好的使用它来解决实际问题。

3. 前提条件
一个算法并不是能适用于任何场景，在使用线性卡尔曼滤波器前，它有两个假设限定了它的应用场景，即：

1.系统是线性的

2.系统和测量噪声是高斯白噪声

什么是高斯白噪声？即噪声满足正态分布，表述如下：

1.高斯白噪声在时间尺度上是互不相关的，即上一时刻的噪声状态并不能决定下一时刻的噪声状态；

2.噪声在所有频率上具有相等的功率，即功率谱密度服从均匀分布；

4. 分析过程
4.1 基本方程
先直接扔出卡尔曼滤波的经典5个方程（来自于参考文献）：

预测（估计）状态方程

更新方程

以上5个方程以矩阵运算形式代表了线性卡尔曼滤波算法的一般形式（不同文献的数学表达方式略有不同）。第一眼看到这几个方程里面的F、K、H之类的变量以及符号肯定是蒙圈的，即使了解了符号代表的意义，在实际过程中怎么使用可能也不是很清楚。

4.2 方程的解释
【注：此节不会完整的再验算一次推导过程，因为篇幅有限，我只能根据我的理解，解释捋清推导过程的一个基本脉络，并解释参考文献中稍微有点绕的地方】

上述算法的一般方程来自于参考文献3，文中以获取机器人的位置和速度这两个变量为例，推导出了（二维）矩阵形式的一般方程，所以结合案例，总结一下方程中各变量符合所代表的意义。

4.2.1 预测（估计）状态方程

【注：以下表述中“预测”和“估计”表示一个意思】

预测状态方程是依据被测对象的数据模型建立的，如例子中所示，机器人的状态x包括位置p和速度v两个信息：

此后引入概率论和数理统计的概念，根据卡尔曼滤波的前提条件“系统噪声为高斯白噪声”，即在外界干扰等因素的存在，使得机器人状态中的位置和速度均符合高斯分布，即有各自的期望以及方差，并且由于位置和速度两者是相关的（速度决定单位时间内的位移），所以方差为协方差，则k时刻的状态期望及协方差可以表示为：

【注：文献中协方差用的是求和号，实在看不惯，换成教科书中的c】

所谓预测方程，即通过当前状态预测下一状态。由运动方程（系统线性）可知，k时刻的状态期望由k-1时刻状态期望决定：

【注：用两种颜色的字体表示不同的状态信息】其中delta_t代表两次预测之间的时间差，并假设机器人处于匀速运动状态。将上两式用矩阵形式表示：

其中：

表示系统状态系数矩阵。由推导过程可见系统状态系数F_k是与系统模型紧密相关的，此处是运动方程，换个模型，F_k随之发生改变。

那么k-1时刻与k时刻的系统估计不确定度（协方差矩阵）又存在什么样的关系呢？已知系统状态分布关系：

那么系统分布的每一个点由k-1时刻变化到k时刻，由上述所知都需乘以一个系统状态系数，那么有：

这是由方差运算的基本性质所决定的。因此有：

【注：一开始在文献中看到此式的解释有点不明白，主要还是感觉符号的使用不规范以及自身概率论基础不牢导致，后来这样理解就稍微清晰一点，上面有关系统状态系数F_k的推导用的是状态分布的期望，而此处又需要从分布出发来考虑】

综上所述，得到系统预测方程：

与本章开头给的一般公式还存在一点细节差异。

在上述分析的过程都是基于假定机器人的运动是匀速运动的，如果运动过程中存在一个加速度a呢，那么运动方程可以改写为：

同样，改写为矩阵形式，得到：

其中：

B_k称为控制矩阵，u_k称为控制向量，我理解的这两项乘积代表对系统施加的“稳定”的外部控制机制，比如就是加减速过程。由此可见，依然与系统的模型有关。【注：有的文献把这项又叫外部噪声，我感觉不是很合适，因为本项更像是对系统施加的稳定的、持续的影响】

另外如果外部给系统施加了一个独立的噪声（依旧要求为高斯白噪声），即分布满足N(0,Q_k)，期望为0，方差为Q_k。同样，由方差计算的基本性质（两个独立分布和的期望和方差性质）可以得到：

忽略掉期望的0项，即得到本章开头的预测方程。

4.2.2 更新方程

更新过程在有些地方又叫“修正过程”，实际上就是真正计算估计值与实测值之间权重的过程。

在上一节中，我们只是对系统在不同时刻的状态进行了“估计”或者“预测”，说白了就是一种基于模型的猜测，实际真正的状态是怎样的，我们还不知道，所以此节的分析要引入“测量”，即某个时刻我们通过检测手段，如GPS、加速度传感器，来真实的获知当前系统状态。

系统的位置和速度仅仅是物理状态的一种表征，有的时候我们通过传感器无法直接获取这样一种物理量，比如加速度传感器输出的并不是加速度，而是电压值，因此需要引入一个空间转换系数矩阵****H，使得传感器的测量空间与系统物理状态空间可以转换。

假设k时刻，系统状态在系统物理空间状态为（X_k，P_k），转换到传感器的测量空间，即：

推导过程与系统状态系数矩阵类似。值得注意的是，此时仅仅代表的是把当前状态的空间转换到传感器测量空间，可以简单理解为一种量纲或者单位的转换，并不是传感器真正的测量值，所以也叫估计测量值（预测测量值）。

【注：文献中看到这里说是传感器的读数分布，我就又蒙圈了，我理解的应该只是通过系统当前状态分布转换到相对应的传感器输出，仅仅是一种状态空间转换的概念。举个可能不太恰当的例子：水当前的状态是沸腾的，那么转换到水壶温度计的读数应该是100℃】。

很多时候，传感器的输出值已经经过处理，与系统的状态对应，所以往往H的值为1。

与此同时，我们通过机器人身上的传感器，如GPS和加速度计，获取到系统当前真实的状态，由于噪声以及传感器精度等问题，测量结果并不是一个稳定的结果，如某个时刻传感器的实测值为Z_k，在这个结果上，实际也叠加有噪声，测量噪声也符合高斯分布（这也是卡尔曼滤波的前提条件），协方差为R_k。

至此，我们已将估计值转换到了传感器空间，并且此时又获得了传感器真实的测量值，如下图所示，那么我们究竟应该“相信”哪一个结果呢？或者说信哪一方更多一些。我在想这就是卡神最NB的地方，他的博士论文论证了这样一个结果：我们应该相信两个分布的乘积！

以上表述中，已经说明了估计测量值分布以及传感器测量值分布均满足高斯分布，那么他们的乘积依旧满足高斯分布，参考文献中给出了推导证明过程。假定两个高斯分布分别为N_0和N_1，那么乘积的新分布N（带撇）的期望和方差如下：

经过一系列的分解，我们令：

那么新分布的期望和方差可以表示为：

用矩阵形式表示（我们又不得不用求和号表示协方差）：

K就是非常重要的卡尔曼增益矩阵。

将估计测量分布和传感器实际测量分布，分布代入上式中，并化简得到：

这就是我们最终得到的更新方程。

更新方程中的X和P（带撇），就是我们此刻的最佳估计，也就是最接近于真实值的结果。我们可以看到，卡尔曼增益扮演的是权重的角色，K越小，我们越相信估计值；K越大，我们越相信测量值。

下图给出的是完整的流程图，每次计算得到的最佳估计结果，代入到下一次的预测中再计算，以实现递归。

以上。

5. 实际运用
上面第四章已经完成了线性卡尔曼滤波算法的推导，那么如何在实际过程来使用呢？实际上上上一章断断续续也说了公式中一些符合的实际含义，那么本章真正意义的把公式用到实际的数据处理当中。

再把卡尔曼滤波算法的5个经典公式扔出来：

再回顾一下式中一些参数的含义：

Bu——控制项，对系统施加的一些稳定的，导致系统状态发生稳定变化的因素；

Q——系统状态估计过程的噪声分布的方差，表示系统估计不确定度的偏差；

F——系统状态系数矩阵，决定系统上一时刻到这一时刻的变化模型；

H——状态空间转换系数，目的是把系统的物理状态转换到跟测量设备（传感器）一样的空间，方便在同一维度下比较；

Z——测量设备（传感器）的实际测量值；

R——测量的不确定度，与噪声和精度有关；

第四章推导的卡尔曼滤波算法是基于运动学建立的，有位置（位移）和速度以及加速度控制等概念，那如何运用在其他领域呢？如加速度传感器测量物体振动、温度传感器测量温度值，以及其他一些一维测量领域，又怎么来用卡尔曼滤波呢。尤其是方程中的Q R的取值等，有些文献让读者毫无根据的瞎试各种Q R，虽然也许也能得到满意的结果，但做法不敢苟同。

以文献中（第四部分下）测温案例类似，假定有一个具有恒温控制系统（带温度计）的液灌，想要知道液体的最佳估计温度，那么如何运用公式：

由于是有温控系统，估计液体在k-1时刻和k时刻的温度状态是一致的，那么Bu项为0，F为1，则：

对于系统估计（协）方差P，由于我们不可能完全知道系统的模型是怎样的，不妨先预设一个值10000（估计不确定度为100，则方差为100^2）【注：标准偏差是描述随机变量围绕均值的分散程度的，是可以用来表示不确定度的，而标准偏差的平方就是方差，那方差也是可以反映不确定度的】，没关系P最终会收敛的。对于温控系统，存在一定的温控偏差，视为系统过程噪声Q，预设为0.0001，则：

由于对液灌中液体的温度估计是用温度值（℃）表征，温度计测量出来的也是温度值，所以估计和测量属于同一个状态空间，不存在状态转换，则H为1。（如果模型有相应的变换，则H以实际为准）

一般应用，我们是知道温度计（传感器）的测量误差，则R需按照设备设置，此案例中是标准差0.1℃，则方差R=0.01【注：所以R值原则上是不能瞎设的】

那么最终有：

由此，每一次计算完最佳估计之后，在迭代进预测方程，最终实现卡尔曼滤波算法。完整迭代过程，可以参见文献。

6. 后续
6.1 结果偏差大
实际运用过程中，卡尔曼滤波器可能存在滞后误差，即所谓的最佳估计最终与真实结果偏差较大。导致滞后误差的原因有两个：

系统动力模型与实际不符；
过程模型的可靠性。选择了非常低的过程噪声（案例中的Q=0.0001），而实际温度波动要大得多。

6.2 非线性系统
理解卡尔曼滤波都是从线性系统出发，处理非线性系统可以采用扩展卡尔曼滤波，我还没学习过……

7.参考文献
1.KalmanFilter.net
2.轻松理解卡尔曼滤波
3.《How a Kalman filter works, in pictures _ Bzarg》
4.《Products and Convolutions of Gaussian Probability Density Functions》
5.图解卡尔曼滤波

转载自：https://www.cnblogs.com/rouwawa/p/11643850.html