kalmanfilter.m

典型的二维kalman滤波原理程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%                  输入各种数据                   %
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rv=input('请输入Rv的值Rv=');
Rw=input('请输入Rw的值Rw=');
L=input('请输入序列长度L=');
A=[0.98,0.009;-0.36,0.801];
B=[-0.019;-0.36];
C=[1,0];
W=[-0.019;-0.36];
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%                  产生信号                   %  
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%
u=zeros(1,L);
for i=1:L
  if(mod(L,30)<11)
    u(i)=1;
  end
end  
xk=zeros(2,L);
randn('state',rand);
w=sqrt(Rw)*randn(1,L);
for j=2:L
  xk(:,j)=A*xk(:,j-1)+B*u(j-1)+W*w(j-1);
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%            卡尔曼过滤                       %
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
randn('state',rand);
v=sqrt(Rv)*randn(1,L);
yk=zeros(1,L);
j=1:L;
yk(j)=C*xk(:,j)+v(j);
Hk=zeros(2,L);
Xk=zeros(2,L);
Pk=zeros(2,2*L);
Pk(:,[1,2])=[0.01,0;0,0.04];
Pk_1=zeros(2,2*L);
for i=2:L
Pk_1(:,[i+1,i+2])=A*Pk(:,[i-1,i])+W*W'*Rw;
Hk(:,i)=Pk_1(:,[i+1,i+2])*C'*inv(C*Pk_1(:,[i+1,i+2])*C'+Rv);
Pk(:,[i,i+1])=(eye(2)-Hk(:,i)*C)*Pk_1(:,[i+1,i+2]);
Xk(:,i)=A*Xk(:,i-1)+Hk(:,i)*(yk(i)-C*A*Xk(:,i-1))+B*u(i-1);
end
Yik=C*xk;
Yk=C*Xk;
figure(1);
subplot(2,1,1);
n=0:L-1;
plot(n,xk(1,(n+1)),'b',n,Xk(1,(n+1)),'--r');
title('原信号xk和估计信号Xk的第一分量');
h=legend('原信号xk的第一分量','估计信号Xk的第一分量',1);
xlabel('n');
ylabel('幅度');
grid
subplot(2,1,2);
plot(n,xk(2,(n+1)),'b',n,Xk(2,(n+1)),'--r');
title('原信号xk和估计信号Xk的第二分量');
h=legend('原信号xk的第二分量','估计信号Xk的第二分量',4);
xlabel('n');
ylabel('幅度');
grid
figure(2);
subplot(2,1,1);
plot(n,xk(1,n+1)-Xk(1,n+1),'b',n,xk(2,n+1)-Xk(2,n+1),'--r');
title('原信号xk和估计信号Xk之差(xk-Xk)的第一分量和第二分量');
h=legend('xk-Xk的第一分量','xk-Xk的第二分量',1);
xlabel('n');
ylabel('幅度');
grid
subplot(2,1,2);
plot(n,Hk(1,n+1),'b',n,Hk(2,n+1),'--r');
title('Hk的第一分量和第二分量');
h=legend('Hk的第一分量','Hk的第二分量',1);
xlabel('n');
ylabel('幅度');
grid
figure(3);
subplot(2,1,1);
plot(n,Yk(n+1),'b',n,Yik(n+1),'--r');
title('Yk和Yik');
h=legend('Yk(n+1)','Yik(n+1)',1);
xlabel('n');
ylabel('幅度');
grid
subplot(2,1,2);
plot(n,Yik,'b',n,Yk,'--r');
title('Yik和Yk');
h=legend('Yik','Yk',1);
xlabel('n');
ylabel('幅度');
grid
%
%%%%%%%%%%%%%%% the end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%