系统建模与仿真实验-实验7

《系统建模与仿真》实验七

实验目的：用Matlab实现Kalman滤波算法仿真。

实验内容：

end

n=3;

xjian(:,1)=zeros(3,1);

p(:,:)=zeros(3);

for i=1:bushu-1

xxjian(:,i+1)=fai*xjian(:,i);

e(:,i+1)=y(:,i+1)-h*xxjian(:,i+1);

pp(:,n*(i-1)+1:n*(i))=fai*p(:,n*(i-1)+1:n*i)*fai'+gama*q*gama';

kf(:,2*i:2*i+1)=pp(:,n*(i-1)+1:n*i)*h'*inv(h*pp(:,n*(i-1)+1:n*i)*h'+r); xjian(:,i+1)=xxjian(:,i+1)+kf(:,2*i:2*i+1)*e(:,i+1);

p(:,n*i+1:n*(i+1))=[eye(3)-kf(:,2*i:2*i+1)*h]*pp(:,n*(i-1)+1:n*i); end

t=1:bushu-2;

subplot(221);

plot(t,x(1,t),'b',t,xxjian(1,t),'r');

subplot(222);

plot(t,x(2,t),'b',t,xxjian(2,t),'r');

subplot(223);

plot(t,x(3,t),'b',t,xxjian(3,t),'r');