汽车位置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