ex8_19.m

华东理工大学自动化系《控制系统分析、设计和应用》教材配套程序实例,有关说明： 1. 安装MATLAB软件。安装时

% ex8_19
% Kalman滤波器设计
figure('color','w');a=[0 1; 0 -1]; b=[0 1]'; c=[1 0]; d=0;
[ap,bp,cp,dp]=augstate(a,b,c,d);h=0.05;t=[0:h:10];  
[xorg,yorg]=step(ap,bp,cp,dp,1,t);u=zeros(length(t),2);
u(:,1)=0.1*rand(length(t),1);u(:,2)=1.4142*rand(length(t),1);
m=mean(u(:,1));u(:,1)=u(:,1)-m+1;m=mean(u(:,2));u(:,2)=u(:,2)-m;
ap=a;bp=[b  [0  0]'];cp=c;dp=[d  1]; [G,H]=c2d(ap,bp,h);K=[0 0]';
xold=[0 0]';pold=diag([10000 10000]);xhatold=[u(1,2) 0]';Q=0.01;R=2;gamma=[0 1]';
[pp,gamma]=c2d(a,gamma,h);y=zeros(length(t),5);
for jj=1:length(t),
   uplant=u(jj,:);xnew=G*xold+H*uplant';
   y(jj,1)=(cp*xnew+dp*uplant')'; y(jj,2:3)=xnew'; pstar=G*pold*G' + gamma*Q*gamma'; 
   K=pstar*cp'*inv(cp*pstar*cp' + R);
   xhatnew=(eye(2) - K*cp) * (G*xhatold + H*uplant') + K*y(jj,1);
   pnew=(eye(2) - K*cp) * pstar; y(jj,4:5)=xhatnew';
   xold=xnew;xhatold=xhatnew;pold=pnew;
end;
axes('pos',[0.1,0.56,0.35,.32]);plot(t,[xorg  y(:,:)]);
axes('pos',[0.52,0.56,0.35,.32]);plot(t,[xorg(:,2)  y(:,[1 2 4])]);
axes('pos',[0.1,0.1,0.35,.32]);plot(t,[xorg(:,3) y(:,[3  5])]);
axes('pos',[0.52,0.1,0.35,.32]);plot(t,[xorg(:,[2 3]) ]);
K,pold