% The script file pwsemi04 is a program to discuss a random function
% and the estimated corresponding random sequence.

sigma0=sqrt(8/3); eta=1/1; alfa=sigma0*sqrt(2*eta);dt=0.02;
wp=alfa*alfa/2/eta; n=300;
P=[1,1/2,1/4;1/2,1/2,3/8;1/4,3/8,3/8]; P=wp*P;
PT=chol(P); PT=PT';
PTD=[1,0,0; 1/2,1/2,0; 1/4,1/2,1/4]; PTD=sqrt(wp)*PTD; 
AO=PT*randn(3,1); AO=AO';
MA=[AO];

FI=[1,0,0; eta*dt,1,0; eta*dt*eta*dt/2, eta*dt, 1]; 
wf=exp(-eta*dt); FI=wf*FI;
QQ=P-FI*P*FI';

q11=sqrt(QQ(1,1)); q21=QQ(1,2)/q11; q31=QQ(1,3)/q11; q12=0; q13=0;
q22=sqrt(QQ(2,2)-q21*q21);q23=0; q32=(QQ(2,3)-q31*q21)/q22;
q33=sqrt(QQ(3,3)-q31*q31-q32*q32);
Q=[q11,q12,q13;q21,q22,q23;q31,q32,q33];


for r=1:n
	DA=FI*AO'+Q*randn(3,1); DA=DA';
	MA=[MA;DA];
	AO=DA;
end

T=(1:n+1)*dt; T=T'; 
% ZE=zeros(size(T)); A2=MA(:,3); DA2=eta*(MA(:,2)-MA(:,3));
% plot(T,A2,'r',T,ZE,'k')
% pause
% close

% Kalman filter procedure

HA=[0,0,1]; R=0.05*0.05;

XB=MA(1,:); XB=XB'; MAF=[XB'];
XP=FI*XB; MPB=P; 

for r=1:n
   XP=FI*XB;
   MPP=FI*MPB*FI'+Q*Q';
   WH=HA*MPP*HA'+R; WH=1/WH;
   KA=MPP*HA'*WH;
   XF=XP+KA*(MA(r,3)-HA*XP);
   MPF=MPP-KA*HA*MPP;
   MAF=[MAF;XF'];
   XB=XF;
   MPB=MPF;
end

plot(T,MA(:,3),'k',T,MAF(:,3),'r.')
title('Comparison A2 black line, estimate red dot, same I.C.')
pause
close

plot(T,MA(:,2),'k',T,MAF(:,2),'r.')
title('Comparison A1 black line, estimate red dot, same I.C.')
pause
close

plot(T,MA(:,1),'k',T,MAF(:,1),'r.')
title('Comparison A0 black line, estimate red dot, same I.C.')
pause
close


HA=[0,0,1]; R=0.05*0.05;

XB=zeros(3,1);  MAF=[XB'];
XP=FI*XB; MPB=P; 

for r=1:n
   XP=FI*XB;
   MPP=FI*MPB*FI'+Q*Q';
   WH=HA*MPP*HA'+R; WH=1/WH;
   KA=MPP*HA'*WH;
   XF=XP+KA*(MA(r,3)-HA*XP);
   MPF=MPP-KA*HA*MPP;
   MAF=[MAF;XF'];
   XB=XF;
   MPB=MPF;
end

plot(T,MA(:,3),'k',T,MAF(:,3),'r.')
title('Comparison A2 black line, estimate red dot, zero I.C.')
pause
close

plot(T,MA(:,2),'k',T,MAF(:,2),'r.')
title('Comparison A1 black line, estimate red dot, zero I.C.')
pause
close

plot(T,MA(:,1),'k',T,MAF(:,1),'r.')
title('Comparison A0 black line, estimate red dot, zero I.C.')
pause
close



AOB=zeros(3,1); MAB=[AOB'];
for r=1:n
	DAB=FI*AOB+Q*randn(3,1); DAB=DAB'; % ZE=zeros(size(T));
	MAB=[MAB;DAB];
	AOB=DAB';
end

A2B=MAB(:,3); T=(1:n+1)*dt; T=T'; 

% plot(T,A2B,'r',T,ZE,'k')
% pause
% close

% Kalman filter procedure

HA=[0,0,1]; R=0.05*0.05;

XB=MAB(1,:); XB=XB'; MABF=[XB'];
XP=FI*XB; MPB=P; 

for r=1:n
   XP=FI*XB;
   MPP=FI*MPB*FI'+Q*Q';
   WH=HA*MPP*HA'+R; WH=1/WH;
   KA=MPP*HA'*WH;
   XF=XP+KA*(MAB(r,3)-HA*XP);
   MPF=MPP-KA*HA*MPP;
   MABF=[MABF;XF'];
   XB=XF;
   MPB=MPF;
end

plot(T,MAB(:,3),'k',T,MABF(:,3),'r.')
title('Comparison A2 black line, estimate red dot, same zero I.C.')
pause
close

plot(T,MAB(:,2),'k',T,MABF(:,2),'r.')
title('Comparison A1 black line, estimate red dot, same zero I.C.')

pause
close
title('Comparison A0 black line, estimate red dot, same zero I.C.')

plot(T,MAB(:,1),'k',T,MABF(:,1),'r.')
pause
close

   
   
