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);
