kalman1.m

关于维纳滤波器、卡尔曼滤波器、自适应滤波器的设计方法

function kalman(L,Ak,Ck,Bk,Wk,Vk,Rw,Rv)
%kalman1(100,0.95,1,0,1,1,0.0975,1) 
w=sqrt(Rw)*randn(1,L);     % w为均值零方差为Rw斯白噪声   
v=sqrt(Rv)*randn(1,L);     % v为均值零方差为Rv高斯白噪声
x0=0;
u=zeros(1,L);
for t=1:1:L;
    if ((0<=mod(t,30))&(mod(t,30)<10))
        u(t)=1;
    end
end
x1=zeros(2,L);
x1(1,1)=[0;0];                               %给x(1)赋初值.
for i=2:L                                 %递推求出x(k).x(k)维数是2*1。
    x(i,i)=Ak*x1(i-1,i-1)+Bk*u(i-1)+Wk*w(i-1);
end
yk=Ck*x+Vk*v;                            
yik=Ck*x;
n=1:L;
subplot(2,2,1);
plot(n,yk,n,yik,'r:');
legend('yk','yik',1)
title('量测值yk,状态值无激励和噪声yik')
Qk=Wk*Wk'*Rw;
Rk=Vk*Vk'*Rv;
V=[0.01 0.04];
P=diag(V);
%p(1)=10^(-12);
P1(1)=Ak*P*Ak'+Qk;
xg(1)=0;
for k=2:L
    P1(k)=Ak*P(k-1)*Ak'+Qk; 
    H(k)=P1(k)*Ck'*inv(Ck*P1(k)*Ck'+Rk);
    I=eye(size(H(k)));
    P(k)=(I-H(k)*Ck)*P1(k);
    xg(k)=Ak*xg(k-1)+H(k)*(yk(k)-Ck*Ak*xg(k-1))+Bk*u(k-1);   
    yg(k)=Ck*xg(k);                                
end
subplot(2,2,2);
plot(n,P(n),n,H(n),'r:')
legend('P(n)','H(n)',4)
title('    均方误差P(n),修正参数H(n)')
subplot(2,2,3);
plot(n,x(n),n,xg(n),'r:')
legend('x(n)','估计xg(n)',1)
title('   状态值x(n),状态估计值xg(n)')
subplot(2,2,4);
plot(n,yik(n),n,yg(n),'r:')
legend('估计yg(n)','yik(n)',1)
title('yik(n),量测值估计yg(n)')
set(gcf,'Color',[1,1,1]);