卡尔曼滤波推导与理解

假设我们要测量房间的温度，我们可以用温度计测量，由于温度计测量精度不高，有±3度的偏差。我们对房间温度进行了10秒钟的测量，每隔1秒测量一次，假设10次数据分别为：

温度测量数据

25.3度
26.5度
24.0度

（温度计开始受到干扰）
35.0度

（干扰消失）

22.9度

25.6度

23.5度

（开启暖气）

28.8度

30.2度

29.5度

通常房间的温度是比较恒定的，而由于温度计的测量误差和设备干扰，导致数据在剧烈跳变。如果这种数据给用户，用户就会给差评了。

因而，由于测量设备一方面有误差，另一方面会引入干扰，工程上通常不能直接调用测量设备的数据，要将数据进行滤波优化再调用。

均值滤波

我们最熟悉的滤波是均值滤波，先采样N次，将N次的值累加起来，再除以N，就可以得到均值。

假设我们对上述温度测量过程进行均值滤波，N取5次。按照均值滤波算法，得到的温度是：

T1

=(25.3+26.5+24.0+35.0+22.9)/5

=26.7度

T2

=(25.6+23.5+28.8+30.2+29.5)/5
=27.5度

以看出经过均值滤波后的数据不再跳变得那么厉害，但是5秒才刷新一次显然太慢了，而且后来房间开了暖气，这个变化也被均值滤波算法淹没掉了。

一阶滞后滤波

为了提高刷新速率，同时避免淹没一些真实的变化，我们换用一阶滞后滤波算法（真装逼）。

一阶滞后滤波算法公式

a*本次采样值 + (1-a)*上次滤波结果

↑

其中a可以理解为信任比例,如果a取0.5，则代表信任本次采样结果占50%的比例，同时相信上次滤波结果也占50%的比例。

由于第1秒时没有上次滤波结果，我们先假定上次滤波结果为26.0度，权重a为0.6，则依据一阶滞后滤波算法可得：

一阶滞后滤波处理的温度数据

 25.3*0.6+26.0*0.4=25.6度

 26.5*0.6+25.6*0.4=26.1度

 24.0*0.6+26.1*0.4=24.8度

 35.0*0.6+24.8*0.4=30.9度

 22.9*0.6+30.9*0.4=26.1度

 25.6*0.6+26.1*0.4=25.8度

 23.5*0.6+25.8*0.4=24.4度

 28.8*0.6+24.4*0.4=27.0度

 30.2*0.6+27.0*0.4=28.9度

 29.5*0.6+25.6*0.4=29.3度

以上一阶滞后滤波算法的结果既减缓了温度的跳变，又提高了刷新速率。但是它仍然没能排除掉35度的那个干扰，同时在房间开了暖气后温度反应也有点滞后。

问？

那么是否有什么算法既可以避开夸张的突变，又可以快速响应合理的突变，还可以做到实时高效地刷新呢？

卡尔曼滤波

以上问题时有答案的，那就是伟大的卡尔曼滤波

↑

前面的一阶滞后滤波算法有一个权重a，这个a在上述计算过程中是不变的，而卡尔曼滤波算法的核心灵魂在于这个a可以根据测量值的变化作实时的更新。

使用卡尔曼滤波前有几个初始条件需要知道

01

原始值

卡尔曼滤波需要一个原始值给它做第一次迭代用，通常可以用传感器先测量N组数据，然后求取平均值，将这个平均值当做原始值。本文测室温的例子可以先定初始值26.0度。

02

测量系统误差

前面已经假定温度计的测量误差是3度。

03

下一次的最大偏差值

由于房间温度相对是恒定的，即使开启暖气下一次最多也只能变化5度，则这个最大偏差值就是5度。

04

原始值误差

由于原始值是通过N次测量求平均得到的，这里面也会有些误差，假定原始值误差是2度。

我们开始应用卡尔曼滤波算法进行迭代。首先，由于我们相信房间温度恒定，下一秒温度和这一秒温度是一样的。我们用原始值26.0度作为起始温度，则下一秒的估计温度也是26.0度，而下一秒的温度计测量温度是25.3度。那么此时我们有两个可能的温度值：26.0度和25.3度，那究竟该信谁多一点呢？即它们的信任比例是多少？我们将测量值的信任比例定义为kg，kg符合这个协方差算式：

kg^2

=(原始值误差^2+下一次的最大偏差值^2) /

  (原始值误差^2+下一次的最大偏差值^2+测量系统误差^2)

代入本例中有

kg^2=(2^2+5^2)/(2^2+5^2+3^2)=0.763

开根号得kg=0.874

即测量值25.3度的信任比例是0.874，则第1秒的温度为：

T1

=26+(25.3-26)*0.874=25.4度

这个25.4度将会变为下一轮迭代计算的原始值，因为根据卡尔曼滤波后的值更加接近真实世界的值了，所以下一轮原始值误差会变化。

下一轮原始值误差此时变为：

((1-Kg)*(本轮原始值误差^2+下一次的最大偏差值 ^2))^0.5

=((1-Kg)*(2^2+5^2))^0.5

=1.9

既然原始值的误差变了，因为kg^2=(原始值误差^2+下一次的最大偏差值^2)/(原始值误差^2+下一次的最大偏差值^2+测量系统误差^2)，所以kg值也会变化，即信任比例发生变化。

然后开始第二轮迭代计算，第二轮的kg^2=(1.9^2+5^2)/(1.9^2+5^2+3^2)=0.760,即kg=0.872,则第2秒的温度为：

T2

=25.4+(26.5-25.4)*0.872=26.4度

之后开始第3轮的迭代

第4轮

第5轮

……

第n轮

卡尔曼滤波经过多次的迭代后会逐渐缩小原始值误差，输出结果也更接近真实的值。对于测量系统反馈的突变值会大幅降低信任比例，而如果测量系统连续两次输出的突变值很接近，则卡尔曼滤波会认为这个时候环境真的变化了，立马又提高信任比例，使得输出值对真实环境变化的响应很迅速。

如果用户改变下一次的最大偏差值，假设将本例中的最大偏差值5度改为2度，那么测量系统测量值的信任比例将会变小，你将会看到经过滤波输出的温度值跳动很小，对干扰的抑制更加明显，但是对于真实环境改变的响应也会变得更加滞后。

篇文章《卡尔曼滤波（上）》里面有两个公式，可能会比较难以理解一点。因而上篇文章就只把这两个公式当做定理去使用，没有做过多说明。由于本篇文章属于进阶说明，需要对此做一个简单易懂的讲解。

两个公式

接下来我们对这两个公式的来由做简单形象的推导，需要应用到一些概率论知识。

我们还是以房间温度为例，假设温度计测量到此刻房间温度为μ ℃，温度误差的方差为σ^2，则此刻真实世界的温度概率符合正态分布，如下图所示：

正态分布 

在网上随便找的图，数据没有意义，核心内容理解即可。

正态分布的数学模型如下：

由于下一个时刻的房间温度有两个来源，一个是我们根据此刻房间温度估计出来的，还有一个是下一时刻温度计测量到的。不管是估计值还是实测值都有误差，这两个值也都符合正态分布，如下图所示：

估计正态 

测量正态 

在网上随便找的图，数据没有意义，核心内容理解即可。

于是就有一个问题，估计温度和测量温度的正态分布究竟信任谁多一点？这里我们就需要对两个正态分布进行融合，求取融合后温度概率分布。

两个独立正态分布融合

计算过程比较复杂，具体请参考链接中的论文。

注：论文是PDF格式

链接：http://www.tina-vision.net/docs/memos/2003-003.pdf

总

之两个独立正态分布函数最终会融合出一个新的正态分布函数，我们将新的正态分布函数定义为：

μ_算  就是真实世界最有可能的温度

σ_算^2  就是这个温度误差的方差

用图形表示会形象点：

分布融合 

毁图秀秀画的，不精确，核心内容理解即可。

我

们求取μ_算和σ_算，依据论文中的计算结果，有：

上面两式都有这一坨：

大坨的东西看得不舒服，我们用kg表示它:

这个kg通俗点讲就是上篇文章提到的信任比例，不俗得讲就叫卡尔曼增益。简化后有：

这两个式子和文章开头两个式子一毛一样有木有！

下

卡尔曼滤波有五个最基本的公式，我们将对其逐一解读。

以上的公式看着很吓人，但我将会对其进行通俗讲解，看完会有满满收获，所以请别跑开。

前两篇文章都只是讲如何用卡尔曼滤波对单种数据进行滤波处理，然而工程上往往有多种数据需要同时滤波处理，比如加速度传感器可以测量3个维度的加速度信息，这就有3种信号需要做滤波处理。假设我们要对n种数据做卡尔曼滤波，我们应该怎么做呢？

们可以用单种信号的卡尔曼滤波方程，连续写n次，然后求取每种信号各自的滤波值。但是这么多个方程写出来会很庞大，因此我们就用矩阵的方式来描述，矩阵可以将n个方程组整合成一个方程，如下图所示：

图中n个方程组最终化简成一个矩阵表达式：AX=B，矩阵表示是不是很简洁？卡尔曼滤波的五个基本公式中的X,A,B,U,P,Q,Kg,H,R,Z,I就都是矩阵，即五个公式其实是五个方程组。

这里还需要知道一个概念叫向量，向量是一维的矩阵，如下图所示：

卡尔曼滤波的五个公式中的X,U,Z都是一维矩阵，也就是向量。

最后还需要知道一个概念叫做矩阵转置，如下图所示：

至此我们拥有了可以理解卡尔曼滤波最基础的条件了，我们开始进入正题。

……
正
题
……

我们开始理解第一个公式：

X(k│k-1)=AX(k-1│k-1)+BU(k-1)

假设你要对飞机的欧拉角进行卡尔曼滤波，你可以用传感器先获取到一个欧拉角和角速度的初始值，并把这个值当做上一时刻(k-1)的值，然后根据上一时刻(k-1)的值你可以估计出这一时刻(k)飞机的欧拉角为：

Pitch(k)=Pitch(k-1)+GyroY(k-1)*dt    
Roll(k)=Roll(k-1)+GyroX(k-1)*dt
Yaw(k)=Yaw(k-1)+GyroZ(k-1)*dt

补充说明

Pitch是俯仰角，GyroY是俯仰角对应的角速度，dt是k-1到k的时间。

Roll是滚转角，GyroX是滚转角对应的角速度，dt是k-1到k的时间。

Yaw是偏航角，GyroZ是偏航角对应的角速度，dt是k-1到k的时间。

想办法将上面的方程组简化成矩阵表示：

设:

k|k-1代表这一时刻根据上一时刻的估计值
k-1|k-1代表上一时刻卡尔曼处理得到的最优值
k-1代表上一时刻实测值

补充说明

这里由于才开始第一轮的滤波准备，没有上一时刻卡尔曼处理得到的最优值，就将k-1时刻测量到的值直接当做最优值。

方程组转换矩阵

则上面三个方程组就可以表示为：
     X(k│k-1)=AX(k-1│k-1)+BU(k-1)
请注意这里A与B不一定要是对角线才有值的矩阵，只是在这个简单例子中是这样的。

补充说明X(k│k-1)就是指根据上一时刻估计出这一时刻的估计值向量。

A是上一时刻状态转移到这一时刻状态的状态转移矩阵。

X(k-1│k-1)是上一时刻的最优值向量，如果卡尔曼滤波是第一次执行，就不存在上一时刻处理得到的最优值，只能推荐将传感器上次获取到的值当做最优值。

B是控制量矩阵。

U(k-1)是上一时刻控制量测量值向量。

至此，第一个公式已经理解。

我们开始理解第二个公式：

P(k│k-1)=AP(k-1│k-1) A^T+Q

回到n种数据做卡尔曼滤波的主题。依据k-1时刻的值我们可以得到k时刻的估计值，但是这个估计值和真实世界的值含有两种误差，一种误差是：

k-1时刻真实世界值X(k-1)和
最优值X(K-1|K-1)之间的误差还有一种误差是：

控制量及其对时间积分造成的误差

后者误差我们用误差向量W(k-1)表示

补充说明

w1,w2…wn指n种数据各自控制量及其对时间积分造成的误差。

卡尔曼滤波的本质

减小误差的方差

我们想办法先将估计值的这两种误差提取出来，可以令真实世界的值为：

X(k)=AX(k-1)+BU(k-1)+W(k-1)

则真实值和估计值之间的误差为：

对这个误差求取方差，矩阵的方差公式为：

补充说明其中D代表方差
YT是Y的转置cov是协方差(不懂可以先百度：-)

则误差的方差D为:

依据方差公式：
D(X±Y)=D(X)+D(Y)±2cov(X+Y)
可得：

由于：
“k-1时刻真实世界值X(k-1)和最优值X(K-1|K-1)之间的误差”
与
“由控制量及其对时间积分造成的误差”

不会互相制约影响，即两种误差独立，则根据协方差元素独立则协方差为0的定理，可得：

则误差的方差可进一步推导为：

矩阵有这么一个性质

应用这个性质我们可得：

设

则误差的方差可简化为：

至此，第二个公式已经理解。

特殊声明：

这回我们要先理解第四个公式，后面会再回头理解第三个公式，原因后面看着看着就能明白了。

第四个公式是：

X(k│k)=X(k│k-1)+Kg(k)(Z(k)-HX(k|k-1))

在理解第四个公式前，需要拿传感器先测量下这一时刻的值，设这一时刻传感器的测量数据向量为Z(k)。由于有些时候X(k)向量中有一些元素是没法用传感器直接测量得到的，但是这个元素又可以利用传感器的其它测量值推算出来，因而我们将这一时刻传感器的测量数据向量定义为：

补充说明

H矩阵可以修饰X(k)中某些元素不能通过传感器直接测量得到，通常H矩阵的斜对角元素是由0和1构成的，0就代表对应元素无法用传感器直接测量，1代表对应元素可以用传感器直接测量。

V(k)向量代表传感器测量误差，比如称重秤的误差为10g，这10g就是这个V(k)。

那这第四个公式在逻辑上就很好理解。先用这一时刻传感器的测量值减去根据上一时刻估计出来的值，得到残差，然后用一个比例系数矩阵Kg乘上这个残差，再加上根据上一时刻的估计值，就可以得到这一时刻最有可能的值，也就是最优值。这个最优值既不完全相信传感器，也不完全相信估计值，只要控制好比例系数，就能得到最适合的值。

至此，第四个公式已经理解。

我们开始理解第五个公式：

P(k│k)=(I-Kg(k)H)P(k|k-1)

回顾下之前第二个公式中的P(k|k-1)，是代表上一时刻的估计值和此刻真实世界值的误差的方差。而P(k|k)代表此刻计算得到的最优值和此刻真实世界值的误差的方差。则有：

P(k│k)=D[X(k)-X(k|k)]

将：

X(k│k)=X(k│k-1)+Kg(k)[Z(k)-HX(k|k-1)]

代入有：

P(k│k)=D{X(k)-[X(k│k-1)+Kg(k)(Z(k)-HX(k│k-1))]}

将

Z(k)=HX(k)+V(k)

代入有：

补充说明

这里的I是元素全为1的矩阵。
由于Kg(k)V(k)属于传感器测量误差，不会制约影响此刻真实世界值和上一时刻估计值的误差，即逗号前后二者独立，协方差为0。

则参照前面的矩阵计算公式有：

设：

有：

矩阵具有这种性质：

依据以上性质，由于I是元素全为1的对称阵， P(k│k-1)是方差阵，方差阵也是对称阵，则可得：

由于卡尔曼滤波是一个不断减小最优值和此刻真实世界值的误差的方差的过程。P(k|k)是最优值和此刻真实世界值的误差的方差矩阵，其对角线元素就是各个元素各自的误差的方差。为了让所有元素的方差最小，我们把每个元素的方差都加起来，即把P(k|k-1)的对角元素方差加起来，求取总的方差值，只要方差值总和最小，则就实现了最优值和此刻真实世界值的误差的方差最小化的目的。将方差矩阵对角元素相加的做法也称为矩阵的迹，通常用tr表示。则:

现在，这个由方差矩阵对角元素总和组成的tr[P(k|k)]函数需要取最小值，他的大小收到Kg(k)这个权重矩阵的影响，我们必须找到最适合的Kg(k)使得和值最小。

数学上有导数为0取极值的方法，如下图所示：

矩阵的迹也有求导的公式：

则我们用 tr[P(k|k)]对Kg(k)求导，得：

由于HP(k│k-1) HT和R(K)都是协方差矩阵，所以他们的转置等于自身，则：

在理解第五个公式的过程中，不小心就推出了第三个公式了，即比例系数矩阵Kg(k)最适合的值已经得到。

那么第五个公式就得以理解

将上式替换进P(k|k)表达式中:

至此，第五个公式已经理解