kalmanfilter(1).m

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%            输入各种数据                 %
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
A=input('请输入A=');
B=input('请输入B=');
C=input('请输入C=');
Rw=input('请输入Rw=');
Rv=input('请输入Rv=');
W=input('请输入W=');
V=input('请输入V=');
Po=input('请输入Po=');
N=input('请输入维数N=');
L=input('请输入样本个数L=');
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%              产生信号                  %
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
xk=zeros(N,L);
randn('state',rand);
w=sqrt(Rw)*randn(N,L);
u=randn(N,L-1);
xk(1)=sqrt(Po)*randn;
for j=2:L
  xk(j)=A*xk(j-1)+W*w(j-1)+B*u(j-1);
end
randn('state',rand);
v=sqrt(Rv)*randn(N,L);
yk=C*xk+V*v;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%          进行卡尔曼滤波                 %
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Xk=zeros(N,L);
Hk=zeros(N,L);
Pk=zeros(N,L);
Pk_1=zeros(N,L);
Pk(1)=Po;
Xk(1)=mean(xk);
for i=2:L;
    Pk_1(i)=(A^2)*Pk(i-1)+W^2*Rw;
    Hk(i)=Pk_1(i)*C*inv(C^2*Pk_1(i)+V^2*Rv);
    Pk(i)=(1-Hk(i)*C)*Pk_1(i);
    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=1:L;
plot(n,Yik,'--r',n,yk,'b');
title('Yik＝C*xk数据和观察数据yk的曲线比较');
xlabel('n');
ylabel('幅度');
legend('Yik','yk',2);
grid
subplot(2,1,2);
n=1:L;
plot(n,xk,'--r',n,Xk,'b');
title('原信号xk和估计信号Xk的曲线比较');
xlabel('n');
ylabel('幅度');
legend('原信号xk','估计信号Xk',2);
grid
figure(2);
subplot(2,1,1);
n=1:L;
plot(n,Pk,'--r',n,Hk,'b');
title('Pk和Hk的曲线比较');
xlabel('n');
ylabel('幅度');
legend('Pk','Hk',2);
grid
subplot(2,1,2);
n=1:L;
plot(n,Yik,'--r',n,Yk,'b');
title('Yik和Yk');
xlabel('n');
ylabel('幅度');
legend('Yik','Yk',2);
grid
%
%%%%%%%%%%%%%%%% the end %%%%%%%%%%%%%%%%%%%%
%
%