卡尔曼滤波递归算法

卡尔曼滤波新奇的地方在于其递归算法,它是以最小均方差为准则的最佳线性估计,核心在于采用系统的理论动态模型进行状态值预估,并利用量测信息进行校正。

上一篇的介绍中我们已经知道,标准卡尔曼滤波的适用条件是线性系统,再次描述一下:

X(k)=A X(k-1)+B U(k)+W(k)

Z(k)=H X(k)+V(k)

从离散控制过程系统的角度看,以上两式中,X(k)是k时刻的系统状态,U(k)是k时刻对系统的控制量;A和B是系统参数,对于多模型系统,它们为矩阵。Z(k)是k时刻的测量值,H 是测量系统的参数,对于多测量系统,H为矩阵;W(k)和V(k)分别表示过程和测量的噪声,假设为高斯白噪声(White Gaussian Noise),它们的covariance (协方差)常用Q、R表示(为简单常假设其不随系统状态变化而变化)。

对于满足上面条件(线性随机微分系统,过程和测量都是高斯白噪声)的问题,卡尔曼滤波器是最优的信息处理器,下面用它们结合其covariances 来估算系统的最优输出。

首先要利用系统的过程模型,来预测系统的下一个状态,假设现在的系统状态是k,根据系统的模型,可知现在的状态为:

1X(kk-1)=A X(k-1k-1)+B U(k)

式1中,X(kk-1)是利用上一状态预测的结果,U(k)为现在状态的控制量,如果没有控制量,它可以为0。系统结果已经更新了,可是对应于X(kk-1)的协方差还没更新,用P表示协方差:

2P(kk-1)=A P(k-1k-1) A’+Q

式2中,P(kk-1)是X(kk-1)对应的协方差,P(k-1k-1)是X(k-1k-1)对应的协方差,A’表示A的转置矩阵,Q是系统过程的协方差。式子1,2就是卡尔曼滤波器5个公式当中的前两个,也就是对系统的预测。

至此有了「现在状态」的预测结果,然后需要再收集此刻的测量值,结合预测值和测量值,就可以得到现在状态(k)的最优化估算值X(kk):

3X(kk)= X(kk-1)+Kg(k) (Z(k)-H X(kk-1))

其中Kg为卡尔曼增益(Kalman Gain):

4Kg(k)= P(kk-1) H’/(H P(kk-1) H’ + R)

到现在为止,已经得到了k状态下最优的估算值X(kk)。但是为了要使卡尔曼滤波器不断的运行下去直到系统过程结束,还要更新k状态下X(kk)的协方差:

5P(kk)=(I-Kg(k) H) P(kk-1)

其中I为1的矩阵,对于单模型单测量,I=1。当系统进入k+1状态时,P(kk)就是式子2的P(k-1k-1),这样,算法就可以自回归的运算下去。根据这5个公式,可以很容易的实现计算机的程序。

油位监控项目回顾

之前的油位监控项目中,我假设油位是线性减少的,每次加完油是一次新的周期(加油很容易判断,油位有明显的上升),利用加油后油位传感器的前500个数据点进行线性回归,得到「系统的状态方程模型」,而油位传感器本身的油位数据构成「系统的量测方程」。假设线性回归方程和量测方程中噪声成分为白噪声且符号高斯分布,白噪声指在整个频率上都有相同强度频率特性的噪声,实际应用中将频率设为常值、带宽大大超过系统带宽的噪声称为「白噪声」,白噪声意味着噪声值和时间不相关,用高斯白噪声来模拟,可以大大简化模型。

经线性回归得到系统的状态方程(假如是Y)后,对每一个油位上报时刻,有两个关于油位的油位数据:根据Y的预测值和油位传感器的测量值,现在要用这两个值结合它们各自的噪声来估算出油位的真实值。

假如我要估算k时刻的实际油位值。首先要根据k-1时刻的油位值,来预测k时刻的油位值,因为相信油位是方程Y线性下降的,所以k时刻的预测值是Y(k),假设是0.9,同时该值的高斯噪声的偏差是0.05(0.05是这样得到的:如果k-1时刻估算出的油位的偏差是0.03,我对自己预测的不确定度是0.04度,它们的平方相加再开方,就是0.05)。然后,我从油位传感器那里得到了k时刻的油位,假设是0.92,同时该值的偏差是0.04。

现在k时刻有两个油位值,分别是0.9和0.92。究竟实际油位是多少呢?相信线性回归函数的预测还是相信油位传感器呢?究竟相信谁多一点,可以用它们的协方差来判断。因为Kg^2=0.05^2/(0.05^2+0.04^2),所以Kg=0.61,这样估算出k时刻的实际油位值是:0.9+0.61*(0.92-0.9)=0.912,可以看出,因为油位传感器的协方差比较小(比较相信油位传感器),所以估算出的最优油位值偏向传感器的值。

现在已经得到k时刻的最优油位值了,下一步就是要进入k+1时刻,进行新的最优估算。到现在为止,好像还没看到什么递归的东西出现。在进入k+1时刻之前,还要算出k时刻那个最优值(0.912)的偏差,算法如下:((1-Kg)*5^2)^0.5=0.031。这里的0.05就是上面的k时刻预测的那个0.9油位值的偏差,得出的0.031就是进入k+1时刻以后k时刻估算出的最优油位值的偏差(对应于上面的0.03)。

就是这样,卡尔曼滤波器就不断的把协方差递归,从而估算出最优的油位值。它运行的很快,而且它只保留了上一时刻的协方差。

  • 发表于:
  • 原文链接:http://kuaibao.qq.com/s/20180106G09W1J00?refer=cp_1026

扫码关注云+社区