卡尔曼滤波

≯℡__Kan透↙ 提交于 2020-03-23 20:36:30

参考:

https://en.wikipedia.org/wiki/Kalman_filter

原文: https://www.cnblogs.com/alantu2018/p/9224001.html

公式:

首先,我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程(Linear Stochastic Difference equation)来描述:X(k)=A X(k-1)+B U(k)+W(k) 
再加上系统的测量值:Z(k)=H X(k)+V(k) 
上两式子中

X(k)k时刻的系统状态

U(k)k时刻对系统的控制量

AB是系统参数,对于多模型系统,他们为矩阵

Z(k)k时刻的测量值

H是测量系统的参数,对于多测量系统,H为矩阵

W(k)V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise),他们的covariance 分别是QR(这里我们假设他们不随系统状态变化而变化)。

对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出(类似上一节那个温度的例子)。

首先我们要利用系统的过程模型,来预测下一状态的系统。假设现在的系统状态是k,根据系统的模型,可以基于系统的上一状态而预测出现在状态:
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
(1)中,X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量,如果没有控制量,它可以为0

到现在为止,我们的系统结果已经更新了,可是,对应于X(k|k-1)covariance还没更新。我们用P表示covariance
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
(2)中,P(k|k-1)X(k|k-1)对应的covarianceP(k-1|k-1)X(k-1|k-1)对应的covarianceA’表示A的转置矩阵,Q是系统过程的covariance。式子12就是卡尔曼滤波器5个公式当中的前两个,也就是对系统的预测。

现在我们有了现在状态的预测结果,然后我们再收集现在状态的测量值。结合预测值和测量值,我们可以得到现在状态(k)的最优化估算值X(k|k)
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg为卡尔曼增益(Kalman Gain)
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

到现在为止,我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束,我们还要更新k状态下X(k|k)covariance
P(k|k)=I-Kg(k) HP(k|k-1) ……… (5)
其中I 1的矩阵,对于单模型单测量,I=1。当系统进入k+1状态时,P(k|k)就是式子(2)P(k-1|k-1)。这样,算法就可以自回归的运算下去。

卡尔曼滤波器的原理基本描述了,式子12345就是他的5 个基本公式。根据这5个公式,可以很容易的实现计算机的程序。

实现:

NB-IoT 终端的上行RSSI的数据的卡尔曼滤波效果:

 

 

 

1python实现

(2) rtklib中的实现

遗留问题:

如何确定过程噪声的Q和测量噪声R

一篇博文中提出了思考:https://blog.csdn.net/u010596768/article/details/79045367

QR动态变化过程怎么处理?

易学教程内所有资源均来自网络或用户发布的内容,如有违反法律规定的内容欢迎反馈
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!