通俗解释卡尔曼滤波

汽车位置p和速度v例子：有点矩阵

https://blog.csdn.net/codesamer/article/details/81191487

飞机速度和位置，跟上面差不多，但是无矩阵

https://blog.csdn.net/varyshare/article/details/95065650

昆虫和食物供给例子:

https://www.jianshu.com/p/f3ba1f3de756

 

https://www.cnblogs.com/polaris-shi/p/9562874.html

 

温度的例子通俗易懂：

假设你有两个传感器，测的是同一个信号。再假设你知道其中贵的那个传感器应该准一些，便宜的那个应该差一些。那有比取平均更好的办法吗？加权平均。怎么加权？假设两个传感器的误差都符合正态分布，假设你知道这两个正态分布的方差，用这两个方差值，（此处省略若干数学公式），你可以得到一个“最优”的权重。接下来，重点来了：假设你只有一个传感器，但是你还有一个数学模型。模型可以帮你算出一个值，但也不是那么准。怎么办？把模型算出来的值，和传感器测出的值，（就像两个传感器那样），取加权平均。OK，最后一点说明：你的模型其实只是一个步长的，也就是说，知道x(k)，我可以求x(k+1)。问题是x(k)是多少呢？答案：x(k)就是你上一步卡尔曼滤波得到的、所谓加权平均之后的那个、对x在k时刻的最佳估计值。于是迭代也有了。这就是卡尔曼滤波。信号处理中的滤波实质上就是讲降噪。一个信号通常是有用的原始信号与一个噪声的叠加，噪声一般用高斯白噪声描述，它们混在一起了，我放大信号噪声也放大，缩小噪声信号也缩小，一般我没有通用办法改善信噪比。但是如果我有好几个信号，里面包含了相同的原始信号，我把它们叠加在一起，原始信号是相干叠加，幅度变成两倍功率变成了四倍，而噪声是非相干叠加，许多地方会相互抵消，功率只变成两倍，这样信噪比就提高了一倍。所有的“滤波”降噪，本质上都是通过相干叠加增强原始信号提高信噪比的。上面的情况是信噪比相同，如果信噪比不同呢？用一步柯西不等式就能证明，对于噪声功率相同的信号，只要按各自信号功率比例混合，就能最大化信噪比。最后，如果不是另采了一个信号，而是用历史测量值来预测，将预测值作为另一个信号，这个预测值的信噪比是可以推算出来的，再采用前面的方法就得到卡尔曼滤波。

 

卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。 关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表。

数据滤波是去除噪声还原真实数据的一种数据处理技术, Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态. 由于, 它便于计算机编程实现, 并能够对现场采集的数据进行实时的更新和处理, Kalman滤波是目前应用最为广泛的滤波方法, 在通信, 导航, 制导与控制等多领域得到了较好的应用。

这个温度的例子挺好：

https://blog.csdn.net/gnehcuoz/article/details/53730940

 最近发现，只有理论推导而没有贴实用代码的博客阅读量都比较少，由此可见，技术博客也是快餐文化的一种。本人后面的博客尽量也贴一些典型代码，来满足更读者的胃口。卡尔曼滤波算法相关的代码后续补上\^_\^

0、卡尔曼滤波的核心内容
假设测量值和估计值都不是100%可信的，通过如下两步主要操作来估计真实值 
根据前一时刻的最优估计 x(k−1|k−1)x(k−1|k−1) 得到当前时刻的普通估计 x(k|k−1)x(k|k−1)
根据当前时刻的普通估计 x(k|k−1)x(k|k−1) 和当前时刻的观测 z(k)z(k)，得到当前时刻的最优估计 x(k|k)x(k|k)

1、问题描述

        假设我们要测量房间的温度，用温度计进行了10分钟的测量，每隔1分钟测量一次，这10次的测量结果如下。我们还明白一个事实：由于测量设备（温度计）自身存在误差，测量过程也会引入干扰，因此，工程上通常不能直接使用测量设备的数据，而是先要对数据进行处理。那么问题来了，该如何进行处理？
25.3
26.5
24.0
35.0（温度计受到干扰）
22.9
25.6
23.5（开启暖气）
28.8
30.2
29.5
 

2、方案一：均值滤波

        均值滤波可简单描述为：连续采样 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

均值滤波确实减缓了数据的跳变，但其更新频率太低，而且真实的变化过程（开启暖气）容易被淹没。

3、方案二：一阶滞后滤波

        为了提高更新频率，同时避免淹没真实的变化过程，可以采用一阶滞后滤波算法， 
本次滤波结果=α∗本次采样值+(1−α)∗上次滤波结果
本次滤波结果=α∗本次采样值+(1−α)∗上次滤波结果
公式中的 αα 表示对本次采样值的信任程度。设 α=0.6α=0.6，由于第1分钟时没有上次滤波结果，可以根据经验初始化为26.0，则结果为：

0.6*25.3+0.4*26.0=25.6
0.6*26.5+0.4*25.6=26.1
0.6*24.0+0.4*26.1=24.8
0.6*35.0+0.4*24.8=30.9
0.6*22.9+0.4*30.9=26.1
0.6*25.6+0.4*26.1=25.8
0.6*23.5+0.4*25.8=24.4
0.6*28.8+0.4*24.4=27.0
0.6*30.2+0.4*27.0=28.9
0.6*29.5+0.4*25.6=29.3

一阶滞后滤波算法既减缓了温度的跳变，又提高了更新频率。但是它仍然没能排除掉干扰，而且在房间开启暖气后温度变化有点滞后。

4、方案三：卡尔曼滤波

        一阶滞后滤波算法的权重 αα 在计算过程中是保持不变的，而卡尔曼滤波算法的核心在于，可以根据测量值的变化实时更新权重。使用卡尔曼滤波需要知道的几个初始条件：
原始值：用于启动迭代，通常取传感器前 N 组测量数据的均值。本例中，原始温度取26.0度。
原始值误差：原始值通常是根据经验得到的，存在误差。本例中，假定原始温度误差是2度。
测量系统误差：假定温度计的测量误差为是3度。
下一次的最大偏差：由于房间温度相对恒定，即使开启暖气下一次最多也只能变化5度。
        使用卡尔曼滤波算法进行迭代：首先，用26.0作为原始温度，由于认为房间温度恒定，因此下一分钟的估计温度也是26.0度。其次，下一分钟的测量温度是25.3度。此时，我们有两个可以参考的温度值，26.0度和25.3度，究竟该如何权衡？定义测量值的信任比例为
kg=原始值误差2+下一次的最大偏差2原始值误差2+下一次的最大偏差2+测量系统误差2−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−⎷
kg=原始值误差2+下一次的最大偏差2原始值误差2+下一次的最大偏差2+测量系统误差2
        代入本例中可得，测量值25.3的信任比例为 
kg=22+5222+52+32−−−−−−−−−−√=0.874
kg=22+5222+52+32=0.874
因此，第一分钟的最优温度估计为

T1=26.0*(1-0.874)+25.3*0.874=25.4

        这个25.4度将会成为下一轮迭代的原始值。此外，由于卡尔曼滤波的结果更加接近真实世界的数值，因此下一轮迭代的原始值误差也会被更新 
下一轮原始值误差=(1−kg)∗(本轮原始值误差2+下一次的最大偏差2)−−−−−−−−−−−−−−−−−−−−−−−−−−−−√
下一轮原始值误差=(1−kg)∗(本轮原始值误差2+下一次的最大偏差2)
        代入本例中可得 
原始值误差=(1−0.874)∗(22+52)−−−−−−−−−−−−−−−−−−√=1.9
原始值误差=(1−0.874)∗(22+52)=1.9
        下面进行第二轮迭代。首先，计算测量值26.5的信任比例 
kg=1.92+521.92+52+32−−−−−−−−−−−−√=0.872
kg=1.92+521.92+52+32=0.872
其次，计算第二分钟的最优温度估计

T2=25.4*(1-0.872)+26.5*0.872=26.4

最后，更新下一轮迭代的原始值误差 
原始值误差=(1−0.872)∗(1.92+52)−−−−−−−−−−−−−−−−−−−√=1.9
原始值误差=(1−0.872)∗(1.92+52)=1.9
        之后进行第三轮迭代 ⋯⋯

        卡尔曼滤波经过多次迭代后会逐渐缩小原始值误差，输出结果也更加接近真实值。如果将本例中的下一次最大偏差从5度改为2度，那么传感器测量值的信任比例将会变小，滤波器输出的温度值跳动也会变小，对干扰的抑制作用会更加明显，但对于真实环境的响应也会变得更加滞后。

5、单变量形式推导

        参考：http://www.tina-vision.net/docs/memos/2003-003.pdf
        设温度的估计值服从正态分布 
f(x)=12πσ2估−−−−−√exp(−(x−μ估)22σ2估)
f(x)=12πσ估2exp⁡(−(x−μ估)22σ估2)
        设温度的测量值也服从正态分布 
g(x)=12πσ2测−−−−−√exp(−(x−μ测)22σ2测)
g(x)=12πσ测2exp⁡(−(x−μ测)22σ测2)
        融合两个正态分布，可得 
h(x)=f(x)g(x)=12πσ测σ估exp(−(x−μ测)22σ2测−(x−μ估)22σ2估)
h(x)=f(x)g(x)=12πσ测σ估exp⁡(−(x−μ测)22σ测2−(x−μ估)22σ估2)
        令指数部分 
β=(x−μ测)22σ2测+(x−μ估)22σ2估=(σ2测+σ2估)x2−2(μ测σ2估+μ估σ2测)x+μ2测σ2估+μ2估σ2测2σ2测σ2估
β=(x−μ测)22σ测2+(x−μ估)22σ估2=(σ测2+σ估2)x2−2(μ测σ估2+μ估σ测2)x+μ测2σ估2+μ估2σ测22σ测2σ估2
        化简可得 
β=(x−μ测σ2估+μ估σ2测σ2测+σ2估)22σ2测σ2估σ2测+σ2估+(μ测−μ估)22(σ2测+σ2估)
β=(x−μ测σ估2+μ估σ测2σ测2+σ估2)22σ测2σ估2σ测2+σ估2+(μ测−μ估)22(σ测2+σ估2)
        于是，两个正态分布的乘积也服从正态分布 
μ新=μ测σ2估+μ估σ2测σ2测+σ2估=kg∗μ测+(1−kg)∗μ估σ新=σ2测σ2估σ2测+σ2估−−−−−−−−⎷=(1−kg)∗σ2估−−−−−−−−−−−√
μ新=μ测σ估2+μ估σ测2σ测2+σ估2=kg∗μ测+(1−kg)∗μ估σ新=σ测2σ估2σ测2+σ估2=(1−kg)∗σ估2
        其中，kgkg 就是前文提到的信任比例，即卡尔曼增益 
kg=σ2估σ2测+σ2估
kg=σ估2σ测2+σ估2

6、矩阵形式推导

        卡尔曼滤波其实就5个公式 

X(k|k−1)=AX(k−1|k−1)+BU(k−1)P(k|k−1)=AP(k−1|k−1)AT+QKg(k)=P(k|k−1)HT[HP(k|k−1)HT+R]−1X(k|k)=X(k|k−1)+Kg(k)[Z(k)−HX(k|k−1)]P(k|k)=[I−Kg(k)H]P(k|k−1)
X(k|k−1)=AX(k−1|k−1)+BU(k−1)P(k|k−1)=AP(k−1|k−1)AT+QKg(k)=P(k|k−1)HT[HP(k|k−1)HT+R]−1X(k|k)=X(k|k−1)+Kg(k)[Z(k)−HX(k|k−1)]P(k|k)=[I−Kg(k)H]P(k|k−1)

        其中，X(k−1|k−1)X(k−1|k−1) 是上一时刻的最优估计；X(k|k−1)X(k|k−1) 是当前时刻的基本估计，由上一时刻的最优估计得来；AA 是状态转移矩阵；BB 是控制量矩阵；U(k−1)U(k−1) 是上一时刻的控制量。

6.1 第二个公式
        设真实世界中，kk 时刻的状态为 
X(k)=AX(k−1)+BU(k−1)+W(k−1)
X(k)=AX(k−1)+BU(k−1)+W(k−1)
则 kk 时刻的真实值 X(k)X(k) 和基本估计 X(k|k−1)X(k|k−1) 之间的误差为 
e(k|k−1)===X(k)−X(k|k−1)AX(k−1)+BU(k−1)+W(k−1)−[AX(k−1|k−1)+BU(k−1)]AX(k−1)−AX(k−1|k−1)+W(k−1)
e(k|k−1)=X(k)−X(k|k−1)=AX(k−1)+BU(k−1)+W(k−1)−[AX(k−1|k−1)+BU(k−1)]=AX(k−1)−AX(k−1|k−1)+W(k−1)
该误差的方差为 
D[e(k|k−1)]======D[X(k)−X(k|k−1)]D[AX(k−1)−AX(k−1|k−1)+W(k−1)]D[AX(k−1)−AX(k−1|k−1)]+D[W(k−1)]+2cov(AX(k−1)−AX(k−1|k−1),W(k−1))D[AX(k−1)−AX(k−1|k−1)]+D[W(k−1)][AX(k−1)−AX(k−1|k−1)][AX(k−1)−AX(k−1|k−1)]T+W(k−1)W(k−1)TA[X(k−1)−X(k−1|k−1)][X(k−1)−X(k−1|k−1)]TAT+W(k−1)W(k−1)T
D[e(k|k−1)]=D[X(k)−X(k|k−1)]=D[AX(k−1)−AX(k−1|k−1)+W(k−1)]=D[AX(k−1)−AX(k−1|k−1)]+D[W(k−1)]+2cov(AX(k−1)−AX(k−1|k−1),W(k−1))=D[AX(k−1)−AX(k−1|k−1)]+D[W(k−1)]=[AX(k−1)−AX(k−1|k−1)][AX(k−1)−AX(k−1|k−1)]T+W(k−1)W(k−1)T=A[X(k−1)−X(k−1|k−1)][X(k−1)−X(k−1|k−1)]TAT+W(k−1)W(k−1)T
在数学形式上有 
D[e(k−1|k−1)]=D[X(k−1)−X(k−1|k−1)]=[X(k−1)−X(k−1|k−1)][X(k−1)−X(k−1|k−1)]T
D[e(k−1|k−1)]=D[X(k−1)−X(k−1|k−1)]=[X(k−1)−X(k−1|k−1)][X(k−1)−X(k−1|k−1)]T
        因而 
D[e(k|k−1)]=AD[e(k−1|k−1)]AT+W(k−1)W(k−1)T
D[e(k|k−1)]=AD[e(k−1|k−1)]AT+W(k−1)W(k−1)T
        令 
P(k|k−1)=D[e(k|k−1)]Q(k−1)=W(k−1)W(k−1)T
P(k|k−1)=D[e(k|k−1)]Q(k−1)=W(k−1)W(k−1)T
        则 
P(k|k−1)=AP(k−1|k−1)AT+Q(k−1)
P(k|k−1)=AP(k−1|k−1)AT+Q(k−1)
        其中，P(k−1|k−1)P(k−1|k−1) 为 k−1k−1 时刻的真实值 X(k−1)X(k−1) 和最优估计 X(k−1|k−1)X(k−1|k−1) 之间的误差的方差，卡尔曼滤波的本质就是减小误差的方差。

6.2 第四个公式

        设当前时刻传感器的测量数据为 Z(k)Z(k)，将其定义为 
Z(k)=HX(k)+V(k)
Z(k)=HX(k)+V(k)
需要说明的是，有时 X(k)X(k) 中的部分数据是无法用传感器直接测量的，但这些数据又可以通过其它测量值推算出来，HH 矩阵就用来表示 X(k)X(k) 中数据的可测量情况。V(k)V(k) 表示传感器的测量误差。至此，第四个公式便很容易理解：当前时刻的残差，乘以当前时刻的卡尔曼增益，再加上当前时刻的基本估计，便得到当前时刻的最优估计。最优估计其实是传感器测量值和基本估计的加权平均。

6.3 第三个公式

        上文中提到，P(k|k−1)P(k|k−1) 是 kk 时刻的真实值 X(k)X(k) 和基本估计 X(k|k−1)X(k|k−1) 之间的误差的方差。那么，P(k|k)P(k|k) 就是 kk 时刻的真实值 X(k)X(k) 和最优估计 X(k|k)X(k|k) 之间的误差的方差， 
P(k|k)==============D[X(k)−X(k|k)]D{X(k)−X(k|k−1)−Kg(k)[Z(k)−HX(k|k−1)]}D{X(k)−X(k|k−1)−Kg(k)Z(k)+Kg(k)HX(k|k−1)}D{X(k)−X(k|k−1)−Kg(k)[HX(k)+V(k)]+Kg(k)HX(k|k−1)}D{X(k)−X(k|k−1)−Kg(k)HX(k)−Kg(k)V(k)+Kg(k)HX(k|k−1)}D{[I−Kg(k)H][X(k)−X(k|k−1)]−Kg(k)V(k)}D{[I−Kg(k)H][X(k)−X(k|k−1)]}+D{Kg(k)V(k)}+2cov(first,second)D{[I−Kg(k)H][X(k)−X(k|k−1)]}+D{Kg(k)V(k)}[I−Kg(k)H][X(k)−X(k|k−1)][X(k)−X(k|k−1)]T[I−Kg(k)H]T+Kg(k)V(k)V(k)TKg(k)T[I−Kg(k)H]P(k|k−1)[I−Kg(k)H]T+Kg(k)R(k)Kg(k)T[P(k|k−1)−Kg(k)HP(k|k−1)][I−Kg(k)H]T+Kg(k)R(k)Kg(k)TP(k|k−1)−P(k|k−1)HTKg(k)T−Kg(k)HP(k|k−1)+Kg(k)HP(k|k−1)HTKg(k)T+Kg(k)R(k)Kg(k)TP(k|k−1)−P(k|k−1)THTKg(k)T−Kg(k)HP(k|k−1)+Kg(k)[HP(k|k−1)HT+R(k)]Kg(k)TP(k|k−1)−[Kg(k)HP(k|k−1)]T−Kg(k)HP(k|k−1)+Kg(k)[HP(k|k−1)HT+R(k)]Kg(k)T
P(k|k)=D[X(k)−X(k|k)]=D{X(k)−X(k|k−1)−Kg(k)[Z(k)−HX(k|k−1)]}=D{X(k)−X(k|k−1)−Kg(k)Z(k)+Kg(k)HX(k|k−1)}=D{X(k)−X(k|k−1)−Kg(k)[HX(k)+V(k)]+Kg(k)HX(k|k−1)}=D{X(k)−X(k|k−1)−Kg(k)HX(k)−Kg(k)V(k)+Kg(k)HX(k|k−1)}=D{[I−Kg(k)H][X(k)−X(k|k−1)]−Kg(k)V(k)}=D{[I−Kg(k)H][X(k)−X(k|k−1)]}+D{Kg(k)V(k)}+2cov(first,second)=D{[I−Kg(k)H][X(k)−X(k|k−1)]}+D{Kg(k)V(k)}=[I−Kg(k)H][X(k)−X(k|k−1)][X(k)−X(k|k−1)]T[I−Kg(k)H]T+Kg(k)V(k)V(k)TKg(k)T=[I−Kg(k)H]P(k|k−1)[I−Kg(k)H]T+Kg(k)R(k)Kg(k)T=[P(k|k−1)−Kg(k)HP(k|k−1)][I−Kg(k)H]T+Kg(k)R(k)Kg(k)T=P(k|k−1)−P(k|k−1)HTKg(k)T−Kg(k)HP(k|k−1)+Kg(k)HP(k|k−1)HTKg(k)T+Kg(k)R(k)Kg(k)T=P(k|k−1)−P(k|k−1)THTKg(k)T−Kg(k)HP(k|k−1)+Kg(k)[HP(k|k−1)HT+R(k)]Kg(k)T=P(k|k−1)−[Kg(k)HP(k|k−1)]T−Kg(k)HP(k|k−1)+Kg(k)[HP(k|k−1)HT+R(k)]Kg(k)T
        其中 
R(k)=V(k)V(k)T
R(k)=V(k)V(k)T
        卡尔曼滤波本质上是一个不断减小（最优估计和真实值之间的）误差的方差的过程。P(k|k)P(k|k) 是误差的方差矩阵，其对角线元素就是各个状态变量的误差的方差，最小化各个方差就等价于最小化方差的和，也即最小化方差矩阵的迹 
∂tr{P(k|k)}∂Kg(k)======∂tr{P(k|k−1)}∂Kg(k)−∂tr{[Kg(k)HP(k|k−1)]T}∂Kg(k)−∂tr{Kg(k)HP(k|k−1)}∂Kg(k)+∂tr{Kg(k)[HP(k|k−1)HT+R(k)]Kg(k)T}∂Kg(k)0−[HP(k|k−1)T]T−[HP(k|k−1)]T+∂tr{Kg(k)[HP(k|k−1)HT+R(k)]Kg(k)T}∂Kg(k)−2P(k|k−1)HT+Kg(k)[HP(k|k−1)HT+R(k)]+Kg(k)[HP(k|k−1)HT+R(k)]T−2P(k|k−1)HT+Kg(k)[HP(k|k−1)HT+R(k)]+Kg(k)[HP(k|k−1)HT]T+Kg(k)R(k)T−2P(k|k−1)HT+Kg(k)[HP(k|k−1)HT+R(k)]+Kg(k)[HP(k|k−1)HT]+Kg(k)R(k)−2P(k|k−1)HT+2Kg(k)[HP(k|k−1)HT+R(k)]
∂tr{P(k|k)}∂Kg(k)=∂tr{P(k|k−1)}∂Kg(k)−∂tr{[Kg(k)HP(k|k−1)]T}∂Kg(k)−∂tr{Kg(k)HP(k|k−1)}∂Kg(k)+∂tr{Kg(k)[HP(k|k−1)HT+R(k)]Kg(k)T}∂Kg(k)=0−[HP(k|k−1)T]T−[HP(k|k−1)]T+∂tr{Kg(k)[HP(k|k−1)HT+R(k)]Kg(k)T}∂Kg(k)=−2P(k|k−1)HT+Kg(k)[HP(k|k−1)HT+R(k)]+Kg(k)[HP(k|k−1)HT+R(k)]T=−2P(k|k−1)HT+Kg(k)[HP(k|k−1)HT+R(k)]+Kg(k)[HP(k|k−1)HT]T+Kg(k)R(k)T=−2P(k|k−1)HT+Kg(k)[HP(k|k−1)HT+R(k)]+Kg(k)[HP(k|k−1)HT]+Kg(k)R(k)=−2P(k|k−1)HT+2Kg(k)[HP(k|k−1)HT+R(k)]
        令 
∂tr{P(k|k)}∂Kg(k)=0
∂tr{P(k|k)}∂Kg(k)=0
        可得 
Kg(k)=P(k|k−1)HT[HP(k|k−1)HT+R(k)]−1
Kg(k)=P(k|k−1)HT[HP(k|k−1)HT+R(k)]−1

6.4 第五个公式

 

这个讲速度和位置的例子：

https://blog.csdn.net/su_bao/article/details/80997077