clear
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   loading GPS/MTS/ACC data   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%load GPS data NAV2(North),NAV3(South),NAV4(East) are three GPS sites on the top of building.Nav 5 is on the shake table. LEI1(Low) and LEI2(High) are on the cantilevel.
eq1NAV2=load('eq1NAV2.txt');
eq1NAV3=load('eq1NAV3.txt');
eq1NAV4=load('eq1NAV4.txt');
eq1NAV5=load('eq1NAV5.txt');
eq1LEI1=load('eq1LEI1.txt');
eq1LEI2=load('eq1LEI2.txt');

eq2NAV2=load('eq2NAV2.txt');
eq2NAV3=load('eq2NAV3.txt');
eq2NAV4=load('eq2NAV4.txt');
eq2NAV5=load('eq2NAV5.txt');
eq2LEI1=load('eq2LEI1.txt');
eq2LEI2=load('eq2LEI2.txt');

eq3NAV2=load('eq3NAV2.txt');
eq3NAV3=load('eq3NAV3.txt');
eq3NAV4=load('eq3NAV4.txt');
eq3NAV5=load('eq3NAV5.txt');
eq3LEI1=load('eq3LEI1.txt');
eq3LEI2=load('eq3LEI2.txt');


eq4NAV3=load('eq4NAV3.txt');
eq4NAV4=load('eq4NAV4.txt');
eq4NAV5=load('eq4NAV5.txt');
eq4NAV6=load('eq4NAV6.txt');
eq4NAV7=load('eq4NAV7.txt');



%load ACCELERATION data.datagpscompEQ_ has 9 columns. 
%Column 1,2,3 are the 3 accelerometers on the 7th floor, 
%Column 4,5,6 are the 3 accelerometers on the 5th floor, 
%column 7,8,9 are the 3 accelerometers on the 3rd floor. 
datagpscompEQ1=load('datagpscompEQ1');
datagpscompEQ2=load('datagpscompEQ2');
datagpscompEQ3=load('datagpscompEQ3');
datagpscompEQ4=load('datagpscompEQ4');
%ACC on the platen
dataEQ1=load('dataGPS1EQ1');
dataEQ2=load('dataGPS1EQ2');
dataEQ3=load('dataGPS1EQ3');
dataEQ4=load('dataGPS1EQ4');
%load MTS data.Transfer inch to cm.
datashaketableEQ1=load('dataGPS2EQ1');
datashaketableEQ2=load('dataGPS2EQ2');
datashaketableEQ3=load('dataGPS2EQ3');
datashaketableEQ4=load('dataGPS2EQ4');
MTSEQ1disp=-0.0254*datashaketableEQ1(:,2);
MTSEQ2disp=-0.0254*datashaketableEQ2(:,2);
MTSEQ3disp=-0.0254*datashaketableEQ3(:,2);
MTSEQ4disp=-0.0254*datashaketableEQ4(:,2);
%load Accelerometers on the platen.
platen1EQ1=dataEQ1(:,17);
platen2EQ1=dataEQ1(:,18);

platen1EQ2=dataEQ2(:,17);
platen2EQ2=dataEQ2(:,18);

platen1EQ3=dataEQ3(:,17);
platen2EQ3=dataEQ3(:,18);

platen1EQ4=dataEQ4(:,17);
platen2EQ4=dataEQ4(:,18);
%seperate accelerometer data by 
%events and stations
top1EQ1=datagpscompEQ1(:,1);
top2EQ1=datagpscompEQ1(:,2);
top3EQ1=datagpscompEQ1(:,3);
mid1EQ1=datagpscompEQ1(:,4);
mid2EQ1=datagpscompEQ1(:,5);
mid3EQ1=datagpscompEQ1(:,6);
low1EQ1=datagpscompEQ1(:,7);
low2EQ1=datagpscompEQ1(:,8);
low3EQ1=datagpscompEQ1(:,9);

top1EQ2=datagpscompEQ2(:,1);
top2EQ2=datagpscompEQ2(:,2);
top3EQ2=datagpscompEQ2(:,3);
mid1EQ2=datagpscompEQ2(:,4);
mid2EQ2=datagpscompEQ2(:,5);
mid3EQ2=datagpscompEQ2(:,6);
low1EQ2=datagpscompEQ2(:,7);
low2EQ2=datagpscompEQ2(:,8);
low3EQ2=datagpscompEQ2(:,9);

top1EQ3=datagpscompEQ3(:,1);
top2EQ3=datagpscompEQ3(:,2);
top3EQ3=datagpscompEQ3(:,3);
mid1EQ3=datagpscompEQ3(:,4);
mid2EQ3=datagpscompEQ3(:,5);
mid3EQ3=datagpscompEQ3(:,6);
low1EQ3=datagpscompEQ3(:,7);
low2EQ3=datagpscompEQ3(:,8);
low3EQ3=datagpscompEQ3(:,9);

top1EQ4=datagpscompEQ4(:,1);
top2EQ4=datagpscompEQ4(:,2);
top3EQ4=datagpscompEQ4(:,3);
mid1EQ4=datagpscompEQ4(:,4);
mid2EQ4=datagpscompEQ4(:,5);
mid3EQ4=datagpscompEQ4(:,6);
low1EQ4=datagpscompEQ4(:,7);
low2EQ4=datagpscompEQ4(:,8);
low3EQ4=datagpscompEQ4(:,9);

%high-pass filter for calculating displacement from acceleration data.
fs=240;%Accelerometer sampling rate
fcutin=0.1;
b=fir1(5000,fcutin/(fs/2),'high');



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%               eq1                  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s1=1:length(top1EQ1);
%acceleration to displacement
top1EQ1f = filtfilt(b,1,top1EQ1);
itop1EQ1=cumtrapz(s1/fs,top1EQ1f);
itop1EQ1f=filtfilt(b,1,itop1EQ1);
iitop1EQ1=cumtrapz(s1/fs,itop1EQ1f);
iitop1EQ1f=filtfilt(b,1,iitop1EQ1);
sdisptop1EQ1=iitop1EQ1f*10;;

top2EQ1f = filtfilt(b,1,top2EQ1);
itop2EQ1=cumtrapz(s1/fs,top2EQ1f);
itop2EQ1f=filtfilt(b,1,itop2EQ1);
iitop2EQ1=cumtrapz(s1/fs,itop2EQ1f);
iitop2EQ1f=filtfilt(b,1,iitop2EQ1);
sdisptop2EQ1=iitop2EQ1f*10;;

top3EQ1f = filtfilt(b,1,top3EQ1);
itop3EQ1=cumtrapz(s1/fs,top3EQ1f);
itop3EQ1f=filtfilt(b,1,itop3EQ1);
iitop3EQ1=cumtrapz(s1/fs,itop3EQ1f);
iitop3EQ1f=filtfilt(b,1,iitop3EQ1);
sdisptop3EQ1=iitop3EQ1f*10;;

mid1EQ1f = filtfilt(b,1,mid1EQ1);
imid1EQ1=cumtrapz(s1/fs,mid1EQ1f);
imid1EQ1f=filtfilt(b,1,imid1EQ1);
iimid1EQ1=cumtrapz(s1/fs,imid1EQ1f);
iimid1EQ1f=filtfilt(b,1,iimid1EQ1);
sdispmid1EQ1=iimid1EQ1f*10;;

mid2EQ1f = filtfilt(b,1,mid2EQ1);
imid2EQ1=cumtrapz(s1/fs,mid2EQ1f);
imid2EQ1f=filtfilt(b,1,imid2EQ1);
iimid2EQ1=cumtrapz(s1/fs,imid2EQ1f);
iimid2EQ1f=filtfilt(b,1,iimid2EQ1);
sdispmid2EQ1=iimid2EQ1f*10;;

mid3EQ1f = filtfilt(b,1,mid3EQ1);
imid3EQ1=cumtrapz(s1/fs,mid3EQ1f);
imid3EQ1f=filtfilt(b,1,imid3EQ1);
iimid3EQ1=cumtrapz(s1/fs,imid3EQ1f);
iimid3EQ1f=filtfilt(b,1,iimid3EQ1);
sdispmid3EQ1=iimid3EQ1f*10;;

low1EQ1f = filtfilt(b,1,low1EQ1);
ilow1EQ1=cumtrapz(s1/fs,low1EQ1f);
ilow1EQ1f=filtfilt(b,1,ilow1EQ1);
iilow1EQ1=cumtrapz(s1/fs,ilow1EQ1f);
iilow1EQ1f=filtfilt(b,1,iilow1EQ1);
sdisplow1EQ1=iilow1EQ1f*10;;

low2EQ1f = filtfilt(b,1,low2EQ1);
ilow2EQ1=cumtrapz(s1/fs,low2EQ1f);
ilow2EQ1f=filtfilt(b,1,ilow2EQ1);
iilow2EQ1=cumtrapz(s1/fs,ilow2EQ1f);
iilow2EQ1f=filtfilt(b,1,iilow2EQ1);
sdisplow2EQ1=iilow2EQ1f*10;;

low3EQ1f = filtfilt(b,1,low2EQ1);
ilow3EQ1=cumtrapz(s1/fs,low2EQ1f);
ilow3EQ1f=filtfilt(b,1,ilow2EQ1);
iilow3EQ1=cumtrapz(s1/fs,ilow2EQ1f);
iilow3EQ1f=filtfilt(b,1,iilow2EQ1);
sdisplow3EQ1=iilow3EQ1f*10;

%%%%top of the building, nav3 with top1 are co-loated GPS and ACC%%%%
s1=1:length(top1EQ1);
accdisp=-sdisptop1EQ1;
s1=s1/240;
plot(s1,accdisp,'r');
hold on
eq1NAV3time=eq1NAV3(:,1)*3600+eq1NAV3(:,2)*60+eq1NAV3(:,3);
disp=eq1NAV3(:,4);
eq1NAV3disp=disp-mean(disp(1:2000));
eq1NAV3time=eq1NAV3time-eq1NAV3time(1);

shift=50.8;


plot(eq1NAV3time(shift*50:length(eq1NAV3time))-eq1NAV3time(shift*50),eq1NAV3disp(shift*50:length(eq1NAV3disp)),'b.')

%%%%RMS%%%%
   % for i=1:128*50
    %    er(i)=(eq1NAV3disp(i)-accdisp(1+round((i-1)*1/50*240)));
    %end
    %rms=sqrt(sum(er.^2)/(i))

%figure
%ery=1:i;
%plot(ery/50,er)

%%%%%%%%%%%%%%%%%Kalman%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
accel=resample(top1EQ1-mean(top1EQ1(1:5*240)),250,240);
accel=accel(1:128*250);
GPS=eq1NAV3disp(shift*50:length(eq1NAV3disp));
GPStime=eq1NAV3time(shift*50:length(eq1NAV3time));
GPS=GPS(1:50*128);
GPStime=GPStime(1:50*128);

time2=length(accel);

q=1;

%key parametor
r=10^-5;


Ta=1/250;
Td=1/50;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];
%p(1:2,1:2)=diag([1 1]);
K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
%vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(GPS))=0;
for n=1:length(GPS)
    z(1,n)=GPS(n);
    %z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;

for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,5) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/5+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);
tl=1:length(sl);

figure
plot(tl/250,sl,'r.')
hold on


plot(eq1NAV3time(shift*50:length(eq1NAV3time))-eq1NAV3time(shift*50),eq1NAV3disp(shift*50:length(eq1NAV3disp)),'b.')

%%RMS%%%
GPSeq1NAV3=eq1NAV3disp(shift*50:length(eq1NAV3disp));
Kalmaneq1TOP=sl;
for i=1:120*50
    ers(i)=GPSeq1NAV3(i)-Kalmaneq1TOP(1+round((i-1)*1/50*250));
end
rms=sqrt(sum(ers.^2)/(i))

ersy=1:i;
figure
plot(ersy/50,ers)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%5th%%%
figure
title('plot of Acceleration and GPS data on the 5th floor');

s22=1:length(mid1EQ1);
plot(s22/240,-sdispmid1EQ1,'r');
hold on
eq1LEI2time=eq1LEI2(:,1)*3600+eq1LEI2(:,2)*60+eq1LEI2(:,3);
eq1LEI2disp=eq1LEI2(:,4);
eq1LEI2disp=eq1LEI2disp-mean(eq1LEI2disp(1:100));
eq1LEI2time=eq1LEI2time-eq1LEI2time(1);
shift4=50.9;
plot(eq1LEI2time(shift4*10:length(eq1LEI2time))-eq1LEI2time(shift4*10),eq1LEI2disp(shift4*10:length(eq1LEI2disp)),'b.')


%%%%%%%kalman%%%%%%%%%%%%%%%

accel=resample(low2EQ1-mean(low2EQ1(1:5*240)),250,240);
accel=accel(1:125*250);
GPS=eq1LEI2disp(shift4*10:length(eq1LEI2disp));
GPS=GPS(1:10*125);

time2=length(accel);


q=1;
r=10^-3;


Ta=1/250;
Td=1/10;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];
%p(1:2,1:2)=diag([1 1]);
K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,25) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/25+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);
tl=1:length(sl);
figure
plot(tl/250,sl,'r.')
hold on

plot(eq1LEI2time(shift4*10:length(eq1LEI2time))-eq1LEI2time(shift4*10),eq1LEI2disp(shift4*10:length(eq1LEI2disp)),'b.')


%%%RMS%%%
GPSeq1LEI2=eq1LEI2disp(shift4*10:length(eq1LEI2disp));
Kalmaneq1LOW=sl;

for i=1:125*10
    ers(i)=GPSeq1LEI2(i)-Kalmaneq1LOW(1+round((i-1)*1/10*250));
end
rms=sqrt(sum(ers.^2)/(i))

ersy=1:i;
figure
plot(ersy/10,ers(1:i))


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%3rd%%% BAD DATA
figure
title('plot of Acceleration and GPS data on the 5th floor');

s22=1:length(low1EQ1);
plot(s22/240,-sdisplow1EQ1,'r');
hold on
eq1LEI1time=eq1LEI1(:,1)*3600+eq1LEI1(:,2)*60+eq1LEI1(:,3);
eq1LEI1disp=eq1LEI1(:,4);
eq1LEI1disp=eq1LEI1disp-mean(eq1LEI1disp(1:100));
eq1LEI1time=eq1LEI1time-eq1LEI1time(1);
shift4=50.9;
plot(eq1LEI1time(shift4*10:length(eq1LEI1time))-eq1LEI1time(shift4*10),eq1LEI1disp(shift4*10:length(eq1LEI1disp)),'b')


%%%%%%%kalman%%%%%%%%%%%%%%%

accel=resample(low1EQ1-mean(low1EQ1(1:5*240)),250,240);
accel=accel(1:125*250);
GPS=eq1LEI1disp(shift4*10:length(eq1LEI1disp));
GPS=GPS(1:10*125);

time2=length(accel);

q=1;
r=10^-3;


Ta=1/250;
Td=1/10;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];

K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,25) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/25+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);
tl=1:length(sl);

figure
plot(tl/250,sl,'r.')
hold on

plot(eq1LEI1time(shift4*10:length(eq1LEI1time))-eq1LEI1time(shift4*10),eq1LEI1disp(shift4*10:length(eq1LEI1disp)),'b.')


%%%RMS%%%
GPSeq1LEI1=eq1LEI1disp(shift4*10:length(eq1LEI1disp));
Kalmaneq1LOW=sl;

for i=1:125*10
    ers(i)=GPSeq1LEI1(i)-Kalmaneq1LOW(1+round((i-1)*1/10*250));
end
rms=sqrt(sum(ers.^2)/(i))

ersy=1:i;
figure
plot(ersy/10,ers(1:i))

%%%% BASE  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s1=1:length(top1EQ1);
s11=1:length(platen1EQ1);
s12=1:length(platen2EQ1);
%acceleration to displacement

platen1EQ1f=filtfilt(b,1,platen1EQ1);
iplaten1EQ1=cumtrapz(s11/fs,platen1EQ1f);
iplaten1EQ1f=filtfilt(b,1,iplaten1EQ1);
iiplaten1EQ1=cumtrapz(s11/fs,iplaten1EQ1f);
iiplaten1EQ1f=filtfilt(b,1,iiplaten1EQ1);
sdispplaten1EQ1=iiplaten1EQ1f*10;


platen2EQ1f=filtfilt(b,1,platen2EQ1);
iplaten2EQ1=cumtrapz(s12/fs,platen2EQ1f);
iplaten2EQ1f=filtfilt(b,1,iplaten2EQ1);
iiplaten2EQ1=cumtrapz(s12/fs,iplaten2EQ1f);
iiplaten2EQ1f=filtfilt(b,1,iiplaten2EQ1);
sdispplaten2EQ1=iiplaten1EQ1f*10;

figure

title('plot of Acceleration and GPS data on the platen');
shift5=51.32;
plot(s11/240,-sdispplaten1EQ1,'r');
hold on

eq1NAV5GPS=eq1NAV5(:,4);
eq1NAV5time=eq1NAV5(:,1)*3600+eq1NAV5(:,2)*60+eq1NAV5(:,3);
eq1NAV5disp=eq1NAV5GPS-mean(eq1NAV5GPS(1000:2500));
time1=eq1NAV5time-eq1NAV5time(1);
plot(time1(shift5*50:length(time1))-time1(shift5*50),eq1NAV5disp(shift5*50:length(eq1NAV5disp)),'b')


%%%%RMS%%%%
%    for i=1:135*50
%        er(i)=(eq2NAV3disp(i)-accdisp2(1+round((i-1)*1/50*240)));
%    end
%    rms=sqrt(sum(er.^2)/(i))

%figure
%ery=1:i;
%plot(ery/50,er)

%%%%%%%%%%%%%%%%%Kalman%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
shift5=51.20;
accel=resample(platen1EQ1-mean(platen1EQ1(1:5*240)),250,240);%resample accelerometer data
accel=accel(1:125*250);
GPS=eq1NAV5disp(shift5*50:length(eq1NAV5disp));
GPS=GPS(1:50*125);

time2=length(accel);

q=1;
r=10^-6;


Ta=1/250;
Td=1/50;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];
%p(1:2,1:2)=diag([1 1]);
K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,5) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/5+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);

tl=1:length(sl);
s2l=xmeasure(2,1:k);
t2=1:length(s2l);
figure
tl=tl/250;
syy=sl(0.4*250:length(sl));
plot(tl(0.4*250:length(tl))-tl(0.4*250),syy-mean(syy(1:4*250)),'r.')
hold on

lyy=eq1NAV5disp((shift5+0.4)*50:length(eq1NAV5disp));
plot(time1((shift5+0.4)*50:length(time1))-time1((shift5+0.4)*50),lyy-mean(lyy(1:4*50)),'b.')

hold on
M1=1:length(MTSEQ1disp);
MTSdispEQ1=MTSEQ1disp-mean(MTSEQ1disp(1:5000));
M1time=M1/1024;
Mshift=.75;
plot(M1time(Mshift*1024+1:length(MTSdispEQ1))-M1time(Mshift*1024+1),MTSdispEQ1(Mshift*1024+1:length(MTSdispEQ1)),'k');


GPSeq1NAV5=lyy-mean(lyy(1:4*50));
Kalmaneq1BASE=syy-mean(syy(1:4*250));
MTSeq1=MTSdispEQ1(Mshift*1024+1:length(MTSdispEQ1));
%%RMS%%%
for i=1:120*50
    ers(i)=GPSeq2NAV5(i)-MTSeq1(1+round((i-1)*1/50*1024));
end
rms=sqrt(sum(ers.^2)/(i))
%ersy=1:i;
%figure
%plot(ersy/50,ers)

for i=1:120*250
    ers2(i)=Kalmaneq1BASE(i)-MTSeq1(1+round((i-1)*1/250*1024));
end
rms2=sqrt(sum(ers2.^2)/(i))





%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%






%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%               eq2                  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s22=1:length(top1EQ2);

top1EQ2f = filtfilt(b,1,top1EQ2);
itop1EQ2=cumtrapz(s22/fs,top1EQ2f);
itop1EQ2f=filtfilt(b,1,itop1EQ2);
iitop1EQ2=cumtrapz(s22/fs,itop1EQ2f);
iitop1EQ2f=filtfilt(b,1,iitop1EQ2);
sdisptop1EQ2=iitop1EQ2f*10;;

top2EQ2f = filtfilt(b,1,top2EQ2);
itop2EQ2=cumtrapz(s22/fs,top2EQ2f);
itop2EQ2f=filtfilt(b,1,itop2EQ2);
iitop2EQ2=cumtrapz(s22/fs,itop2EQ2f);
iitop2EQ2f=filtfilt(b,1,iitop2EQ2);
sdisptop2EQ2=iitop2EQ2f*10;;

top3EQ2f = filtfilt(b,1,top3EQ2);
itop3EQ2=cumtrapz(s22/fs,top3EQ2f);
itop3EQ2f=filtfilt(b,1,itop3EQ2);
iitop3EQ2=cumtrapz(s22/fs,itop3EQ2f);
iitop3EQ2f=filtfilt(b,1,iitop3EQ2);
sdisptop3EQ2=iitop3EQ2f*10;;

mid1EQ2f = filtfilt(b,1,mid1EQ2);
imid1EQ2=cumtrapz(s22/fs,mid1EQ2f);
imid1EQ2f=filtfilt(b,1,imid1EQ2);
iimid1EQ2=cumtrapz(s22/fs,imid1EQ2f);
iimid1EQ2f=filtfilt(b,1,iimid1EQ2);
sdispmid1EQ2=iimid1EQ2f*10;;

mid2EQ2f = filtfilt(b,1,mid2EQ2);
imid2EQ2=cumtrapz(s22/fs,mid2EQ2f);
imid2EQ2f=filtfilt(b,1,imid2EQ2);
iimid2EQ2=cumtrapz(s22/fs,imid2EQ2f);
iimid2EQ2f=filtfilt(b,1,iimid2EQ2);
sdispmid2EQ2=iimid2EQ2f*10;;

mid3EQ2f = filtfilt(b,1,mid3EQ2);
imid3EQ2=cumtrapz(s22/fs,mid3EQ2f);
imid3EQ2f=filtfilt(b,1,imid3EQ2);
iimid3EQ2=cumtrapz(s22/fs,imid3EQ2f);
iimid3EQ2f=filtfilt(b,1,iimid3EQ2);
sdispmid3EQ2=iimid3EQ2f*10;;

low1EQ2f = filtfilt(b,1,low1EQ2);
ilow1EQ2=cumtrapz(s22/fs,low1EQ2f);
ilow1EQ2f=filtfilt(b,1,ilow1EQ2);
iilow1EQ2=cumtrapz(s22/fs,ilow1EQ2f);
iilow1EQ2f=filtfilt(b,1,iilow1EQ2);
sdisplow1EQ2=iilow1EQ2f*10;;

low2EQ2f = filtfilt(b,1,low2EQ2);
ilow2EQ2=cumtrapz(s22/fs,low2EQ2f);
ilow2EQ2f=filtfilt(b,1,ilow2EQ2);
iilow2EQ2=cumtrapz(s22/fs,ilow2EQ2f);
iilow2EQ2f=filtfilt(b,1,iilow2EQ2);
sdisplow2EQ2=iilow2EQ2f*10;;

low3EQ2f = filtfilt(b,1,low2EQ2);
ilow3EQ2=cumtrapz(s22/fs,low2EQ2f);
ilow3EQ2f=filtfilt(b,1,ilow2EQ2);
iilow3EQ2=cumtrapz(s22/fs,ilow2EQ2f);
iilow3EQ2f=filtfilt(b,1,iilow2EQ2);
sdisplow3EQ2=iilow3EQ2f*10;

%%%%top of the building, nav3 with top1 are co-loated GPS and ACC%%%%
s22=1:length(top1EQ2);
accdisp2=-sdisptop1EQ2;
s22=s22/240;
plot(s22,accdisp2,'r');
hold on
eq2NAV3time=eq2NAV3(:,1)*3600+eq2NAV3(:,2)*60+eq2NAV3(:,3);
disp2=eq2NAV3(:,4);
eq2NAV3disp=disp2-mean(disp2(1:2000));
eq2NAV3time=eq2NAV3time-eq2NAV3time(1);

shift2=58.9;


plot(eq2NAV3time(shift2*50:length(eq2NAV3time))-eq2NAV3time(shift2*50),eq2NAV3disp(shift2*50:length(eq2NAV3disp)),'b.')

%%%%RMS%%%%
    for i=1:135*50
        er(i)=(eq2NAV3disp(i)-accdisp2(1+round((i-1)*1/50*240)));
    end
    rms=sqrt(sum(er.^2)/(i))

figure
ery=1:i;
plot(ery/50,er)

%%%%%%%%%%%%%%%%%Kalman%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
accel=resample(top1EQ2-mean(top1EQ2(1:5*240)),250,240);
accel=accel(1:130*250);
GPS=eq2NAV3disp(shift2*50:length(eq2NAV3disp));
GPS=GPS(1:50*135);

time2=length(accel);

q=1;
r=10^-4;


Ta=1/250;
Td=1/50;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];

K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,5) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/5+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);

tl=1:length(sl);

figure
plot(tl/250,sl,'r.')
hold on


plot(eq2NAV3time(shift2*50:length(eq2NAV3time))-eq2NAV3time(shift2*50),eq2NAV3disp(shift2*50:length(eq2NAV3disp)),'b.')

GPSeq2NAV3=eq2NAV3disp(shift2*50:length(eq2NAV3disp));
Kalmaneq2TOP=sl;
%%RMS%%%
for i=1:130*50
    ers(i)=GPSeq2NAV3(i)-Kalmaneq2TOP(1+round((i-1)*1/50*250));
end
rms=sqrt(sum(ers.^2)/(i))

%ersy=1:i;
%figure
%plot(ersy/50,ers)





%%%5 floor%%%
figure
title('plot of Acceleration and GPS data on the 5th floor');

s22=1:length(mid1EQ2);
plot(s22/240,-sdispmid1EQ2,'r');
hold on
eq2LEI1time=eq2LEI1(:,1)*3600+eq2LEI1(:,2)*60+eq2LEI1(:,3);
eq2LEI1disp=eq2LEI1(:,4);
eq2LEI1disp=eq2LEI1disp-mean(eq2LEI1disp(1:100));
eq2LEI1time=eq2LEI1time-eq2LEI1time(1);
shift3=42.9;
plot(eq2LEI1time(shift3*10:length(eq2LEI1time))-eq2LEI1time(shift3*10),eq2LEI1disp(shift3*10:length(eq2LEI1disp)),'b.')


%%%%%%%kalman%%%%%%%%%%%%%%%

accel=resample(mid1EQ2-mean(mid1EQ2(1:5*240)),250,240);
accel=accel(1:135*250);
GPS=eq2LEI1disp(shift3*10:length(eq2LEI1disp));
GPS=GPS(1:10*135);

time2=length(accel);

q=1;
r=10^-5;


Ta=1/250;
Td=1/10;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];
%p(1:2,1:2)=diag([1 1]);
K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    %z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,25) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/25+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);
tl=1:length(sl);

figure
plot(tl/250,sl,'r.')
hold on

plot(eq2LEI1time(shift3*10:length(eq2LEI1time))-eq2LEI1time(shift3*10),eq2LEI1disp(shift3*10:length(eq2LEI1disp)),'b.')

%%%RMS%%%
GPSeq2LEI1=eq2LEI1disp(shift3*10:length(eq2LEI1disp));
Kalmaneq2MID=sl;

for i=1:135*10
    ers(i)=GPSeq2LEI1(i)-Kalmaneq2MID(1+round((i-1)*1/10*250));
end
rms=sqrt(sum(ers.^2)/(i))

%ersy=1:i;
%figure
%plot(ersy/10,ers(1:i))


%%%%%3rd floor%%%%%%%

figure
title('plot of Acceleration and GPS data on the 5th floor');

s22=1:length(low1EQ2);
plot(s22/240,-sdisplow1EQ2,'r');
hold on
eq2LEI2time=eq2LEI2(:,1)*3600+eq2LEI2(:,2)*60+eq2LEI2(:,3);
eq2LEI2disp=eq2LEI2(:,4);
eq2LEI2disp=eq2LEI2disp-mean(eq2LEI2disp(1:50));
eq2LEI2time=eq2LEI2time-eq2LEI2time(1);
shift4=26.4;
plot(eq2LEI2time(shift4*10:length(eq2LEI2time))-eq2LEI2time(shift4*10),eq2LEI2disp(shift4*10:length(eq2LEI2disp)),'b.')


%%%%%%%kalman%%%%%%%%%%%%%%%

accel=resample(low1EQ2-mean(low1EQ2(1:5*240)),250,240);
accel=accel(1:135*250);
GPS=eq2LEI2disp(shift4*10:length(eq2LEI2disp));
GPS=GPS(1:10*135);

time2=length(accel);

q=1;
r=10^-4;


Ta=1/250;
Td=1/10;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];

K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,25) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/25+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);

tl=1:length(sl);
s2l=xmeasure(2,1:k);
t2=1:length(s2l);
figure
plot(tl/250,sl,'r.')
hold on

plot(eq2LEI2time(shift4*10:length(eq2LEI2time))-eq2LEI2time(shift4*10),eq2LEI2disp(shift4*10:length(eq2LEI2disp)),'b.')

%%%RMS%%%
GPSeq2LEI2=eq2LEI2disp(shift4*10:length(eq2LEI2disp));
Kalmaneq2LOW=sl;

for i=1:135*10
    ers(i)=GPSeq2LEI2(i)-Kalmaneq2LOW(1+round((i-1)*1/10*250));
end
rms=sqrt(sum(ers.^2)/(i))

ersy=1:i;
figure
plot(ersy/10,ers(1:i))



%%%% BASE  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s1=1:length(top1EQ2);
s11=1:length(platen1EQ2);
s12=1:length(platen2EQ2);
%acceleration to displacement

platen1EQ2f=filtfilt(b,1,platen1EQ2);
iplaten1EQ2=cumtrapz(s11/fs,platen1EQ2f);
iplaten1EQ2f=filtfilt(b,1,iplaten1EQ2);
iiplaten1EQ2=cumtrapz(s11/fs,iplaten1EQ2f);
iiplaten1EQ2f=filtfilt(b,1,iiplaten1EQ2);
sdispplaten1EQ2=iiplaten1EQ2f*10;

platen2EQ2f=filtfilt(b,1,platen2EQ2);
iplaten2EQ2=cumtrapz(s12/fs,platen2EQ2f);
iplaten2EQ2f=filtfilt(b,1,iplaten2EQ2);
iiplaten2EQ2=cumtrapz(s12/fs,iplaten2EQ2f);
iiplaten2EQ2f=filtfilt(b,1,iiplaten2EQ2);
sdispplaten2EQ2=iiplaten1EQ2f*10;

figure

title('plot of Acceleration and GPS data on the platen');
shift5=56.32;
plot(s11/240,-sdispplaten1EQ2,'r');
hold on

eq2NAV5GPS=eq2NAV5(:,4);
eq2NAV5time=eq2NAV5(:,1)*3600+eq2NAV5(:,2)*60+eq2NAV5(:,3);
eq2NAV5disp=eq2NAV5GPS-mean(eq2NAV5GPS(1000:2500));
time1=eq2NAV5time-eq2NAV5time(1);
plot(time1(shift5*50:length(time1))-time1(shift5*50),eq2NAV5disp(shift5*50:length(eq2NAV5disp)),'b')





%%%%RMS%%%%
%    for i=1:135*50
%        er(i)=(eq2NAV3disp(i)-accdisp2(1+round((i-1)*1/50*240)));
%    end
%    rms=sqrt(sum(er.^2)/(i))

%figure
%ery=1:i;
%plot(ery/50,er)

%%%%%%%%%%%%%%%%%Kalman%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
shift5=56.30;
accel=resample(platen1EQ2-mean(platen1EQ2(1:5*240)),250,240);
accel=accel(1:130*250);
GPS=eq2NAV5disp(shift5*50:length(eq2NAV5disp));
GPS=GPS(1:50*130);

time2=length(accel);

q=1;
r=10^-6;


Ta=1/250;
Td=1/50;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];
K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,5) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/5+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);

tl=1:length(sl);
s2l=xmeasure(2,1:k);
t2=1:length(s2l);
figure
tl=tl/250;
plot(tl(0.5*250:length(tl))-tl(0.5*250),sl(0.5*250:length(sl)),'r.')
hold on


plot(time1((shift5+0.5)*50:length(time1))-time1((shift5+0.5)*50),eq2NAV5disp((shift5+0.5)*50:length(eq2NAV5disp)),'b.')

hold on
M2=1:length(MTSEQ2disp);
MTSdispEQ2=MTSEQ2disp-mean(MTSEQ2disp(1:5000));
M2time=M2/1024;
Mshift=0.75;
plot(M2time(Mshift*1024+1:length(MTSdispEQ2))-M2time(Mshift*1024+1),MTSdispEQ2(Mshift*1024+1:length(MTSdispEQ2)),'k');


GPSeq2NAV5=eq2NAV5disp((shift5+0.5)*50:length(eq2NAV5disp));
Kalmaneq2BASE=sl(0.5*250:length(sl));
MTSeq2=MTSdispEQ2(Mshift*1024+1:length(MTSdispEQ2));

%%RMS%%%
for i=1:130*50
    ers(i)=GPSeq2NAV5(i)-MTSeq2(1+round((i-1)*1/50*1024));
end
rms=sqrt(sum(ers.^2)/(i))
%ersy=1:i;
%figure
%plot(ersy/50,ers)

for i=1:125*250
    ers2(i)=Kalmaneq2BASE(i)-MTSeq2(1+round((i-1)*1/250*1024));
end
rms2=sqrt(sum(ers2.^2)/(i))




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%              EQ3            %             
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

s22=1:length(top1EQ3);
%acceleration to displacement
top1EQ3f = filtfilt(b,1,top1EQ3);
itop1EQ3=cumtrapz(s22/fs,top1EQ3f);
itop1EQ3f=filtfilt(b,1,itop1EQ3);
iitop1EQ3=cumtrapz(s22/fs,itop1EQ3f);
iitop1EQ3f=filtfilt(b,1,iitop1EQ3);
sdisptop1EQ3=iitop1EQ3f*10;;



mid1EQ3f = filtfilt(b,1,mid1EQ3);
imid1EQ3=cumtrapz(s22/fs,mid1EQ3f);
imid1EQ3f=filtfilt(b,1,imid1EQ3);
iimid1EQ3=cumtrapz(s22/fs,imid1EQ3f);
iimid1EQ3f=filtfilt(b,1,iimid1EQ3);
sdispmid1EQ3=iimid1EQ3f*10;;



low1EQ3f = filtfilt(b,1,low1EQ3);
ilow1EQ3=cumtrapz(s22/fs,low1EQ3f);
ilow1EQ3f=filtfilt(b,1,ilow1EQ3);
iilow1EQ3=cumtrapz(s22/fs,ilow1EQ3f);
iilow1EQ3f=filtfilt(b,1,iilow1EQ3);
sdisplow1EQ3=iilow1EQ3f*10;;


%%%%top of the building, nav3 with top1 are co-loated GPS and ACC%%%%
s22=1:length(top1EQ3);
accdisp2=-sdisptop1EQ3;
s22=s22/240;
plot(s22,accdisp2,'r');
hold on
eq3NAV3time=eq3NAV3(:,1)*3600+eq3NAV3(:,2)*60+eq3NAV3(:,3);
disp2=eq3NAV3(:,4);
eq3NAV3disp=disp2-mean(disp2(1:2000));
eq3NAV3time=eq3NAV3time-eq3NAV3time(1);

shift2=56.9;


plot(eq3NAV3time(shift2*50:length(eq3NAV3time))-eq3NAV3time(shift2*50),eq3NAV3disp(shift2*50:length(eq3NAV3disp)),'b.')

%%%%RMS%%%%
 %   for i=1:135*50
 %       er(i)=(eq2NAV3disp(i)-accdisp2(1+round((i-1)*1/50*240)));
 %   end
%    rms=sqrt(sum(er.^2)/(i))

%figure
%ery=1:i;
%plot(ery/50,er)

%%%%%%%%%%%%%%%%%Kalman%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%top%%%
accel=resample(top1EQ3-mean(top1EQ3(1:5*240)),250,240);
accel=accel(1:125*250);
GPS=eq3NAV3disp(shift2*50:length(eq3NAV3disp));
GPS=GPS(1:50*125);

time2=length(accel);


q=1;
r=10^-6;


Ta=1/250;
Td=1/50;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];

K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,5) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/5+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);

tl=1:length(sl);
s2l=xmeasure(2,1:k);
t2=1:length(s2l);
figure
plot(tl/250,sl,'r.')
hold on


plot(eq3NAV3time(shift2*50:length(eq3NAV3time))-eq3NAV3time(shift2*50),eq3NAV3disp(shift2*50:length(eq3NAV3disp)),'b.')


GPSeq3NAV3=eq3NAV3disp(shift2*50:length(eq3NAV3disp));
Kalmaneq3TOP=sl;
%%RMS%%%
for i=1:125*50
    ers(i)=GPSeq3NAV3(i)-Kalmaneq3TOP(1+round((i-1)*1/50*250));
end
rms=sqrt(sum(ers.^2)/(i))

ersy=1:i;
figure
plot(ersy/50,ers(125*50))



%%%5 floor%%%
figure
title('plot of Acceleration and GPS data on the 5th floor');

s22=1:length(mid1EQ3);
plot(s22/240,-sdispmid1EQ3,'r');
hold on
eq3LEI1time=eq3LEI1(:,1)*3600+eq3LEI1(:,2)*60+eq3LEI1(:,3);
eq3LEI1disp=eq3LEI1(:,4);
eq3LEI1disp=eq3LEI1disp-mean(eq3LEI1disp(570:620));
eq3LEI1time=eq3LEI1time-eq3LEI1time(1);

shift3=57;

plot(eq3LEI1time(shift3*10:length(eq3LEI1time))-eq3LEI1time(shift3*10),eq3LEI1disp(shift3*10:length(eq3LEI1disp)),'b')


%%%%%%%kalman%%%%%%%%%%%%%%%

accel=resample(mid1EQ3-mean(mid1EQ3(1:5*240)),250,240);
accel=accel(1:125*250);
GPS=yy-mean(yy(1*10:5*10));
GPS=GPS(1:10*125);

time2=length(accel);

q=1;
r=10^-5;


Ta=1/250;
Td=1/10;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];

K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,25) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/25+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);

tl=1:length(sl);
s2l=xmeasure(2,1:k);
t2=1:length(s2l);
figure
plot(tl/250,sl,'r.')
hold on

plot(eq3LEI1time(shift3*10:length(eq3LEI1time))-eq3LEI1time(shift3*10),yy-mean(yy(1*10:5*10)),'b')

%%%RMS%%%
GPSeq3LEI1=yy-mean(yy(1*10:5*10));
Kalmaneq3MID=sl;

for i=1:125*10
    ers(i)=GPSeq3LEI1(i)-Kalmaneq3MID(1+round((i-1)*1/10*250));
end
rms=sqrt(sum(ers.^2)/(i))

ersy=1:i;
figure
plot(ersy/10,ers(1:i))


%%%%%3rd floor%%%%%%%

figure
title('plot of Acceleration and GPS data on the 5th floor');

s22=1:length(low1EQ3);
plot(s22/240,-sdisplow1EQ3,'r');
hold on
eq3LEI2time=eq3LEI2(:,1)*3600+eq3LEI2(:,2)*60+eq3LEI2(:,3);
eq3LEI2disp=eq3LEI2(:,4);
eq3LEI2disp=eq3LEI2disp-mean(eq3LEI2disp(1:50));
eq3LEI2time=eq3LEI2time-eq3LEI2time(1);
shift4=57;
plot(eq3LEI2time(shift4*10:length(eq3LEI2time))-eq3LEI2time(shift4*10),eq3LEI2disp(shift4*10:length(eq3LEI2disp)),'b')


%%%%%%%kalman%%%%%%%%%%%%%%%

accel=resample(low1EQ3-mean(low1EQ3(1:5*240)),250,240);
accel=accel(1:125*250);
GPS=eq3LEI2disp(shift4*10:length(eq3LEI2disp));
GPS=GPS(1:10*125);

time2=length(accel);

q=1;
r=5*10^-4;


Ta=1/250;
Td=1/10;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];
K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,25) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/25+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);
tl=1:length(sl);
s2l=xmeasure(2,1:k);
t2=1:length(s2l);
figure
plot(tl/250,sl,'r.')
hold on

plot(eq3LEI2time(shift4*10:length(eq3LEI2time))-eq3LEI2time(shift4*10),eq3LEI2disp(shift4*10:length(eq3LEI2disp)),'b')

%%%RMS%%%
GPSeq3LEI2=eq3LEI2disp(shift4*10:length(eq3LEI2disp));
Kalmaneq3LOW=sl;

for i=1:125*10
    ers(i)=GPSeq3LEI2(i)-Kalmaneq3LOW(1+round((i-1)*1/10*250));
end
rms=sqrt(sum(ers.^2)/(i))

ersy=1:i;
figure
plot(ersy/10,ers(1:i))



%%%% BASE  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s1=1:length(top1EQ3);
s11=1:length(platen1EQ3);
s12=1:length(platen2EQ3);
%acceleration to displacement

platen1EQ3f=filtfilt(b,1,platen1EQ3);
iplaten1EQ3=cumtrapz(s11/fs,platen1EQ3f);
iplaten1EQ3f=filtfilt(b,1,iplaten1EQ3);
iiplaten1EQ3=cumtrapz(s11/fs,iplaten1EQ3f);
iiplaten1EQ3f=filtfilt(b,1,iiplaten1EQ3);
sdispplaten1EQ3=iiplaten1EQ3f*10;

platen2EQ3f=filtfilt(b,1,platen2EQ3);
iplaten2EQ3=cumtrapz(s12/fs,platen2EQ3f);
iplaten2EQ3f=filtfilt(b,1,iplaten2EQ3);
iiplaten2EQ3=cumtrapz(s12/fs,iplaten2EQ3f);
iiplaten2EQ3f=filtfilt(b,1,iiplaten2EQ3);
sdispplaten2EQ3=iiplaten1EQ3f*10;

figure

title('plot of Acceleration and GPS data on the platen');
shift5=54.02;
plot(s11/240,-sdispplaten1EQ3,'r');
hold on

eq3NAV5GPS=eq3NAV5(:,4);
eq3NAV5time=eq3NAV5(:,1)*3600+eq3NAV5(:,2)*60+eq3NAV5(:,3);
eq3NAV5disp=eq3NAV5GPS-mean(eq3NAV5GPS(1000:2500));
time1=eq3NAV5time-eq3NAV5time(1);
plot(time1(shift5*50:length(time1))-time1(shift5*50),eq3NAV5disp(shift5*50:length(eq3NAV5disp)),'b')





%%%%RMS%%%%
%    for i=1:135*50
%        er(i)=(eq2NAV3disp(i)-accdisp2(1+round((i-1)*1/50*240)));
%    end
%    rms=sqrt(sum(er.^2)/(i))

%figure
%ery=1:i;
%plot(ery/50,er)

%%%%%%%%%%%%%%%%%Kalman%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

accel=resample(platen1EQ3-mean(platen1EQ3(1:5*240)),250,240);
accel=accel(1:125*250);
GPS=eq3NAV5disp(shift5*50:length(eq3NAV5disp));
GPS=GPS(1:50*125);

time2=length(accel);

q=1;
r=10^-6;


Ta=1/250;
Td=1/50;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];

K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,5) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/5+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);

tl=1:length(sl);
s2l=xmeasure(2,1:k);
t2=1:length(s2l);
figure
tl=tl/250;
plot(tl(2.9*250:length(tl))-tl(2.9*250),sl(2.9*250:length(sl)),'r.')
hold on


plot(time1((shift5+2.9)*50:length(time1))-time1((shift5+2.9)*50),eq3NAV5disp((shift5+2.9)*50:length(eq3NAV5disp)),'b.')

hold on
M3=1:length(MTSEQ3disp);
MTSdispEQ3=MTSEQ3disp-mean(MTSEQ3disp(1:5000));
M3time=M3/1024;
Mshift=0.0;
plot(M2time(Mshift*1024+1:length(MTSdispEQ3))-M2time(Mshift*1024+1),MTSdispEQ3(Mshift*1024+1:length(MTSdispEQ3)),'k');


GPSeq3NAV5=eq3NAV5disp((shift5+0.5)*50:length(eq3NAV5disp));
Kalmaneq3BASE=sl(0.5*250:length(sl));
MTSeq3=MTSdispEQ3(Mshift*1024+1:length(MTSdispEQ3));
%%RMS%%%
for i=1:120*50
    ers(i)=GPSeq3NAV5(i)-MTSeq3(1+round((i-1)*1/50*1024));
end
rms=sqrt(sum(ers.^2)/(i))

for i=1:120*250
    ers2(i)=Kalmaneq3BASE(i)-MTSeq3(1+round((i-1)*1/250*1024));
end
rms2=sqrt(sum(ers2.^2)/(i))

ersy=1:i;
figure
plot(ersy/50,ers)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%           EQ4            %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s4=1:length(top1EQ4);

top1EQ4f = filtfilt(b,1,top1EQ4);
itop1EQ4=cumtrapz(s4/fs,top1EQ4f);
itop1EQ4f=filtfilt(b,1,itop1EQ4);
iitop1EQ4=cumtrapz(s4/fs,itop1EQ4f);
iitop1EQ4f=filtfilt(b,1,iitop1EQ4);
sdisptop1EQ4=iitop1EQ4f*10;

top2EQ4f = filtfilt(b,1,top2EQ4);
itop2EQ4=cumtrapz(s4/fs,top2EQ4f);
itop2EQ4f=filtfilt(b,1,itop2EQ4);
iitop2EQ4=cumtrapz(s4/fs,itop2EQ4f);
iitop2EQ4f=filtfilt(b,1,iitop2EQ4);
sdisptop2EQ4=iitop2EQ4f*10;;

top3EQ4f = filtfilt(b,1,top3EQ4);
itop3EQ4=cumtrapz(s4/fs,top3EQ4f);
itop3EQ4f=filtfilt(b,1,itop3EQ4);
iitop3EQ4=cumtrapz(s4/fs,itop3EQ4f);
iitop3EQ4f=filtfilt(b,1,iitop3EQ4);
sdisptop3EQ4=iitop3EQ4f*10;;

mid1EQ4f = filtfilt(b,1,mid1EQ4);
imid1EQ4=cumtrapz(s4/fs,mid1EQ4f);
imid1EQ4f=filtfilt(b,1,imid1EQ4);
iimid1EQ4=cumtrapz(s4/fs,imid1EQ4f);
iimid1EQ4f=filtfilt(b,1,iimid1EQ4);
sdispmid1EQ4=iimid1EQ4f*10;;

mid2EQ4f = filtfilt(b,1,mid2EQ4);
imid2EQ4=cumtrapz(s4/fs,mid2EQ4f);
imid2EQ4f=filtfilt(b,1,imid2EQ4);
iimid2EQ4=cumtrapz(s4/fs,imid2EQ4f);
iimid2EQ4f=filtfilt(b,1,iimid2EQ4);
sdispmid2EQ4=iimid2EQ4f*10;;

mid3EQ4f = filtfilt(b,1,mid3EQ4);
imid3EQ4=cumtrapz(s4/fs,mid3EQ4f);
imid3EQ4f=filtfilt(b,1,imid3EQ4);
iimid3EQ4=cumtrapz(s4/fs,imid3EQ4f);
iimid3EQ4f=filtfilt(b,1,iimid3EQ4);
sdispmid3EQ4=iimid3EQ4f*10;;

low1EQ4f = filtfilt(b,1,low1EQ4);
ilow1EQ4=cumtrapz(s4/fs,low1EQ4f);
ilow1EQ4f=filtfilt(b,1,ilow1EQ4);
iilow1EQ4=cumtrapz(s4/fs,ilow1EQ4f);
iilow1EQ4f=filtfilt(b,1,iilow1EQ4);
sdisplow1EQ4=iilow1EQ4f*10;;

low2EQ4f = filtfilt(b,1,low2EQ4);
ilow2EQ4=cumtrapz(s4/fs,low2EQ4f);
ilow2EQ4f=filtfilt(b,1,ilow2EQ4);
iilow2EQ4=cumtrapz(s4/fs,ilow2EQ4f);
iilow2EQ4f=filtfilt(b,1,iilow2EQ4);
sdisplow2EQ4=iilow2EQ4f*10;;

low3EQ4f = filtfilt(b,1,low2EQ4);
ilow3EQ4=cumtrapz(s4/fs,low2EQ4f);
ilow3EQ4f=filtfilt(b,1,ilow2EQ4);
iilow3EQ4=cumtrapz(s4/fs,ilow2EQ4f);
iilow3EQ4f=filtfilt(b,1,iilow2EQ4);
sdisplow3EQ4=iilow3EQ4f*10;


%%%%%%%%%%7th
figure
title('plot of Acceleration and GPS data on the 5th floor');

s22=1:length(top1EQ4);
plot(s22/240,-sdisptop1EQ4,'r');
hold on
eq4NAV3time=eq4NAV3(:,1)*3600+eq4NAV3(:,2)*60+eq4NAV3(:,3);
eq4NAV3disp=eq4NAV3(:,4);
eq4NAV3disp=eq4NAV3disp-mean(eq4NAV3disp(1:100));
eq4NAV3time=eq4NAV3time-eq4NAV3time(1);

shift3=45.1;
yy=eq4NAV3disp(shift3*50:length(eq4NAV3disp))-mean(eq4NAV3disp(shift3*50:(shift3+5)*50));

plot(eq4NAV3time(shift3*50:length(eq4NAV3time))-eq4NAV3time(shift3*50),yy-mean(yy(1*10:5*50)),'b')


%%%%%%%kalman%%%%%%%%%%%%%%%

accel=resample(top1EQ4-mean(top1EQ4(1:5*240)),250,240);
accel=accel(1:100*250);
GPS=yy-mean(yy(1*50:5*50));
GPS=GPS(1:50*100);

time2=length(accel);


q=1;
r=5*10^-5;


Ta=1/250;
Td=1/10;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];

K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,5) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/5+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);

tl=1:length(sl);

figure
plot(tl/250,sl,'r.')
hold on

plot(eq4NAV3time(shift3*50:length(eq4NAV3time))-eq4NAV3time(shift3*50),yy-mean(yy(1:5*50)),'b')

%%%RMS%%%
GPSeq4NAV3=yy-mean(yy(1:5*50));
Kalmaneq4TOP=sl;

for i=1:100*50
    ers(i)=GPSeq4NAV3(i)-Kalmaneq4TOP(1+round((i-1)*1/50*250));
end
rms=sqrt(sum(ers.^2)/(i))

ersy=1:i;
figure
plot(ersy/10,ers(1:i))


%%%%%%%%%%5th%%%%%%%%%%%
figure
title('plot of Acceleration and GPS data on the 5th floor');

s22=1:length(mid1EQ4);
plot(s22/240,-sdispmid1EQ4,'r');
hold on
eq4NAV6time=eq4NAV6(:,1)*3600+eq4NAV6(:,2)*60+eq4NAV6(:,3);
eq4NAV6disp=eq4NAV6(:,4);
eq4NAV6disp=eq4NAV6disp-mean(eq4NAV6disp(1:100));
eq4NAV6time=eq4NAV6time-eq4NAV6time(1);

shift3=45.1;
yy=eq4NAV6disp(shift3*50:length(eq4NAV6disp))-mean(eq4NAV6disp(shift3*50:(shift3+5)*50));

plot(eq4NAV6time(shift3*50:length(eq4NAV6time))-eq4NAV6time(shift3*50),yy-mean(yy(1*10:5*50)),'b')


%%%%%%%kalman%%%%%%%%%%%%%%%

accel=resample(mid1EQ4-mean(mid1EQ4(1:5*240)),250,240);
accel=accel(1:100*250);
GPS=yy-mean(yy(1*50:5*50));
GPS=GPS(1:50*100);

time2=length(accel);

q=1;
r=5*10^-5;


Ta=1/250;
Td=1/10;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];

K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,5) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/5+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);

tl=1:length(sl);
s2l=xmeasure(2,1:k);
t2=1:length(s2l);
figure
plot(tl/250,sl,'r.')
hold on

plot(eq4NAV6time(shift3*50:length(eq4NAV6time))-eq4NAV6time(shift3*50),yy-mean(yy(1:5*50)),'b')

%%%RMS%%%
GPSeq4NAV6=yy-mean(yy(1:5*50));
Kalmaneq4MID=sl;

for i=1:100*50
    ers(i)=GPSeq4NAV6(i)-Kalmaneq4MID(1+round((i-1)*1/50*250));
end
rms=sqrt(sum(ers.^2)/(i))

ersy=1:i;
figure
plot(ersy/10,ers(1:i))


%%%%%3rd floor%%%%%%%

figure
title('plot of Acceleration and GPS data on the 3rd floor');

s22=1:length(low1EQ4);
plot(s22/240,-sdisplow1EQ4,'r');
hold on
eq4NAV7time=eq4NAV7(:,1)*3600+eq4NAV7(:,2)*60+eq4NAV7(:,3);
eq4NAV7disp=eq4NAV7(:,4);
eq4NAV7disp=eq4NAV7disp-mean(eq4NAV7disp(1:100));
eq4NAV7time=eq4NAV7time-eq4NAV7time(1);

shift3=45.1;
yy=eq4NAV7disp(shift3*50:length(eq4NAV7disp))-mean(eq4NAV7disp(shift3*50:(shift3+5)*50));

plot(eq4NAV7time(shift3*50:length(eq4NAV7time))-eq4NAV7time(shift3*50),yy-mean(yy(1*10:5*50)),'b')


%%%%%%%kalman%%%%%%%%%%%%%%%

accel=resample(low1EQ4-mean(low1EQ4(1:5*240)),250,240);
accel=accel(1:100*250);
GPS=yy-mean(yy(1*50:5*50));
GPS=GPS(1:50*100);

time2=length(accel);


q=1;
r=5*10^-5;


Ta=1/250;
Td=1/10;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];

K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,5) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/5+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);

tl=1:length(sl);
s2l=xmeasure(2,1:k);
t2=1:length(s2l);
figure
plot(tl/250,sl,'r.')
hold on

plot(eq4NAV7time(shift3*50:length(eq4NAV7time))-eq4NAV7time(shift3*50),yy-mean(yy(1:5*50)),'b')

%%%RMS%%%
GPSeq4NAV7=yy-mean(yy(1:5*50));
Kalmaneq4LOW=sl;

for i=1:100*50
    ers(i)=GPSeq4NAV7(i)-Kalmaneq4LOW(1+round((i-1)*1/50*250));
end
rms=sqrt(sum(ers.^2)/(i))

ersy=1:i;
figure
plot(ersy/10,ers(1:i))


%%%BASE%%%%
s1=1:length(top1EQ4);
s11=1:length(platen1EQ4);
s12=1:length(platen2EQ4);
%acceleration to displacement

platen1EQ4f=filtfilt(b,1,platen1EQ4);
iplaten1EQ4=cumtrapz(s11/fs,platen1EQ4f);
iplaten1EQ4f=filtfilt(b,1,iplaten1EQ4);
iiplaten1EQ4=cumtrapz(s11/fs,iplaten1EQ4);
iiplaten1EQ4f=filtfilt(b,1,iiplaten1EQ4);
sdispplaten1EQ4=iiplaten1EQ4f*10;


platen2EQ4f=filtfilt(b,1,platen2EQ4);
iplaten2EQ4=cumtrapz(s12/fs,platen2EQ4f);
iplaten2EQ4f=filtfilt(b,1,iplaten2EQ4);
iiplaten2EQ4=cumtrapz(s12/fs,iplaten2EQ4f);
iiplaten2EQ4f=filtfilt(b,1,iiplaten2EQ4);
sdispplaten2EQ4=iiplaten1EQ4f*10;

figure

title('plot of Acceleration and GPS data on the platen');
shift5=45.1;
plot(s11/240,-sdispplaten1EQ4,'r');
hold on

eq4NAV5GPS=eq4NAV5(:,4);
eq4NAV5time=eq4NAV5(:,1)*3600+eq4NAV5(:,2)*60+eq4NAV5(:,3);
eq4NAV5disp=eq4NAV5GPS-mean(eq4NAV5GPS(1000:2500));
time1=eq4NAV5time-eq4NAV5time(1);
plot(time1(shift5*50:length(time1))-time1(shift5*50),eq4NAV5disp(shift5*50:length(eq4NAV5disp)),'b')






%%%%RMS%%%%
%    for i=1:135*50
%        er(i)=(eq2NAV3disp(i)-accdisp2(1+round((i-1)*1/50*240)));
%    end
%    rms=sqrt(sum(er.^2)/(i))

%figure
%ery=1:i;
%plot(ery/50,er)

%%%%%%%%%%%%%%%%%Kalman%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

accel=resample(platen1EQ4-mean(platen1EQ4(1:5*240)),250,240);
accel=accel(1:100*250);
GPS=eq4NAV5disp(shift5*50:length(eq4NAV5disp));
GPS=GPS(1:50*100);

time2=length(accel);

q=1;
r=10^-6;


Ta=1/250;
Td=1/50;
xx(2,1:time2)=0;
xmeasure(2,1:time2-1)=0;

p(2,1:2*time2)=0;
p(1:2,1:2)=[1 0;0 1];

K(2,1:time2-1)=0;
Ad=[1 Ta;0 1];
Bd=[Ta^2/2;Ta];
Qd=[q*Ta^3/3 q*Ta^2/2;q*Ta^2/2 q*Ta];
Rd=r/Ta;
H=[1 0];
vel=diff(GPS);
xx(1:2,1)=[0;0];
z(2,1:length(vel))=0;
for n=1:length(vel)
    z(1,n)=GPS(n);
    z(2,n)=vel(n);
end
zz=H*z;
u=-accel*10;
for k=1:time2-1
    xx(1:2,k+1)=Ad*xx(1:2,k)+Bd*u(k);
    p(1:2,2*k+1:2*(k+1))=Ad*p(1:2,2*(k-1)+1:2*k)*Ad'+Qd;
    if mod(k-1,5) == 0
       K(1:2,k)=p(1:2,2*k+1:2*(k+1))*H'/[H*p(1:2,2*k+1:2*(k+1))*H'+Rd];
       xx(1:2,k+1)=xx(1:2,k+1)+K(1:2,k)*[zz((k-1)/5+1)-H*xx(1:2,k+1)];
       p(1:2,2*k+1:2*(k+1))=[eye(2)-K(1:2,k)*H]*p(1:2,2*k+1:2*(k+1));
    end
    xmeasure(1:2,k)=xx(1:2,k+1);
end

sl=xmeasure(1,1:k);
tl=1:length(sl);

figure
tl=tl/250;
plot(tl,sl,'r.')

hold on


plot(time1(shift5*50:length(time1))-time1(shift5*50),eq4NAV5disp(shift5*50:length(eq4NAV5disp)),'b')

hold on
M4=1:length(MTSEQ4disp);
MTSdispEQ4=MTSEQ4disp-mean(MTSEQ4disp(1:5000));
M4time=M4/1024;
Mshift=0.0;
plot(M4time(Mshift*1024+1:length(MTSdispEQ4))-M4time(Mshift*1024+1),MTSdispEQ4(Mshift*1024+1:length(MTSdispEQ4)),'k');


GPSeq4NAV5=eq4NAV5disp((shift5)*50:length(eq4NAV5disp));
Kalmaneq4BASE=sl;
MTSeq4=MTSdispEQ4(Mshift*1024+1:length(MTSdispEQ4));
%%RMS%%%
for i=1:100*50
    ers(i)=GPSeq4NAV5(i)-MTSeq4(1+round((i-1)*1/50*1024));
end
rms=sqrt(sum(ers.^2)/(i))
%ersy=1:i;
%figure
%plot(ersy/50,ers)

for i=1:100*250-1
    ers2(i)=Kalmaneq4BASE(i)-MTSeq4(1+round((i-1)*1/250*1024));
end
rms2=sqrt(sum(ers2.^2)/(i))

