ukfvsekf.m

扩展卡尔曼滤波与无迹卡尔曼滤波的跟踪滤波性能的比较

clear all;
close all;

Dx1(1)=0.2; %initial state x1
Dx2(1)=0.5;  %initial state x2

Du_1=normrnd(0,1,[1 50]); %input1
Du_2=normrnd(0,1,[1 50]);  %input2

Pn1=normrnd(0,1,[1 50]); %process noise for state1
Pn2=normrnd(0,1,[1 50]);%process noise for state2

%covariance of Pn1 and Pn2
%  105.0170   -2.9921
%   -2.9921  393.0418

Mn=normrnd(0,0.5,[1 50]); %measurement noise

for i=2:50
    S1=0.1*Dx1(i-1)-0.25*Dx2(i-1)+0.25*Du_1(i-1)-0.15*Du_2(i-1);
    S2=-0.25*Dx1(i-1)+0.1*Dx2(i-1)-0.15*Du_1(i-1)+0.1*Du_2(i-1);
    Dx1(i)=S1+Pn1(i-1);
    Dx2(i)=S2+Pn2(i-1);
    Dy(i)=0.5*Dx1(i)+0.5*Dx2(i)+Mn(i);
end

figure(1)
subplot(3,1,1),plot(Du_1,'r'),hold on,plot(Du_2,'b');
title('Input Profile');
legend('Input variable 1 (u1)','Input variable 2 (u2)');
subplot(3,1,2),plot(Dx1,'r'),hold on,plot(Dx2,'b');
title('State Profile');
legend('State 1 (x1)','State 2 (x2)');
subplot(3,1,3),plot(Dy);
title('Output');
legend('output (y)');

%**************************UKF******************
AA=[0.1 -0.25;-0.25 0.1];
BB=[0.25 -0.15;-0.15 0.1];
CC=[0.5 0.5];

L=2;
a=0.8;
b=0;
k=3-L;
% k=500;
nd=a^2*(L+k)-L;

W0m=nd/(L+nd);
% W0c=W0m;
W0c=nd/((L+nd)+(1-a^2+b));
% W0m=W0c;

Wim=1/(2*(L+nd));
Wic=Wim;

X1_0=0.2;
X2_0=0.5;
V1_0=0;
V2_0=0;
N_0=0;

X0=[X1_0;X2_0];
xh=X0;
P0=8*eye(2);
Pxh=P0;

Qh=[10 0;0 20];
Rh=0.2;
lambda=0.1;

for lin=2:50

    Pxh_chol=sqrt(L+nd)*chol(Pxh);
    DPH=[xh,xh];
    XH=[xh,DPH+Pxh_chol,DPH-Pxh_chol];
    StateUKFb{lin}=XH;

    %**************time update
    xhh_1=[0;0];
    Xihh_1=[0;0];
    for i=1:5
        if i==1
        hd=XH(:,i);
        Xihh_1(:,i)=AA*hd+BB*[Du_1(i);Du_2(i)];
        xhh_1=W0m*Xihh_1(:,i)+xhh_1;
        else
        hd=XH(:,i);
        Xihh_1(:,i)=AA*hd+BB*[Du_1(i);Du_2(i)];
        xhh_1=Wim*Xihh_1(:,i)+xhh_1;
        end
    end

    Pxhh_1=[0 0;0 0];
    for i1=1:5
        if i1==1
        Pxhh_1=W0c*(Xihh_1(:,i1)-xhh_1)*(Xihh_1(:,i1)-xhh_1)'+Pxhh_1+Qh;
        else
        Pxhh_1=Wic*(Xihh_1(:,i1)-xhh_1)*(Xihh_1(:,i1)-xhh_1)'+Pxhh_1+Qh;
        end
    end


    for i2=1:5
        Yihh_1(i2)=CC*Xihh_1(:,i2);
    end

    yhh_1=0;
    for i3=1:5
        if i3==0
            yhh_1=W0m*Yihh_1(i3)+yhh_1;
        else
            yhh_1=Wim*Yihh_1(i3)+yhh_1;
        end
    end

    %*****************Measurement update
    Pyh=0;
    for i4=1:5
        if i4==1
            Pyh=W0c*(Yihh_1(i4)-yhh_1)*(Yihh_1(i4)-yhh_1)'+Rh;
        else
            Pyh=Wic*(Yihh_1(i4)-yhh_1)*(Yihh_1(i4)-yhh_1)'+Rh;
        end
    end

    Pxyh=[0;0];
    for i5=1:5
        if i5==1
            Pxyh=W0c*(Xihh_1(:,i5)-xhh_1)*(Yihh_1(i5)-yhh_1)';
        else
            Pxyh=Wic*(Xihh_1(:,i5)-xhh_1)*(Yihh_1(i5)-yhh_1)';
        end
    end

    Gh=Pxyh/Pyh;
    xh=xhh_1+Gh*(Dy(lin)-yhh_1);

    StateUKF(:,lin)=xh;

    Pxh=Pxhh_1-Gh*Pyh*Gh';
%     Pxh=Pxhh_1-Pxyh*Pxyh'/Pyh;

%     % Re_ukf adaptive?
%     Rh=(1-lambda)*Rh + lambda*(Dy(lin)- yhh_1)^2;
%     % Rr_ukf adaptive?
%     Qh=(1-lambda)*Qh + lambda*Gh*(Dy(lin)- yhh_1)^2*Gh';

    PreUKF_y(lin)=CC*xh;
end

% figure(1)
% subplot(3,1,1),plot(Dx1,'b'),hold on,plot(Du_1,'r'),axis([0 50 -5 5]);
% subplot(3,1,2),plot(Dx2,'b'),hold on,plot(Du_2,'r'),axis([0 50 -5 5]);
% subplot(3,1,3),plot(Dy,'b'),hold on,plot(PreUKF_y,'r'),axis([0 50 -3 3]);
% figure(2)
% plot3(Du_1,Du_2,Dy);


%************************EKF************************
EX1_0=0.8;
EX2_0=0.9;
EXh1(1)=EX1_0;
EXh2(1)=EX2_0;

EP0=8*eye(2);
EPh=EP0;
EQ=[2 0;0 2];
ER=0.5;

for hen=2:50
%    State estimation propagation
     EXhb(:,hen)=AA*[EXh1(hen-1);EXh2(hen-1)]+BB*[Du_1(hen-1);Du_2(hen-1)];

%    Error covariance propagation
     EPh_1=AA*EPh*AA'+EQ;

%    Kalman gain matrix
     EG=EPh_1*CC'/(CC*EPh_1*CC'+ER);

%    State update
     Eex=EXhb(:,hen)+EG*(Dy(hen)-CC*EXhb(:,hen));
     EXh1(hen)=Eex(1);
     EXh2(hen)=Eex(2);
     StateEKF(:,hen)=Eex;

%    Error covariance update
     EPh=EPh_1-EG*CC*EPh_1;

     PreEKF_y(hen)=CC*Eex;

end



%Create mov
%aviobj = avifile('mymovie.avi','fps',1);
mymov = avifile('test2.avi');

%H handle to figure
H = figure(3);

axis tight;
for fra=2:50
subplot(2,2,1),plot(1:fra,Du_1(1:fra),'r'),hold on,plot(1:fra,Du_2(1:fra),'b'),axis([0 50 -5 5]);
legend('Input 1 (u1)','input 2 (u2)');
xlabel('Time');

subplot(2,2,2),scatter(StateUKFb{1,fra}(1,1),StateUKFb{1,fra}(2,1),'c*'),axis([-15 15 -15 15]);
hold on;
scatter(StateUKFb{1,fra}(1,2),StateUKFb{1,fra}(2,2),'g*'),axis([-15 15 -15 15]);
scatter(StateUKFb{1,fra}(1,3),StateUKFb{1,fra}(2,3),'g*'),axis([-15 15 -15 15]);
scatter(StateUKFb{1,fra}(1,4),StateUKFb{1,fra}(2,4),'g*'),axis([-15 15 -15 15]);
scatter(StateUKFb{1,fra}(1,5),StateUKFb{1,fra}(2,5),'g*'),axis([-15 15 -15 15]);
scatter(StateUKF(1,fra),StateUKF(2,fra),'r*'),axis([-15 15 -15 15]);
scatter(StateEKF(1,fra),StateEKF(2,fra),'b*'),axis([-15 15 -15 15]);
title('state points (SP) evolution')
xlabel('x1');
ylabel('x2');
%legend('SP(k-1)','Sigma SP(k-1)','Sigma SP(k-1)','Sigma SP(k-1)','Sigma SP(k-1)','UKF SP(k)','EKF SP(k)');
hold off;

subplot(2,2,3),plot(1:fra,Dx1(1:fra),'r'),hold on,plot(1:fra,Dx2(1:fra),'b'),axis([0 50 -5 5]);
legend('State 1 (x1)','State 2 (x2)');
xlabel('Time');

subplot(2,2,4),plot(1:fra,Dy(1:fra),'b'),hold on,plot(1:fra,PreUKF_y(1:fra),'r'),plot(1:fra,PreEKF_y(1:fra),'c'),axis([0 50 -3 3]);
% legend('Output','UKF Prediction','EKF Prediction');
xlabel('Time');

%getfram current axis
frame = getframe(H);

%add frame
mymov = addframe(mymov,frame);
end

%close
mymov = close(mymov);

%create avi
% movie2avi(mymov,'c:\test.avi');

figure(2)
subplot(2,1,1),plot(Dy,'b'),hold on,plot(PreUKF_y,'r'),plot(PreEKF_y,'c');
title('performance of UKF and EKF');
legend('Real value','Prediction by UKF','Prediction by EKF');

subplot(2,1,2),scatter(1:50,PreUKF_y-Dy,'r.'),hold on,scatter(1:50,PreEKF_y-Dy,'c.');
title('absolute error');
legend('UKF','EKF');

figure(4)
subplot(3,1,1),plot(StateUKF(1,:),'r'),hold on,plot(StateEKF(1,:),'b');
subplot(3,1,2),plot(StateUKF(2,:),'r'),hold on,plot(StateEKF(2,:),'b');
subplot(3,1,3),plotyy(1:50,Dy,1:50,PreUKF_y);