kalman2.m

一个matlab实现卡尔曼滤波的程序

function kalman2(Rv,Rw)

L=100;    
Vk=1;                             
Ck=[1,0]; 
I=eye(2);                        
Ak=[0.98 0.009;-0.36 0.801];      
Bk=[-0.019;-0.36];                
Wk=[-0.019;-0.36];                
                     

x=[0;0];
x1=zeros(1,L);
x2=zeros(1,L);
xg=[0;0];
X=zeros(2,L);
u=genu(L);                             %调用产生u函数的子函数genu          
w=sqrt(Rw)*randn(1,L);            
v=sqrt(Rv)*randn(1,L);            
for i=2:L;                        
x=Ak*x+Bk*u(i-1)+Wk*w(i-1);
x1(i)=x(1);
x2(i)=x(2);
end
X(1,:)=x1;  
X(2,:)=x2;   
Yik=Ck*X;                                     
y(1)=v(1);
y=Ck*X+v;                                    
k=1:L;
subplot(3,2,1);
plot(k,Yik,'r',k,y,'b');
legend('Yik','y');                 
grid ON

Rk=Vk*Vk'*Rv;
Qk=Wk*Wk'*Rw;

Pk=[0.01,0;0,0.04];
Xg(:,1)=xg;
p=zeros(2,2*L);
p(:,1)=Pk(:,1);     
p(:,2)=Pk(:,2); 
for i=2:L;
Pp=Ak*Pk*Ak'+Qk;
Hk=Pp*Ck'*inv(Ck*Pp*Ck'+Rk);
xg(:,i)=Ak*xg(:,i-1)+Hk*(y(i)-Ck*Ak*xg(:,i-1))+Bk*u(i-1);
Pk=(I-Hk*Ck)*Pp;   
hk(:,i-1)=Hk;
Xg(:,i)=xg(:,i);
p(:,2*i-1)=Pk(:,1);
p(:,2*i)=Pk(:,2);
end
Yk=Ck*Xg;
subplot(3,2,2);
plot(k,Yik,'r',k,Yk,'b');
axis([1,L,-1,0]);
legend('Yik','Yk');         
grid ON

subplot(3,2,3);
plot(k,Xg(1,:),'r',k,X(1,:),'b');
legend('Xg(1,:)','X(1,:)');                
grid ON
subplot(3,2,4);
plot(k,Xg(2,:),'r',k,X(2,:),'b');
legend('Xg(2,:)','X(2,:)');                 
grid ON
n=1:99;
subplot(3,2,5);
plot(n,hk(1,:),'r',n,p(1,1:2:2*L-2),'b',n,p(1,2:2:2*L-2),'b');
legend('hk(1)','p(1,1)','p(1,2)');                 
grid ON
subplot(3,2,6)
plot(n,hk(2,:),'r',n,p(2,1:2:2*L-2),'b',n,p(2,2:2:2*L-2),'b');
legend('hk(2)','p(2,1)','p(2,2)');                  
grid ON