% Script file pwoprv03 is used to study the measured damped vibrations.
% The column matrix of measured values is denoted by Y, its elements
% by Y(r). We introduce the following column matrices E(r)=exp(-x3*r*dt),
% C(r)=cos(2*pi*x4*r*dt),S(r)=sin(2*pi*x4*r*dt), R(r)=r*dt; We introduce
% the following notations: EC=E.*C, ES=E.*S, REC=R.*EC, RES=R.*REC,
% RREC=R.*REC, RRES=R.*RES. The column matrix of errors is EY=Y-x1*EC-x2*ES,
% the mean square error is a scalar EJ=EY'*EY; The variance of Y is
% V=mean(Y'*Y) and thus the relative error is RE=sqrt(EJ/V).

x1=5; x2=0; x3=0.6; x4=10; 
n1=100; dt=0.001; np=3; %number of periods 
R=(1:np*n1)*dt; R=R'; 
E=exp(-x3*R); C=cos(2*pi*x4*R); S=sin(2*pi*x4*R);
EC=E.*C; ES=E.*S; 
YM=x1*EC+x2*ES; %This is calculated with the given parameters
% and will be used to estimate these values 
plot(R,YM)
title('The given values a=5,b=0, eta=0.6, f=10')
pause
close
VI=(YM'*YM);
%VI=(VI)/np/n1
%SVI=sqrt(VI)

x1=0; x2=0; x3=0; x4=10.5; %assumed initial values

C=cos(2*pi*x4*R); S=sin(2*pi*x4*R);
RC=R.*C; RS=R.*S; 
RRC=R.*RC; RReS=R.*RS;
YA=YM-x1*C-x2*S; EJA=YA'*YA;

YA1=-C; YA2=-S;
YA11=0; YA12=0;
EJA1=2*YA1'*YA;
EJA11=2*YA1'*YA1;
EJA12=2*YA1'*YA2;

YA2=-S;
EJA2=2*YA2'*YA;
EJA21=2*YA2'*YA1;
EJA22=2*YA2'*YA2;

WA=[EJA11,EJA12;EJA21,EJA22];
IWA=inv(WA);
PA=-[EJA1;EJA2];
DXA=IWA*PA;

x1=x1+DXA(1,1); x2=x2+DXA(2,1); x3=x3; x4=x4;
[x1,x2,x3,x4]

C=cos(2*pi*x4*R); S=sin(2*pi*x4*R);
Y=YM-x1*C-x2*S; EJ=Y'*Y; WEY=sqrt(EJ/VI)
if WEY>0.01
   WEY=WEY
else
   close
end  
plot(R,YM,'k',R,Y,'r',R,YM-Y,'r',R,zeros(size(R)),'k')
title('a and b estimated, initial values a=0, b=0, f=10.5')
pause
close

sprintf('%s','The iterations for three parameters a,b,f')
  
for s=1:10

C=cos(2*pi*x4*R); S=sin(2*pi*x4*R);
RC=R.*C; RS=R.*S; 
RRC=R.*RC; RRS=R.*RS;
Y=YM-x1*C-x2*S; EJ=Y'*Y;WEY=sqrt(EJ/VI);
ITERATIOS=[s,x1,x2,x3,x4,WEY]
Y1=-C; Y2=-S; Y4=x1*2*pi*RS-x2*2*pi*RC;

Y11=0; Y12=0;  Y14=2*pi*RS;
EJ1=2*Y1'*Y;
EJ11=2*Y1'*Y1;
EJ12=2*Y1'*Y2;
EJ14=2*Y14'*Y+2*Y1'*Y4;

Y24=-2*pi*RC;
EJ2=2*Y2'*Y;
EJ21=2*Y2'*Y1;
EJ22=2*Y2'*Y2;
EJ24=2*Y24'*Y+2*Y2'*Y4;

Y41=2*pi*RS; Y42=-2*pi*RC; Y44=x1*4*pi*pi*RRC+x2*4*pi*RRS;
EJ4=2*Y4'*Y;
EJ41=2*Y41'*Y+2*Y4'*Y1;
EJ42=2*Y42'*Y+2*Y4'*Y2;
EJ44=2*Y44'*Y+2*Y4'*Y4;

W3=[EJ11,EJ12,EJ14;EJ21,EJ22,EJ24;EJ41,EJ42,EJ44];
P3=-[EJ1;EJ2;EJ4];
IW3=inv(W3);
DX3=IW3*P3;
x1=x1+DX3(1,1); x2=x2+DX3(2,1); x3=x3; x4=x4+DX3(3,1);

end

Y=YM-x1*C-x2*S;
plot(R,YM,'k',R,Y,'r',R,YM-Y,'r',R,zeros(size(R)),'k')
title('a,b,f estimated, no damping') 
pause
close


sprintf('%s','The iterations for four parametres a, b, eta, f')
   
for s=1:10

E=exp(-x3*R); C=cos(2*pi*x4*R); S=sin(2*pi*x4*R);
EC=E.*C; ES=E.*S; REC=R.*EC; RES=R.*ES; 
RREC=R.*REC; RRES=R.*RES;
EY=YM-x1*EC-x2*ES; EJ=EY'*EY;WEY=sqrt(EJ/VI);
ITERATIOS=[s,x1,x2,x3,x4,WEY]
EY1=-EC; EY2=-ES; EY3=x1*REC+x2*RES;EY4=x1*2*pi*RES-x2*2*pi*REC;

EY11=0; EY12=0; EY13=REC; EY14=2*pi*RES;
EJ1=2*EY1'*EY;
EJ11=2*EY1'*EY1;
EJ12=2*EY1'*EY2;
EJ13=2*EY13'*EY+2*EY1'*EY3;  %+EY3'*EY1+EY'*EY13
EJ14=2*EY14'*EY+2*EY1'*EY4;

EY23=RES; EY24=-2*pi*REC;
EJ2=2*EY2'*EY;
EJ21=2*EY2'*EY1;
EJ22=2*EY2'*EY2;
EJ23=2*EY23'*EY+2*EY2'*EY3; 
EJ24=2*EY24'*EY+2*EY2'*EY4;

EY31=REC; EY32=RES; EY33=-x1*RREC-x2*RRES;
EY34=-x1*2*pi*RRES+x2*2*pi*RRES;
EJ3=2*EY3'*EY;
EJ31=2*EY31'*EY+2*EY3'*EY1;
EJ32=2*EY32'*EY+2*EY3'*EY2;
EJ33=2*EY33'*EY+2*EY3'*EY3;
EJ34=2*EY34'*EY+2*EY3'*EY4;


EY41=2*pi*RES; EY42=-2*pi*REC; EY43=-x1*2*pi*RRES+x2*2*pi*RREC;
EY44=x1*4*pi*pi*RREC+x2*4*pi*RRES;
EJ4=2*EY4'*EY;
EJ41=2*EY41'*EY+2*EY4'*EY1;
EJ42=2*EY42'*EY+2*EY4'*EY2;
EJ43=2*EY43'*EY+2*EY4'*EY3;
EJ44=2*EY44'*EY+2*EY4'*EY4;

W1=[EJ11,EJ12,EJ13,EJ14;EJ21,EJ22,EJ23,EJ24];
W2=[EJ31,EJ32,EJ33,EJ34;EJ41,EJ42,EJ43,EJ44];
W4=[W1;W2];
P4=-[EJ1;EJ2;EJ3;EJ4];
IW4=inv(W4);
DX4=IW4*P4;
x1=x1+DX4(1,1); x2=x2+DX4(2,1); x3=x3+DX4(3,1); x4=x4+DX4(4,1);

end
Y=YM-x1*EC-x2*ES;
plot(R,YM,'k',R,Y,'r',R,YM-Y,'r',R,zeros(size(R)),'k')
title('a,b,f,eta estimated')
pause
close

