kalmanmodel.m

一个很好的卡尔曼滤波程序呵呵

function kalmanmodel
A=[1.1269 -0.4940 0.1129;1 0 0;0 1 0];
B=[-0.3832;0.5919;0.5191];
C=[1 0 0];
Plant = ss(A,[B B],C,0,-1,'inputname',{'u' 'w'},'outputname','y')
Q=0.6;R=3;
[kalmf,l,p,m]=kalman(Plant,Q,R);
kalmf=kalmf(1,:);
kalmf
a=A;
b=[B B 0*B];
c=[C;C];
d=[0 0 0;0 0 1];
P=ss(a,b,c,d,-1,'inputname',{'u' 'w' 'v'},'outputname',{'y' 'yv'});
sys=parallel(P,kalmf,1,1,[],[]);

SimModel=feedback(sys,1,4,2,1);
simmodeldisplay=2
SimModel=SimModel([1 3],[1 2 3])
simmodeldisplay=3
t=[0:100]';
u=sin(t/5);
n=length(t);
randn('seed',0);
w=sqrt(Q)*randn(n,1)
wdisplay=66
v=sqrt(R)*randn(n,1);
[out,x]=lsim(SimModel,[w,v,u])
outxdisplay=77
y=out(:,1)
ydisplay=88
ye=out(:,2)
yedisplay=99
yv=y+v;
subplot(211),plot(t,y,'--',t,ye,'o');
hold on ;
plot(t,u,'r');
xlabel('No. of samples'),ylabel('Output')
title('Kalman filter response');
subplot(212),plot(t,y-yv,'-.',t,y-ye,'o');
xlabel('No. of samples'), ylabel('Error')