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

《系统建模与仿真》实验七实验题目：Kalman滤波时数：3学时实验目的：用Matlab实现Kalman滤波算法仿真。实验内容：实验习题1：考虑系统200010.50(1)01()0()0011100()()()010TTxtTxtwtytxtvt其中T0=0.01,()wt，()vt的均值为零，方差分别是Qw=4和2.625003.25vQ的独立高斯白噪声，取1,,200t，求Kalman滤波器ˆ(|)xtt、并分别画出ˆ(|)xtt的三个分量ˆ(|)(1,2,3)kxttk与其真实值()kxt比较的仿真图形clc;clear;bushu=200;q=4;randn('seed',1);w=sqrt(q)*randn(1,bushu);r=[2.6250;03.25];randn('seed',2);v=sqrt(r)*randn(2,bushu);fai=[10.010.5*0.01*0.01;010.01;001];gama=[001]';h=[100;010];x(:,1)=[000]';y(:,1)=h*x(:,1)+v(:,1);fori=2:bushux(:,i)=fai*x(:,i-1)+gama*w(i-1);y(:,i)=h*x(:,i)+v(:,i);endn=3;xjian(:,1)=zeros(3,1);p(:,:)=zeros(3);fori=1:bushu-1xxjian(:,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);endt=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');