% Author: Tomas Trafina, Bc., system control/robotics field % Czech Technical University in Prague, Dept. of Control Engineering % email address: ttrafina@gmail.com % May 2018; Last revision: 17-May-2018 % run system_init.m (and imu_data.m) before execution of this script %------------- BEGIN CODE -------------- %% time constants filter.TSm=0.002; % time step of the prection step filter.t0=95.0; % start time of the working interval filter.t1=293.0; % end time of the working interval % Uncomment the following two lines if real measurements are used %sensor1 = sensorImu; %sensor2 = sensorRtk; %% static parameters for filter using RPY and NED measurements % filter.n=12; % number of states % syms Ts % F=eye(filter.n) + [zeros(6,filter.n); % transition matrix % eye(filter.n-6)*Ts zeros(filter.n-6,6)]; % Qc=[diag([100,100,10,250,100,100]./filter.TSm) zeros(6,filter.n-6); zeros(filter.n-6,filter.n)]; % process noise representation % Qtmp=F*Qc*transpose(F); % filter.Q=int(Qtmp,Ts); % % Ts=filter.TSm; % filter.Ad=double(subs(F)); % filter.Q=double(subs(filter.Q)); % process covariance matrix % clear F Qc Qtmp Ts % % filter.H1=[zeros(3,6) eye(3) zeros(3)]; % outputs mapping of RPY angles % filter.H2=[zeros(3,9) eye(3)]; % outputs mapping of NED position % filter.R1=sensor.Rimu(7:9,7:9); % filter.R2=sensor.Rrtk; % % meas1 = sensor1.signals.values(:,7:9); % use only RPY measurement from IMU %% static parameters for filter using ang. acc., lin. acc., RPY and NED measurements filter.n=18; % number of system's states syms Ts F=eye(filter.n) + [zeros(6,filter.n); % transition matrix eye(filter.n-6)*Ts zeros(filter.n-6,6)]+[zeros(filter.n-6,filter.n); eye(6)*0.5*(Ts^2) zeros(6,filter.n-6)]; Qc=[diag([100,100,10,250,100,100]) zeros(6,filter.n-6); zeros(filter.n-6,filter.n)]; % process noise representation Qtmp=F*Qc*transpose(F); filter.Q=int(Qtmp,Ts) Ts=filter.TSm; filter.Ad=double(subs(F)); filter.Q=double(subs(filter.Q)); % process covariance matrix clear F Qc Qtmp Ts Haa=[eye(3) zeros(3,15)]; % outputs mapping of angular acceleration Hla=[zeros(3) eye(3) zeros(3,12)]; % outputs mapping of linear acceleration Hrpy=[zeros(3,12) eye(3) zeros(3)]; % outputs mapping of RPY angles Hned=[zeros(3,15) eye(3)]; % outputs mapping of NED position filter.H1=[Haa; Hla; Hrpy]; % output mapping of IMU filter.H2=Hned; % output mapping of GNSS sensor clear Haa Hla Hrpy Hned filter.R1=sensor.Rimu; filter.R2=sensor.Rrtk; meas1 = sensor1.signals.values; % use all available measurements from IMU %% filtering process est.signals.rpy=zeros(3,(filter.t1-filter.t0)/filter.TSm+1); est.signals.ned=zeros(3,(filter.t1-filter.t0)/filter.TSm+1); est.time=zeros(1,(filter.t1-filter.t0)/filter.TSm+1); t=filter.t0; % initial time x=zeros(filter.n,1); % initial guess of state vector P=eye(filter.n); % initial guess of error covariance matrix ID1=1; % iterator over IMU measurements ID2=1; % iterator over GNSS sensor measurements % get index for each sensors measurements first after the the start time while 1 if(sensor1.time(ID1)) <= t ID1 = ID1+1; end if(sensor2.time(ID2)) <= t ID2 = ID2+1; end if((sensor1.time(ID1))>t) && (sensor2.time(ID2)>t) break end end % use first measurement for initial states estimate x=x+transpose(filter.H1)*transpose(meas1(ID1,:))+transpose(filter.H2)*transpose(sensor2.signals.values(ID2,:)); est.signals.rpy(:,1)=x(filter.n-5:filter.n-3); % first estimated values of RPY angles est.signals.ned(:,1)=x(filter.n-2:filter.n); % first estimated values of NED position est.time(1)=t; % time of the first estimate est.idx=2; while 1 if ID1 >= length(sensor1.time) || ID2 >= length(sensor2.time) break; end if t >= filter.t1 break; end t=t+filter.TSm; % increment time of the prediction x=filter.Ad*x; % predict next state P=filter.Ad*P*transpose(filter.Ad)+filter.Q; % predict estimate covariance if t >= sensor1.time(ID1) || t >= sensor2.time(ID2) % if some measurement arrived if t >= sensor1.time(ID1) && t >= sensor2.time(ID2) % if both sensors have measurement z=[transpose(meas1(ID1,:)); transpose(sensor2.signals.values(ID2,:))]; H=[filter.H1; filter.H2]; R=blkdiag(filter.R1,filter.R2); ID1=ID1+1; ID2=ID2+1; elseif t >= sensor1.time(ID1) % if only IMU mesurement is available z=transpose(meas1(ID1,:)); H=filter.H1; R=filter.R1; ID1=ID1+1; elseif t >= sensor2.time(ID2) % if only GPS mesurement is available z=transpose(sensor2.signals.values(ID2,:)); H=filter.H2; R=filter.R2; ID2=ID2+1; end y=z-H*x; % calculate innovation S=H*P*transpose(H)+R; % innovation covariance K=(P*transpose(H))/S; % optimal Kalman gain x=x+K*y; % update state estimate P=P-(K*H*P); % update estimate covariance end est.signals.rpy(:,est.idx)=x(filter.n-5:filter.n-3); est.signals.ned(:,est.idx)=x(filter.n-2:filter.n); est.time(:,est.idx)=t; est.idx=est.idx+1; end clear t x P ID1 ID2 z H R y S K %% real application plotting figure ha(1)=subplot(2,2,1); tmp = interp1(sensor1.time,sensor1.signals.values, est.time); rpyDiff = abs(est.signals.rpy - transpose(tmp(:,7:9))); plot(est.time,est.signals.rpy(3,:),'b', est.time,est.signals.rpy(2,:),'g', est.time,est.signals.rpy(1,:),'r') hold on plot(est.time,tmp(:,7),'k--', est.time,tmp(:,8),'k--', est.time,tmp(:,9),'k--') hold off xlim([filter.t0 filter.t1]) grid on title('Euler angles') xlabel('time [s]') ylabel('angle [rad]') legend('filtered yaw', 'filtered pitch', 'filtered roll', 'measured values') ha(2)=subplot(2,2,3); plot(est.time,rpyDiff(3,:),'b', est.time,rpyDiff(2,:),'g', est.time,rpyDiff(1,:),'r') xlim([filter.t0 filter.t1]) grid on title('Differences between measured and filtered values (absolute)') xlabel('time [s]') ylabel('angle [rad]') legend('yaw', 'pitch', 'roll') ha(3)=subplot(2,2,2); tmp = interp1(sensor2.time,sensor2.signals.values, est.time); nedDiff = abs(est.signals.ned - transpose(tmp)); plot(est.time,est.signals.ned(3,:),'b', est.time,est.signals.ned(2,:),'g', est.time,est.signals.ned(1,:),'r') hold on plot(est.time,tmp(:,1),'k--', est.time,tmp(:,2),'k--', est.time,tmp(:,3),'k--') hold off xlim([filter.t0 filter.t1]) grid on title('NED position') xlabel('time [s]') ylabel('distance [m]') legend('filtered down', 'filtered east', 'filtered north', 'measured values') ha(4)=subplot(2,2,4); plot(est.time,nedDiff(3,:),'b', est.time,nedDiff(2,:),'g', est.time,nedDiff(1,:),'r') xlim([filter.t0 filter.t1]) grid on title('Differences between measured and filtered values (absolute)') xlabel('time [s]') ylabel('distance [m]') legend('down', 'east', 'north') linkaxes(ha, 'x'); clear ha tmp %% simulation plotting % figure; % subplot(3,1,1) % p1 = plot(scope.time,scope.signals(6).values(:,1),'r', scope.time,scope.signals(2).values(:,1),'g', est.time ,est.signals.rpy(1,:),'b'); % p1(2).LineWidth=2; % p1(3).LineWidth=2; % grid on % title('Roll') % xlabel('time [s]') % ylabel('angle [rad]') % legend('measured value', 'real value', 'filtered value') % % subplot(3,1,2) % p1 = plot(scope.time,scope.signals(6).values(:,2),'r', scope.time,scope.signals(2).values(:,2),'g', est.time,est.signals.rpy(2,:),'b'); % p1(2).LineWidth=2; % p1(3).LineWidth=2; % grid on % title('Pitch') % xlabel('time [s]') % ylabel('angle [rad]') % legend('measured value', 'real value', 'filtered value') % % subplot(3,1,3) % p1 = plot(scope.time,scope.signals(6).values(:,3),'r', scope.time,scope.signals(2).values(:,3),'g', est.time,est.signals.rpy(3,:),'b'); % p1(2).LineWidth=2; % p1(3).LineWidth=2; % grid on % title('Yaw') % xlabel('time [s]') % ylabel('angle [rad]') % legend('measured value', 'real value', 'filtered value') % % figure; % subplot(3,1,1) % p1 = plot(scope.time,scope.signals(8).values(:,1),'r', scope.time,scope.signals(4).values(:,1),'g', est.time ,est.signals.ned(1,:),'b'); % p1(2).LineWidth=2; % p1(3).LineWidth=2; % grid on % title('North') % xlabel('time [s]') % ylabel('distance [m]') % legend('measured value', 'real value', 'filtered value') % % subplot(3,1,2) % p1 = plot(scope.time,scope.signals(8).values(:,2),'r', scope.time,scope.signals(4).values(:,2),'g', est.time,est.signals.ned(2,:),'b'); % p1(2).LineWidth=2; % p1(3).LineWidth=2; % grid on % title('East') % xlabel('time [s]') % ylabel('distance [m]') % legend('measured value', 'real value', 'filtered value') % % subplot(3,1,3) % p1 = plot(scope.time,scope.signals(8).values(:,3),'r', scope.time,scope.signals(4).values(:,3),'g', est.time,est.signals.ned(3,:),'b'); % p1(2).LineWidth=2; % p1(3).LineWidth=2; % grid on % title('Down') % xlabel('time [s]') % ylabel('distance [m]') % legend('measured value', 'real value', 'filtered value') % % clear realNum measNum out p1