当看完邓老师的《信息融合滤波理论及其应用》之后,你就会发现信息融合的研究方向就是如下两个方面:1.系统状态的滤波、平滑、估计;2.系统噪声和观测噪声的估计。系统状态的滤波就是根据1,2,.....,t时刻的观测值估计当前t时刻的状态值;系统状态的平滑就是利用1,2,.....t,......t+k时刻的观测值估计当前t时刻的状态值;系统状态的预测就是利用1,2,........t时刻的观测值预测第t+1时刻的状态值。系统噪声的估计就是利用前t+N时刻的观测值,估计系统噪声w(t)的大小;观测噪声的估计就是利用前t+N时刻的观测值,估计观测噪声v(t)的大小,,示意图如下。
我的上一篇博客卡尔曼滤波的推导中,卡尔曼滤波的五个公式中就包含了状态的预测和估计过程,这篇博客主要讲系统状态的平滑过程。
卡尔曼滤波的平滑主要有三种平滑方式:固定点平滑、固定滞后平滑、固定区间平滑。这三种平滑方式的介绍我觉得严恭敏老师讲义上说的很清楚,所以直接贴上讲义上的描述:
对于具体卡尔曼平滑器的算法描述,我就不准备采用严老师讲义的算法了,因为严老师的算法虽然好理解,但是并不实用,存在很多矩阵求逆过程,计算过程过于复杂,所以这里介绍一下邓老师书中的平滑器算法。
定理1:对于如下系统:
(1)
(2)
最优递推固定点Kalman平滑器为:
(3)
平滑误差有:
(4)
其中:
(5)
为t+N时刻的新息,不懂的看上一篇博客卡尔曼滤波推导,里面有具体介绍。
证明过程:
由递推射影定理可知:
(6)
其中:
(7)
因为有:
(8)
(9)
根据式9的一步误差递推式子,可以求出与
之间的关系表达式如下:
(10)
式9中,,Ai,Bi分别为系数矩阵。
(11)
因为存在:
当
时
当
时
当
时
当
时
由式10和式11可得:
(12)
则有:
(13)
因为有:
(14)
(15)
定理得证。
定理2:最优固定滞后Kalman平滑器如下:
(16)
(17)
定理2可以通过定理1递推证明。
定理3:固定区间Kalman平滑器如下:
(18)
其中:
(19)
其中有.
方差阵为:
(20)
其中有:
(21)
其中有:
证明过程:
由定理2可知
当N=N-t时,带入上式有:
(22)
将K(t/t+i)的表达式带入上式可得:
(23)
则有:
(24)
(25)
则有:
对于方差:
(26)
(27)
由此有:
(28)
(29)
由此定理三得证。
综上所述,卡尔曼平滑器介绍完毕(这一篇文章用到了很多我上一篇讲到的知识点,有不懂的可以看看的上一篇博客卡尔曼滤波的推导)。
算法仿真:
考虑随机系统:
其中Qw=0.4,R=10,仿真效果图如下:
matlab仿真代码如下:
clc;
clear;
N=500;
A=[1,0;0.2,0.6];
G=[1;2];
H=[1,1];
Q=0.4;
R=10;
RandNum=normrnd(0,sqrt(Q),1,N);
RandNum1=normrnd(0,sqrt(R),1,N);
%%获得测量数据
x(:,1)=[0;0];
y(1)=0;
for i=2:N
x(:,i)=A*x(:,i-1)+G*RandNum(i);
y(i)=H*x(:,i)+RandNum1(i);
end
Pk(:,:,1)=[Q,0;0,R];
X(:,1)=[0;0];
I=eye(2);
K(:,1)=[0;0];
Xk(:,1)=[0;0];
Pkk(:,:,1)=[0,0;0,0];
for i=2:N
Xk(:,i)=A*X(:,i-1);
Pkk(:,:,i)=A*Pk(:,:,i-1)*A'+G*Q*G';
K(:,i)=Pkk(:,:,i)*H'/(H*Pkk(:,:,i)*H'+R);
X(:,i)=Xk(:,i)+K(:,i)*(y(i)-H*Xk(:,i));
Pk(:,:,i)=(I-K(:,i)*H)*Pkk(:,:,i);
end
N1=3;
for i=2:N-N1-1
Ks=K(:,i)*(y(i)-H*Xk(:,i));
for j=2:N1+1
KN=eye(2);
for k=1:j-1
KN=KN*(I-K(:,i+k-1)*H)'*A';
end
KN=Pkk(i)*KN*H'/(H*Pkk(i+j-1)*H'+R);
Ks=Ks+KN*(y(i+j-1)-H*Xk(:,i+j-1));
end
X2(:,i)=Xk(:,i)+Ks;
end
X2(:,N-2)=X(:,N-2);
X2(:,N-1)=X(:,N-1);
X2(:,N)=X(:,N);
t=1:N;
figure(1);
subplot(2,2,1);
plot(t,x(1,:),'r',t,X(1,:),'k');
subplot(2,2,2);
plot(t,x(2,:),'r',t,X(2,:),'k');
subplot(2,2,3);
plot(t,X(1,:),'r',t,X2(1,:),'k');
subplot(2,2,4);
plot(t,X(2,:),'r',t,X2(2,:),'k');
figure(2);
plot(t,X(1,:)-x(1,:),'r',t,X2(1,:)-x(1,:),'k');
figure(3);
plot(t,X(2,:)-x(2,:),'r',t,X2(2,:)-x(2,:),'k');