diff --git a/PostProcess_Matlab/LongRunInit.m b/PostProcess_Matlab/LongRunInit.m index 3608e65..be79efc 100644 --- a/PostProcess_Matlab/LongRunInit.m +++ b/PostProcess_Matlab/LongRunInit.m @@ -95,7 +95,8 @@ zeroUptCount=0; odomUptCount=1; %% updates -odomUpdate=true; +odomUpdate=false; zeroUpdate=true; nonHolo=true; -backProp=true; +backProp=false; +gpsResults=false; diff --git a/PostProcess_Matlab/MAINRun.asv b/PostProcess_Matlab/MAINRun.asv new file mode 100644 index 0000000..cab83bb --- /dev/null +++ b/PostProcess_Matlab/MAINRun.asv @@ -0,0 +1,199 @@ +%% Differential Wheel Rover Low Level Navigation post processing script +% Authors: Cagri Kilic and Jason Gross +% Date: September 2018 +%% ----------------------------------------------------------------------- +clc; +clear; +load LongRunData.mat % LshapeData.mat %ForwardDrive.mat +%% odometry calculations +odom +%% initialize variables +LongRunInit % LshapeInit.mat %ForwardDriveInit.mat +%% Double Low Pass Filter +% dema +for i=2:L + dtIMU=tTimuAdis(i)-tTimuAdis(i-1); % calculates imu delta time from rostomat output tTimu + TimeIMU(i)=dtIMU+TimeIMU(i-1); % creates imu time from delta time starting from zero + %% ------------------------------------------------------------------- + %% IMU mounting orientation for BadgerRover + omega_b_ib=[-Gy(i),Gx(i),Gz(i)]-bg(:,i-1)'; %rad/s IMU gyro outputs minus estimated gyro bias + f_ib_b = [-Ay(i),Ax(i),Az(i)]-ba(:,i-1)'; %m/s^2 IMU acceleration minus estimated acce bias + v_ib_b= f_ib_b* dtIMU; %m/s acceleration times delta IMU = velocity + %% -------------------------------------------------------------------- + %% Attitude Update + [insAttPlus, Cb2nPlus,Cb2nMinus,Omega_n_en,Omega_n_ie,R_N,R_E,omega_n_in,omega_n_ie] = AttUpdate(insAtt(:,i-1),omega_ie,insLLH(:,i-1),omega_b_ib,ecc,Ro,insVel(:,i-1),dtIMU); + insAtt(:,i)=insAttPlus; + %% Velocity Update + [insVelPlus] = VelUpdate(Cb2nMinus, Cb2nPlus, v_ib_b,insVel(:,i-1),insLLH(:,i-1),omega_ie,Ro,ecc,dtIMU); + insVel(:,i)=insVelPlus; + %% Position Update + [insLLHPlus,R_EPlus,r_eb_e] = PosUpdate(insLLH(:,i-1),insVel(:,i),insVel(:,i-1),Ro,ecc,dtIMU); + insLLH(:,i)=insLLHPlus; + insXYZ(:,i)=r_eb_e; + %% -------------------------------------------------------------------- + %% Error State Model--eq 14.63 + [STM] = insErrorStateModel_LNF(R_EPlus,R_N,insLLH(:,i),insVel(:,i),dtIMU,Cb2nPlus,omega_ie,omega_n_in,f_ib_b); + %% Propagation of the Error + x_err=STM*x_err; + %% Q matrix -- + F21= -skewsymm(Cb2nPlus*(f_ib_b')); + Q=getQins(F21,Cb2nPlus,insLLH(:,i),R_N,R_E,dtIMU); + %% P matrix + P = STM*P*STM' + Q; + if odomUpdate() + %% -------------------------------------------------------------------- + Cn2bPlus=Cb2nPlus'; + omega_b_ie=Cn2bPlus*omega_n_ie; + omega_b_ei=-omega_b_ie; + omega_b_eb=omega_b_ei+omega_b_ib'; + %% Measurement Matrix -- integration part for INS -- eq 16.48 + H11= H11+ [1,0,0]*Cn2bPlus*skewsymm(insVel(:,i))*dtIMU; + H12= H12+ [1,0,0]*Cn2bPlus*dtIMU; + H21= H21+ sin(insAtt(2,i))*[0,cos(insAtt(1,i)),sin(insAtt(1,i))]*Cn2bPlus*dtIMU; + H31= H31+ [0,1,0]*Cn2bPlus*skewsymm(insVel(:,i))*dtIMU; + H32= H32+ [0,1,0]*Cn2bPlus*dtIMU; + H41= H41+ [0,0,1]*Cn2bPlus*skewsymm(insVel(:,i))*dtIMU; + H42= H42+ [0,0,1]*Cn2bPlus*dtIMU; + %% Measurement Innovation -- integration part for INS -- eq 16.42 + z11=z11+[1,0,0]*(Cn2bPlus*insVel(:,i) + skewsymm(omega_b_eb)*[-A,0,0]')*dtIMU; + z21=z21+cos(insAtt(2,i))*dtIMU; + z31=z31+[0,1,0]*(Cn2bPlus*insVel(:,i) + skewsymm(omega_b_eb)*[-A,0,0]')*dtIMU; + z41=z41+[0,0,1]*(Cn2bPlus*insVel(:,i) + skewsymm(omega_b_eb)*[-A,0,0]')*dtIMU; + % z and H values are integrated for IMU only above. When the Odometry + % update is available (tTimu(i)>=tTodom(kk)) integrated values will be + % updated by odometry measurements, used in the filter and destroyed. + end + if nonHolo() + [insAtt(:,i),insVel(:,i),insLLH(:,i),x_err,P]= nonHolonomic(insVel(:,i),insAtt(:,i),insLLH(:,i),x_err,P,omega_n_ie,omega_b_ib,A); + end + %% -------------------------------------------------------------------- + xState{1,i}=x_err; + PStore{1,i}=P; + STMStore{1,i}=STM+eye(15).*x_err; + if kk=tTodom(kk) % Odometry update is available + bb(counter)=i; % counts for the number of `i` when the loop goes into odometry updates + dt_odom=tTodom(kk)-tTodom(kk-1); + %% ------------------------------------------------------------ + %% Odometry Update + if odomUpdate() + odomUptCount=odomUptCount+1; + + P_old=P; + insAtt_old= insAtt(:,i); + insVel_old= insVel(:,i); + insLLH_old= insLLH(:,i); + x_err_old=x_err; + [insAtt(:,i),insVel(:,i),insLLH(:,i),x_err,P,postFitOdom]= odomUpt(insVel(:,i),insAtt(:,i),insLLH(:,i),x_err,P,... + insAtt(:,i-1),insAtt(3,bb(counter-1)),dt_odom,rearVel(kk),headRate(kk),s_or,... + H11,H12,H21,H31,H32,H41,H42,z11,z21,z31,z41,T_r,... + sigma_or_L,sigma_or_R,sigma_cmc,s_delta_or); + odompostfit(:,odomUptCount)=postFitOdom.z-postFitOdom.H*x_err; + S=postFitOdom.H*P*postFitOdom.H'+[0.00045,0,0,0;0,0.1152,0,0;0,0,0.0025,0;0,0,0,0.0025]; + chisq(:,odomUptCount)=odompostfit(:,odomUptCount)'*inv(S)*odompostfit(:,odomUptCount); + mahala(:,odomUptCount)=sqrt(odompostfit(:,odomUptCount)'*inv(S)*odompostfit(:,odomUptCount)); + + if mahala(1,odomUptCount)>5 + + % insAtt(:,i)=insAtt_old; + % insVel(:,i)=insVel_old; + % insLLH(:,i)=insLLH_old; + % P=P_old; + % x_err=x_err_old; + LLHcorrected2(:,cttr2)=insLLH(:,i); + cttr2=cttr2+1; + end + slipR(:,odomUptCount)=(rearVel(odomUptCount)-sqrt(insVel(1,i)^2+insVel(2,i)^2))/rearVel(odomUptCount); + if slipR(:,odomUptCount) < -50 + slipR (:,odomUptCount) = 0; + end + if abs(slipR(1,odomUptCount))>0.25 + LLHcorrected1(:,cttr0)=insLLH(:,i); + cttr0=cttr0+1; + end + end % odom update + % % % ------------------------------------------------------------ + % destroy H and z values for the next values + H11=zeros(1,3); + H12=zeros(1,3); + H21=zeros(1,3); + H31=zeros(1,3); + H32=zeros(1,3); + H24=zeros(1,3); + H41=zeros(1,3); + H42=zeros(1,3); + z11=0; + z21=0; + z31=0; + z41=0; + counter=counter+1; + kk=kk+1; + end % odom update not available + %% ---------------------------------------------------------------- + + if abs(lin_x(kk))<0.04% triggers zupt + zeroUptCount=zeroUptCount+1; + if zeroUpdate() + [insVel(:,i),insAtt(:,i),insLLH(:,i),x_err,P,postFitZero] = zeroUpd(insVel(:,i),insAtt(:,i),insLLH(:,i),x_err,P,omega_b_ib); + end + zCtr(i)=cttr3+offsetCtr; + LLHcorrected(:,cttr3)=insLLH(:,i); + cttr3=cttr3+1; + else + zCtr(i)=0; + offsetCtr=offsetCtr+1; + end % zero update not available + + if backProp() + if zCtr(i)-zCtr(i-1) < 0 % checks the zero update counter diff. If it is > 0 ZeroUpdate applied + doBackProp=false; + for j=i-1:-1:2 + if zCtr(j)-zCtr(j-1)<0 + doBackProp=true; + lastZindex=j; + break; + end + end + if doBackProp + x_err_s=xState{1,i}; + P_s=PStore{1,i}; + STM_s=STMStore{1,i}; + for dd=i:-1:lastZindex + [P_s,x_err_s] = smoothback(PStore{dd-1},PStore{dd},STMStore{dd-1},xState{dd-1},xState{dd},x_err_s,P_s,STM_s); + insVel(:,dd)=insVel(:,dd)-x_err_s(4:6); + insLLH(:,dd)=insLLH(:,dd)-x_err_s(7:9); + Cn2b_propBack= eulr2dcm(insAtt(:,dd)); + insAtt(:,dd)=dcm2eulr((eye(3)-skewsymm(x_err_s(1:3)))*Cn2b_propBack'); + ba(1:3,dd)=x_err_s(10:12); + bg(1:3,dd)=x_err_s(10:12); + end + else + ba(1:3,i)=x_err(10:12); % acce bias, this value will be removed from IMU acce output + bg(1:3,i)=x_err(13:15); % gyro bias, this value will be removed from IMU gyro output + end % doBackProp + end % if ZeroUpdate applied + else + end + end + + sig1(i)=3*sqrt(abs(P(1,1))); % 3 sigma values of att_x -roll + sig2(i)=3*sqrt(abs(P(2,2))); % 3 sigma values of att_y -pitch + sig3(i)=3*sqrt(abs(P(3,3))); % 3 sigma values of att_z -yaw + + sig4(i)=3*sqrt(abs(P(4,4))); % 3 sigma values of vel_x -forward + sig5(i)=3*sqrt(abs(P(5,5))); % 3 sigma values of vel_y -left + sig6(i)=3*sqrt(abs(P(6,6))); % 3 sigma values of vel_z -down + + sig7(i)=3*sqrt(abs(P(7,7))); % 3 sigma values of pos_x -latitude + sig8(i)=3*sqrt(abs(P(8,8))); % 3 sigma values of pos_y -longitude + sig9(i)=3*sqrt(abs(P(9,9))); % 3 sigma values of pos_z -height + gpsLonger(:,i)=[llhGPS(1,kk);llhGPS(2,kk);llhGPS(end,kk)]; + x_State(:,i)=[insAtt(:,i);insVel(:,i);insLLH(:,i);ba(:,i);bg(:,i)]; + Cn2b_corr= eulr2dcm(insAtt(:,i)); + insAttCorr(:,i)=dcm2eulr((eye(3)-skewsymm(x_err(1:3)))*Cn2b_corr'); + insVelCorr(:,i)=insVel(:,i)-x_err(4:6); + insLLHCorr(:,i)=insLLH(:,i)-x_err(7:9); +end +figureGeneration diff --git a/PostProcess_Matlab/MAINRun.m b/PostProcess_Matlab/MAINRun.m index 0499d41..9a3cc5c 100644 --- a/PostProcess_Matlab/MAINRun.m +++ b/PostProcess_Matlab/MAINRun.m @@ -70,14 +70,14 @@ xState{1,i}=x_err; PStore{1,i}=P; STMStore{1,i}=STM+eye(15).*x_err; - if kk=tTodom(kk) % Odometry update is available bb(counter)=i; % counts for the number of `i` when the loop goes into odometry updates dt_odom=tTodom(kk)-tTodom(kk-1); %% ------------------------------------------------------------ - % % Odometry Update + %% Odometry Update if odomUpdate() odomUptCount=odomUptCount+1; @@ -189,7 +189,9 @@ sig7(i)=3*sqrt(abs(P(7,7))); % 3 sigma values of pos_x -latitude sig8(i)=3*sqrt(abs(P(8,8))); % 3 sigma values of pos_y -longitude sig9(i)=3*sqrt(abs(P(9,9))); % 3 sigma values of pos_z -height + if gpsResults() gpsLonger(:,i)=[llhGPS(1,kk);llhGPS(2,kk);llhGPS(end,kk)]; + end x_State(:,i)=[insAtt(:,i);insVel(:,i);insLLH(:,i);ba(:,i);bg(:,i)]; Cn2b_corr= eulr2dcm(insAtt(:,i)); insAttCorr(:,i)=dcm2eulr((eye(3)-skewsymm(x_err(1:3)))*Cn2b_corr'); diff --git a/PostProcess_Matlab/figGen.m b/PostProcess_Matlab/figGen.m index 92812f5..02765ca 100644 --- a/PostProcess_Matlab/figGen.m +++ b/PostProcess_Matlab/figGen.m @@ -73,8 +73,10 @@ fill( [tTimu(1:end-1,1)'-tTimu(1) fliplr(tTimu(1:end-1,1)'-tTimu(1))], [ENU_3P(1,:) fliplr(ENU3P(1,:))], 'r','DisplayName','3\sigma Covariance Hull' ); hold on plot(tTimu(1:end-1,1)-tTimu(1),ENU(1,:),'-b','DisplayName','Filter Estimation East'); +if gpsResults() hold on; plot(tTodom-min(tTodom),ENUGPS(1,1:length(tTodom)),'-k','DisplayName','GPS Solution East'); +end % hold on; % plot(tTodom-min(tTodom),Pos_x-Pos_x(1),'-.g','DisplayName','odom') % hold on @@ -90,9 +92,10 @@ fill( [tTimu(1:end-1,1)'-tTimu(1) fliplr(tTimu(1:end-1,1)'-tTimu(1))], [ENU_3P(2,:) fliplr(ENU3P(2,:))], 'r','DisplayName','3\sigma Covariance Hull' ); hold on plot(tTimu(1:end-1,1)-tTimu(1),ENU(2,:),'-b','DisplayName','Filter Estimation North'); +if gpsResults() hold on; plot(tTodom-min(tTodom),ENUGPS(2,1:length(tTodom)),'-k','DisplayName','GPS Solution North'); - +end % plot(tTimu(1:end-1,1)-tTimu(1),ENU(2,:),'-.b','DisplayName','LLN'); % hold on; % plot(tTodom-min(tTodom),ENUGPS(2,1:length(tTodom)),'-.r','DisplayName','true') @@ -111,9 +114,10 @@ fill( [tTimu(1:end-1,1)'-tTimu(1) fliplr(tTimu(1:end-1,1)'-tTimu(1))], [ENU_3P(3,:) fliplr(ENU3P(3,:))], 'r','DisplayName','3\sigma Covariance Hull' ); hold on plot(tTimu(1:end-1,1)-tTimu(1),ENU(3,:),'-b','DisplayName','Filter Estimation Up'); +if gpsResults() hold on; plot(tTodom-min(tTodom),ENUGPS(3,1:length(tTodom)),'-k','DisplayName','GPS Solution Up'); - +end % plot(tTimu(1:end-1,1)-tTimu(1),ENU(3,:)-ENU(3,1),'-.b','DisplayName','LLN'); % hold on; % plot(tTodom-min(tTodom),ENUGPS(3,1:length(tTodom)),'-.r','DisplayName','true') @@ -183,15 +187,17 @@ % legend('show'); h(6)=figure; -plot(ENU(1,:),ENU(2,:),'.--r','DisplayName','INS') +plot(ENU(1,:),ENU(2,:),'.--r','DisplayName','CoreNav') hold on -plot(ENU(1,1),ENU(2,1),'o','DisplayName','INS Start') +plot(ENU(1,1),ENU(2,1),'o','DisplayName','CoreNav Start') hold on -plot(ENU(1,end),ENU(2,end),'*','DisplayName','INS End') +plot(ENU(1,end),ENU(2,end),'*','DisplayName','CoreNav End') +if gpsResults() hold on -plot(ENUGPS(1,:),ENUGPS(2,:),'.-k','DisplayName','Truth') +plot(ENUGPS(1,:),ENUGPS(2,:),'.-k','DisplayName','GPS ENU') +end hold on -plot(ENUcorrected(1,:),ENUcorrected(2,:),'--og','DisplayName','LLH') +plot(ENUcorrected(1,:),ENUcorrected(2,:),'--og','DisplayName','CoreNav') if odomUpdate hold on @@ -199,15 +205,17 @@ hold on plot(ENUcorrected2(1,:),ENUcorrected2(2,:),'*c','DisplayName','Mahalanobis') end +if gpsResults hold on plot(ENUGPS(1),ENUGPS(2),'o','DisplayName','Truth Start') hold on plot(ENUGPS(1,end),ENUGPS(2,end),'*','DisplayName','Truth End') +end xlabel('E(m) ') ylabel('N(m) ') legend('show'); - +if gpsResults() h(7)=figure; subplot(311),plot(tTimu(1:end-1,1)-tTimu(1),gpsLongerXYZ(1,:)-xyz(1,:),'DisplayName','x_{err}') ylabel('x (m)') @@ -218,7 +226,7 @@ subplot(313),plot(tTimu(1:end-1,1)-tTimu(1),gpsLongerXYZ(3,:)-xyz(3,:),'DisplayName','z_{err}') ylabel('z (m)') xlabel('time (sec)') - +end h(8)=figure; hold off @@ -239,12 +247,14 @@ end plot3(ENU(1,:),ENU(2,:),ENU(3,:),'.r','DisplayName','Filter Estimate') +if gpsResults hold on % plot3(ENU3P(1,:),ENU3P(2,:),ENU3P(3,:),'--k','DisplayName','Covariance Ellipse') % hold on % plot3(ENU_3P(1,:),ENU_3P(2,:),ENU_3P(3,:),'--k') % hold on plot3(ENUGPS(1,:),ENUGPS(2,:),ENUGPS(3,:),'.-k','DisplayName','GPS Solution') +end grid on xlabel('East (m)') ylabel('North (m)') diff --git a/PostProcess_Matlab/figureGeneration.m b/PostProcess_Matlab/figureGeneration.m index 9bbc05e..0b8b410 100644 --- a/PostProcess_Matlab/figureGeneration.m +++ b/PostProcess_Matlab/figureGeneration.m @@ -1,6 +1,8 @@ for ss=1:length(insLLH) xyz(:,ss)=llh2xyz(insLLH(:,ss)); + if gpsResults() gpsLongerXYZ(:,ss)=llh2xyz(gpsLonger(:,ss)); + end xyz_3P(:,ss)=llh2xyz([insLLH(1,ss)-sig7(ss);insLLH(2,ss)-sig8(ss);insLLH(3,ss)-sig9(ss)]); xyz3P(:,ss)=llh2xyz([insLLH(1,ss)+sig7(ss);insLLH(2,ss)+sig8(ss);insLLH(3,ss)+sig9(ss)]); ENU(:,ss)=xyz2enu(xyz(:,ss),xyzInit); @@ -33,18 +35,19 @@ ENUcorrected1(:,ss4)=xyz2enu(xyzCorrected1(:,ss4),xyzInit); end end +if gpsResults() gpsLongerXYZ(:,1)=xyzInit; RMSE_east=sqrt(mean((xyz(1,:)-gpsLongerXYZ(1,:)).^2)); %R Mean Squared Error RMSE_north=sqrt(mean((xyz(2,:)-gpsLongerXYZ(2,:)).^2)); %R Mean Squared Error RMSE_up=sqrt(mean((xyz(3,:)-gpsLongerXYZ(3,:)).^2)); %R Mean Squared Error - +end %% xy_err=ENU33P(1:2,:)-ENU3(1:2,:); for i=1:length(xy_err) dPx(1,i)=sqrt(xy_err(1:2,i)'*xy_err(1:2,i)); end %% - +if gpsResults errXY=xyz(1:2,:)-gpsLongerXYZ(1:2,:); for i=1:length(errXY) dPx2(1,i)=sqrt(errXY(1:2,i)'*errXY(1:2,i)); @@ -53,10 +56,11 @@ medHor=median(dPx2) stdHor=std(dPx2) maxHor=max(dPx2) + RMSE_e=sqrt(mean((xyz(1,:)-gpsLongerXYZ(1,:)).^2)) %R Mean Squared Error RMSE_n=sqrt(mean((xyz(2,:)-gpsLongerXYZ(2,:)).^2)) %R Mean Squared Error RMSE_u=sqrt(mean((xyz(3,:)-gpsLongerXYZ(3,:)).^2)) %R Mean Squared Error - +end figGen % hold off % ellipsegen diff --git a/PostProcess_Matlab/odom.m b/PostProcess_Matlab/odom.m index d04539e..ae2d738 100644 --- a/PostProcess_Matlab/odom.m +++ b/PostProcess_Matlab/odom.m @@ -1,37 +1,3 @@ -%odom -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% -% wheel_radius=0.1651;%meters wheelbase=0.555; %meters i=1;