gw2.m

这是黑龙江大学自控理论专业同学在最优估计课程当中用matlab编写的gw算法!

clc;clear;
D1=[0.2,0.9;0.6,-0.2];
D2=[-0.35,-0.45;0.42,-0.09]
Qe=eye(2);
R(1:2,1:2)=Qe+D1*Qe*D1'+D2*Qe*D2';
R(:,3:4)=D1*Qe+D2*Qe*D1';
R(:,5:6)=D2*Qe;
for i=4:100
    R(:,2*i-1:2*i)=zeros(2,2);
end
Rre(1:2,1:2)=R(:,1:2);
for t=1:99
    Rre(2*t+1:2*t+2,1:2)=R(:,2*t+1:2*t+2);
    for i=t-1:-1:0
        sum=zeros(2,2);
        for s=i+1:min(2,t)
            sum=sum+Rre(2*t+1:2*t+2,2*(t-s)+1:2*(t-s)+2)...
                *inv(Rre(2*(t-s)+1:2*(t-s)+2,2*(t-s)+1:2*(t-s)+2))*Rre(2*(t-i)+1:2*(t-i)+2,2*(t-s)+1:2*(t-s)+2)';
        end 
        Rre(2*t+1:2*t+2,2*(t-i)+1:2*(t-i)+2)=R(:,2*i+1:2*i+2)-sum;
    end
end
for i=1:100
    Qe1(:,2*i-1:2*i)=Rre(2*i-1:2*i,2*i-1:2*i);
end
for j=1:2
  for t=1+j:100
     d(2*j-1:2*j,2*t-1:2*t)=Rre(2*t-1:2*t,2*(t-j-1)+1:2*(t-j))*inv(Qe1(:,2*t-1:2*t));
 end
end
t=1:100;
figure(1);
subplot(221);
plot(t,d(1,2*t-1),'k-');
title('d1');
line([1,100],[0.2,0.2]);

subplot(222);
plot(t,d(1,2*t),'k-');
line([1,100],[0.9,0.9]);

subplot(223);
plot(t,d(2,2*t-1),'k-');
line([1,100],[0.6,0.6]);
subplot(224);
plot(t,d(2,2*t),'k-');
line([1,100],[-0.2,-0.2])

figure(2);
subplot(221);
plot(t,d(3,2*t-1),'k-');
title('d2')
line([1,100],[-0.35,-0.35]);

subplot(222);
plot(t,d(3,2*t),'k-');
line([1,100],[-0.45,-0.45]);

subplot(223);
plot(t,d(4,2*t-1),'k-');
line([1,100],[0.42,0.42]);
subplot(224);
plot(t,d(4,2*t),'k-');
line([1,100],[-0.09,-0.09])

figure(3);
subplot(221);
plot(t,Qe1(1,2*t-1),'k-');
title('Qe')
line([1,100],[1,1]);
subplot(222);
plot(t,Qe1(1,2*t),'k-');
line([1,100],[0,0]);
subplot(223);
plot(t,Qe1(2,2*t-1),'k-');
line([1,100],[0,0]);
subplot(224);
plot(t,Qe1(2,2*t),'k-');
line([1,100],[1,1]);