kalman.m

KALMAN滤波源程序

clear all
A= [1 1 0 0;0 1 0 0;0 0 1 1;0 0 0 1];
B=[0.5 0; 1 0;0 0.5; 0 1];
C=[1 0 0 0;0 0 1 0];
q=1000;
Q=q*eye(2);
I=eye(4);
R=[1 0;0 1];
P(:,:,1)=[1 1 0 0;1 2 0 0;0 0 1 1;0 0 1 2];
W=randn(2,50)*q^0.5;
V=randn(2,50); 
X(:,1)=[5 1 7 2]';  %X(:,1)表示矩阵X的第一列
for i=1:50
    X(:,i+1)=A*X(:,i)+B*W(:,i);	
    Y(:,i)=C*X(:,i)+V(:,i);         %仿真数据产生
end
Xg(:,1)=[5 1 7 2]';
P(:,:,1)=[1 1 0 0;1 2 0 0;0 0 1 1;0 0 1 2];
for k=1:49
    P1(:,:,k+1)=A*P(:,:,k)*A'+B*Q*B';
    H(:,:,k+1)=P1(:,:,k+1)*C'*inv(C*P1(:,:,k+1)*C'+R);
    P(:,:,k+1)=(I-H(:,:,k+1)*C)*P1(:,:,k+1);
    Xg(:,k+1)=A*Xg(:,k)+H(:,:,k+1)*(Y(:,k+1)-C*A*Xg(:,k));
end
t=1:50;
u=1:51;
figure(1);
plot(t,Xg(1,:),'b',u,X(1,:),'r');
title('???¨?á??');
figure(2);
plot(t,Xg(2,:),'b',u,X(2,:),'r');
title('???¨?á??');
figure(3);
plot(t,Xg(3,:),'b',u,X(3,:),'r');
title('???¨?á??');
figure(4);
plot(t,Xg(4,:),'b',u,X(4,:),'r');
title('???¨?á??');