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