├── functions ├── INS.m ├── GPS_IMU.m ├── ekf_p.m ├── Rphi_m.m ├── Rlambda_m.m ├── Psicalc.m ├── normalize_angle_f.m ├── ekf_u.m ├── euler2quat_f.m ├── rot.m ├── quat2euler_f.m ├── Omegacalc.m ├── Hquat_psi_GPS_f.m ├── subtr_ang.m ├── headingangle.m ├── sysquat_m.m ├── syspos_m.m └── sys_ati.m ├── EulerAccel.m ├── .gitattributes ├── GPS_data.m ├── IMU_data.m ├── README.md ├── .gitignore ├── EulerEKF.m ├── syspos4_m.m ├── sysEuler_f.m ├── plot_fig.m ├── EulerEKF_bg.m ├── main_INS.m └── malaga-urban-dataset-extract-04_all-sensors_GPS.txt /functions/INS.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GYengera/Inerital-Navigation-System/HEAD/functions/INS.m -------------------------------------------------------------------------------- /functions/GPS_IMU.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GYengera/Inerital-Navigation-System/HEAD/functions/GPS_IMU.m -------------------------------------------------------------------------------- /functions/ekf_p.m: -------------------------------------------------------------------------------- 1 | function [xp, Pp] = ekf_p (x,P,sys) 2 | Q = sys.Q; 3 | Phi = sys.Phi; 4 | xp = Phi*x; 5 | Pp = Phi*P*Phi' + Q; -------------------------------------------------------------------------------- /EulerAccel.m: -------------------------------------------------------------------------------- 1 | function [phi theta] = EulerAccel(ax, ay) 2 | % 3 | % 4 | g = 9.8; 5 | 6 | theta = asin( ax / g ); 7 | phi = asin( -ay / (g*cos(theta)) ); 8 | 9 | -------------------------------------------------------------------------------- /functions/Rphi_m.m: -------------------------------------------------------------------------------- 1 | function [Rphi] = Rphi_m(lambda); 2 | 3 | e = 0.0818; % excentricidade 4 | a = 6378137; % raio equatiorial m 5 | 6 | Rphi = a/((1-e^2*sin(lambda)^2)^0.5); -------------------------------------------------------------------------------- /functions/Rlambda_m.m: -------------------------------------------------------------------------------- 1 | function [Rlambda] = Rlambda_m(lambda); 2 | 3 | e = 0.0818; % excentricidade 4 | a = 6378137; % raio equatiorial m 5 | 6 | Rlambda = (a*(1-e^2))/((1-e^2*sin(lambda)^2)^1.5); -------------------------------------------------------------------------------- /functions/Psicalc.m: -------------------------------------------------------------------------------- 1 | function [Psi] = Psicalc(q) 2 | 3 | % [Psi] = Psicalc(q) 4 | % Psi = [ -q1 -q2 -q3; 5 | % q0 -q3 q2; 6 | % q3 q0 -q1; 7 | % -q2 q1 q0]; 8 | 9 | q0 = q(1); 10 | q1 = q(2); 11 | q2 = q(3); 12 | q3 = q(4); 13 | 14 | Psi = [ -q1 -q2 -q3; 15 | q0 -q3 q2; 16 | q3 q0 -q1; 17 | -q2 q1 q0]; 18 | -------------------------------------------------------------------------------- /functions/normalize_angle_f.m: -------------------------------------------------------------------------------- 1 | function [angle] = normalize_angle_f(r,low) 2 | % Compute the angle in the semi-closed interval [low, low + 2PI) that represents the same rotation as the angle r 3 | % r angle in radians 4 | % low starting value of the normalization interval 5 | % angle = equivalent angle in the interval [low, low + 2PI) computed as r - 2PI * FLOOR((r - low) / (2PI)) 6 | 7 | angle = r - 2*pi*floor((r - low)/(2*pi)); -------------------------------------------------------------------------------- /functions/ekf_u.m: -------------------------------------------------------------------------------- 1 | function [x,P] = ekf_u(xp, Pp, z, sys,opt) 2 | 3 | H = sys.H; 4 | R = sys.R; 5 | 6 | m = size(xp,1); 7 | epsilon = 10^(-1); % To prevent inversion of a singular matrix 8 | K = Pp*H'*inv(H*Pp*H' + R + epsilon*eye(size(R))); 9 | P = (eye(m) - K*H)*Pp; 10 | switch opt 11 | case 0 12 | x = xp + K*(z-H*xp); 13 | case 1 14 | x = xp + K*z; % z = deltaz; 15 | otherwise 16 | disp('Unknow method ekf'); 17 | end -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | 7 | # Standard to msysgit 8 | *.doc diff=astextplain 9 | *.DOC diff=astextplain 10 | *.docx diff=astextplain 11 | *.DOCX diff=astextplain 12 | *.dot diff=astextplain 13 | *.DOT diff=astextplain 14 | *.pdf diff=astextplain 15 | *.PDF diff=astextplain 16 | *.rtf diff=astextplain 17 | *.RTF diff=astextplain 18 | -------------------------------------------------------------------------------- /GPS_data.m: -------------------------------------------------------------------------------- 1 | %% Load GPS data 2 | gps_mod = load('malaga-urban-dataset-extract-04_all-sensors_GPS.txt'); 3 | % gps_select = [stamp latitude longitude altitude heading] 4 | gps_select = [gps_mod(:,1) gps_mod(:,2) gps_mod(:,3) gps_mod(:,4)];% gps_mod(:,8)]; 5 | n_gps = size(gps_select,1); 6 | 7 | stamp_gps = gps_select(:,1); 8 | 9 | lat_v = gps_select(:,2); 10 | 11 | lon_v = gps_select(:,3); 12 | 13 | alt_v = gps_select(:,4); 14 | 15 | %heading_v =gps_select(:,5); 16 | -------------------------------------------------------------------------------- /functions/euler2quat_f.m: -------------------------------------------------------------------------------- 1 | function [q] = euler2quat_f(eangles) 2 | 3 | psi = eangles.psi; 4 | theta = eangles.theta; 5 | phi = eangles.phi; 6 | 7 | q = [cos(phi/2)*cos(theta/2)*cos(psi/2) + sin(phi/2)*sin(theta/2)*sin(psi/2); 8 | sin(phi/2)*cos(theta/2)*cos(psi/2) - cos(phi/2)*sin(theta/2)*sin(psi/2); 9 | cos(phi/2)*sin(theta/2)*cos(psi/2) + sin(phi/2)*cos(theta/2)*sin(psi/2); 10 | cos(phi/2)*cos(theta/2)*sin(psi/2) - sin(phi/2)*sin(theta/2)*cos(psi/2)]; -------------------------------------------------------------------------------- /functions/rot.m: -------------------------------------------------------------------------------- 1 | function [R] = rot(q) 2 | 3 | q0 = q(1); 4 | q1 = q(2); 5 | q2 = q(3); 6 | q3 = q(4); 7 | 8 | R(1,1) = q0^2 + q1^2 - q2^2 - q3^2; 9 | R(2,1) = 2*(q1*q2 - q0*q3); 10 | R(3,1) = 2*(q1*q3 + q0*q2); 11 | 12 | R(1,2) = 2*(q1*q2 + q0*q3); 13 | R(2,2) = q0^2 - q1^2 + q2^2 - q3^2; 14 | R(3,2) = 2*(q2*q3 - q0*q1); 15 | 16 | R(1,3) = 2*(q1*q3 - q0*q2); 17 | R(2,3) = 2*(q2*q3 + q0*q1); 18 | R(3,3) = q0^2 - q1^2 - q2^2 + q3^2; %2*(q0*q0 + q3*q3) - 1;% 19 | 20 | 21 | -------------------------------------------------------------------------------- /IMU_data.m: -------------------------------------------------------------------------------- 1 | %% Load IMU data 2 | path(path,'functions') 3 | 4 | imu_mod = load('malaga-urban-dataset-extract-04_all-sensors_IMU.txt'); 5 | % imu_select = [stamp orientation_euler angular_velocity linear_acceleration ] 6 | imu_select = [imu_mod(:,1) imu_mod(:,11:13) imu_mod(:,5:7) imu_mod(:,2:4)]; 7 | n_imu = size(imu_select,1); 8 | 9 | stamp_imu = imu_select(:,1); 10 | 11 | e_imu_v = imu_select(:,2:4)'; 12 | 13 | ang_vel_v = imu_select(:,5:7)'; 14 | 15 | lin_accel_v = imu_select(:,8:10)'; 16 | 17 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Attitude and Position reference system using IMU + GPS data. 2 | 3 | Extract #4 from the Málaga Stereo and Laser Urban Data Set (http://www.mrpt.org/MalagaUrbanDataset#33_Binary_files) has been used. 4 | 5 | Run main_INS.m to obtain the results. 6 | 7 | References: 8 | 9 | J. A. Farrel, Aided navigation GPS with high rate sensors. New York: The McGRaw-Hill Companies, 2008. 10 | 11 | P. Kim, Kalman Filter for Beginners: with MATLAB Examples. CreateSpace Independent Publishing Platform, 2011. 12 | -------------------------------------------------------------------------------- /functions/quat2euler_f.m: -------------------------------------------------------------------------------- 1 | function [eangles] = quat2euler_f(q) 2 | 3 | q0 = q(1); 4 | q1 = q(2); 5 | q2 = q(3); 6 | q3 = q(4); 7 | 8 | 9 | eangles.psi = atan2((2*(q1*q2 + q0*q3)),(q0^2 + q1^2 - q2^2 - q3^2)); 10 | % sintheta = -2*(q1*q3 - q0*q2); 11 | % eangles.theta = atan2(sintheta,2*(q1*q2 + q0*q3)*(1/sin(eangles.psi))); 12 | eangles.theta = asin(-2*(q1*q3 - q0*q2)); 13 | % if norm(-2*(q1*q3 - q0*q2))>1 14 | % jj=0; 15 | % end 16 | eangles.phi = atan2((2*(q2*q3 + q0*q1)),(q0^2 - q1^2 - q2^2 + q3^2)); -------------------------------------------------------------------------------- /functions/Omegacalc.m: -------------------------------------------------------------------------------- 1 | function [Omega] = Omegacalc(omega) 2 | % [Omega] = Omegacalc(omega) 3 | % Omega = [0 -omega(1) -omega(2) -omega(3); 4 | % omega(1) 0 omega(3) -omega(2); 5 | % omega(2) -omega(3) 0 omega(1); 6 | % omega(3) omega(2) -omega(1) 0]; 7 | 8 | Omega = [0 -omega(1) -omega(2) -omega(3); 9 | omega(1) 0 omega(3) -omega(2); 10 | omega(2) -omega(3) 0 omega(1); 11 | omega(3) omega(2) -omega(1) 0]; -------------------------------------------------------------------------------- /functions/Hquat_psi_GPS_f.m: -------------------------------------------------------------------------------- 1 | function [H] = Hquat_psi_GPS_f(q, g,opt) 2 | 3 | 4 | q0 = q(1); 5 | q1 = q(2); 6 | q2 = q(3); 7 | q3 = q(4); 8 | 9 | r = rot(q); 10 | 11 | switch opt 12 | case 1 13 | H = [eye(4), zeros(4,3)]; 14 | 15 | case 0 16 | H = [eye(4), zeros(4,3)]; 17 | H(5,1) = (2*q3*r(1,1) - 2*q0*r(1,2))/(r(1,1)^2); 18 | H(5,2) = (2*q2*r(1,1) - 2*q1*r(1,2))/(r(1,1)^2); 19 | H(5,3) = (2*q1*r(1,1) + 2*q2*r(1,2))/(r(1,1)^2); 20 | H(5,4) = (2*q0*r(1,1) + 2*q3*r(1,2))/(r(1,1)^2); 21 | H(5,5:7) = zeros(1,3); 22 | end -------------------------------------------------------------------------------- /functions/subtr_ang.m: -------------------------------------------------------------------------------- 1 | function [dif] = subtr_ang(ang1,ang2) 2 | 3 | temp1 = ang1; 4 | % make theta in [0,2pi) 5 | if(temp1 >0) 6 | temp1 = temp1 - 2*pi*floor(temp1/(2*pi)); 7 | end 8 | if(temp1 <0) 9 | temp1 = 2*pi - (-temp1 - 2*pi*floor(-temp1/(2*pi))); 10 | end 11 | 12 | temp2 = ang2; 13 | % make theta in [0,2pi) 14 | if(temp2 > 0) 15 | temp2 = temp2 - 2*pi*floor(temp2/(2*pi)); 16 | end 17 | if(temp2 < 0) 18 | temp2 = 2*pi - (-temp2 - 2*pi*floor(-temp2/(2*pi))); 19 | end 20 | 21 | dif = temp1 - temp2; 22 | if(dif > pi) 23 | dif = dif - 2*pi; 24 | end 25 | if(dif < -pi) 26 | dif = dif + 2*pi; 27 | end -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Windows image file caches 2 | Thumbs.db 3 | ehthumbs.db 4 | 5 | # Folder config file 6 | Desktop.ini 7 | 8 | # Recycle Bin used on file shares 9 | $RECYCLE.BIN/ 10 | 11 | # Windows Installer files 12 | *.cab 13 | *.msi 14 | *.msm 15 | *.msp 16 | 17 | # Windows shortcuts 18 | *.lnk 19 | 20 | # ========================= 21 | # Operating System Files 22 | # ========================= 23 | 24 | # OSX 25 | # ========================= 26 | 27 | .DS_Store 28 | .AppleDouble 29 | .LSOverride 30 | 31 | # Thumbnails 32 | ._* 33 | 34 | # Files that might appear in the root of a volume 35 | .DocumentRevisions-V100 36 | .fseventsd 37 | .Spotlight-V100 38 | .TemporaryItems 39 | .Trashes 40 | .VolumeIcon.icns 41 | 42 | # Directories potentially created on remote AFP share 43 | .AppleDB 44 | .AppleDesktop 45 | Network Trash Folder 46 | Temporary Items 47 | .apdisk 48 | -------------------------------------------------------------------------------- /functions/headingangle.m: -------------------------------------------------------------------------------- 1 | function [angle] = headingangle(m) 2 | 3 | angle = 0; 4 | % if m(1) < 0 5 | % angle = pi() - atan(m(2)/m(3)); 6 | % end 7 | % if m(1) > 0 & m(2) > 0 8 | % angle = 2*pi() - atan(m(2)/m(3)); 9 | % end 10 | % if m(1) > 0 & m(2) < 0 11 | % angle = - atan(m(2)/m(3)); 12 | % end 13 | % if m(1) == 0 & m(2) < 0 14 | % angle = pi()/2; 15 | % end 16 | % if m(1) == 0 & m(2) > 0 17 | % angle = 3*pi()/2; 18 | % end 19 | % Para os dados x_ 20 | %|\ 21 | %z y 22 | % if m(2) > 0 23 | % angle = pi/2 - atan(m(1)/m(2)); 24 | % elseif m(2)<0 25 | % angle = pi + pi/2 - atan(m(1)/m(2)); 26 | % elseif m(2)==0 & m(1)<0 27 | % angle = pi; 28 | % elseif m(2)==0 & m(1)>0 29 | % angle = 0; 30 | % end 31 | %z y 32 | % Para os dados x_|/ 33 | m(2) = - m(2); 34 | if m(2) > 0 35 | angle = pi/2 - atan(m(1)/m(2)); 36 | elseif m(2)<0 37 | angle = pi + pi/2 - atan(m(1)/m(2)); 38 | elseif m(2)==0 & m(1)<0 39 | angle = pi; 40 | elseif m(2)==0 & m(1)>0 41 | angle = 0; 42 | end 43 | 44 | if angle > pi 45 | angle = -2*pi + angle; 46 | end 47 | 48 | 49 | -------------------------------------------------------------------------------- /EulerEKF.m: -------------------------------------------------------------------------------- 1 | function [phi theta psi] = EulerEKF(z, rates, dt) 2 | % 3 | % 4 | persistent H Q R 5 | persistent x P 6 | persistent firstRun 7 | 8 | 9 | if isempty(firstRun) 10 | H = [ 1 0 0; 11 | 0 1 0 ]; 12 | 13 | Q = [ 0.0001 0 0; 14 | 0 0.0001 0; 15 | 0 0 0.1 ]; 16 | 17 | R = [ 6 0; 18 | 0 6 ]; 19 | 20 | x = [0 0 0]'; 21 | P = 10*eye(3); 22 | 23 | firstRun = 1; 24 | end 25 | 26 | 27 | A = Ajacob(x, rates, dt); 28 | 29 | xp = fx(x, rates, dt); 30 | Pp = A*P*A' + Q; 31 | 32 | K = Pp*H'*inv(H*Pp*H' + R); 33 | 34 | x = xp + K*(z - H*xp); 35 | P = Pp - K*H*Pp; 36 | 37 | 38 | phi = x(1); 39 | theta = x(2); 40 | psi = x(3); 41 | 42 | 43 | %------------------------------ 44 | function xp = fx(xhat, rates, dt) 45 | % 46 | % 47 | phi = xhat(1); 48 | theta = xhat(2); 49 | 50 | p = rates(1); 51 | q = rates(2); 52 | r = rates(3); 53 | 54 | xdot = zeros(3, 1); 55 | xdot(1) = p + q*sin(phi)*tan(theta) + r*cos(phi)*tan(theta); 56 | xdot(2) = q*cos(phi) - r*sin(phi); 57 | xdot(3) = q*sin(phi)*sec(theta) + r*cos(phi)*sec(theta); 58 | 59 | xp = xhat + xdot*dt; 60 | 61 | 62 | %------------------------------ 63 | function A = Ajacob(xhat, rates, dt) 64 | % 65 | % 66 | A = zeros(3, 3); 67 | 68 | 69 | phi = xhat(1); 70 | theta = xhat(2); 71 | 72 | p = rates(1); 73 | q = rates(2); 74 | r = rates(3); 75 | 76 | A(1,1) = q*cos(phi)*tan(theta) - r*sin(phi)*tan(theta); 77 | A(1,2) = q*sin(phi)*sec(theta)^2 + r*cos(phi)*sec(theta)^2; 78 | A(1,3) = 0; 79 | 80 | A(2,1) = -q*sin(phi) - r*cos(phi); 81 | A(2,2) = 0; 82 | A(2,3) = 0; 83 | 84 | A(3,1) = q*cos(phi)*sec(theta) - r*sin(phi)*sec(theta); 85 | A(3,2) = q*sin(phi)*sec(theta)*tan(theta) + r*cos(phi)*sec(theta)*tan(theta); 86 | A(3,3) = 0; 87 | 88 | A = eye(3) + A*dt; -------------------------------------------------------------------------------- /syspos4_m.m: -------------------------------------------------------------------------------- 1 | function [sys] = syspos4_m(p, v, q, varQ, varR, tau, T) 2 | 3 | lambda = p(1); 4 | phi = p(2); 5 | h = p(3); 6 | 7 | vN = v(1); 8 | vE = v(2); 9 | vD = v(3); 10 | 11 | Rlambda = Rlambda_m(lambda); 12 | Rphi = Rphi_m(lambda); 13 | RT = rot(q)'; 14 | 15 | F(1,1) = 0; 16 | F(1,2) = 0; 17 | F(1,3) = -(vN)/(Rlambda + h)^2; 18 | F(2,1) = (vE*sin(lambda))/((Rphi + h)*cos(lambda)^2); 19 | F(2,2) = 0; 20 | F(2,3) = -(vE)/((Rphi + h)^2*cos(lambda)); 21 | F(3,1) = 0; 22 | F(3,2) = 0; 23 | F(3,3) = 0; 24 | 25 | F(1,4) = inv(Rlambda + h); 26 | F(1,5) = 0; 27 | F(1,6) = 0; 28 | F(2,4) = 0; 29 | F(2,5) = inv((Rphi + h)*cos(lambda)); 30 | F(2,6) = 0; 31 | F(3,4) = 0; 32 | F(3,5) = 0; 33 | F(3,6) = -1; 34 | 35 | F(1:3, 7:9) = zeros(3,3); 36 | 37 | F(4,1) = -(vE)^2/(Rphi + h) - (vE^2*sin(lambda)^2)/((Rphi + h)*cos(lambda)^2); 38 | F(4,2) = 0; 39 | F(4,3) = (vE^2*sin(lambda))/((Rphi + h)^2*cos(lambda)) -(vN*vD)/((Rlambda + h)^2); 40 | F(5,1) = (vE*vN) /(Rphi + h) + (vE*vN*(sin(lambda)^2))/((Rphi + h)*(cos(lambda)^2)); 41 | F(5,2) = 0; 42 | F(5,3) = -((vE*vN)*sin(lambda))/((Rphi + h)^2*cos(lambda)) - (vE*vD)/((Rphi + h)^2); 43 | F(6,1) = 0; 44 | F(6,2) = 0; 45 | F(6,3) = (vE^2)/((Rphi + h)^2) + (vN^2)/((Rlambda + h)^2); 46 | 47 | F(4,4) = vD/(Rlambda + h); 48 | F(4,5) = -(2*vE*sin(lambda))/((Rphi + h)*cos(lambda)); 49 | F(4,6) = vN/(Rlambda + h); 50 | F(5,4) = (vE*sin(lambda))/((Rphi + h)*cos(lambda)); 51 | F(5,5) = (vN*sin(lambda))/((Rphi + h)*cos(lambda)) + vD/(Rphi + h); 52 | F(5,6) = vE/(Rphi + h); 53 | F(6,4) = -(2*vN)/(h); 54 | F(6,5) = -(2*vE)/(Rphi + h); 55 | F(6,6) = 0; 56 | 57 | F(4:6, 7:9) = -RT; 58 | 59 | F(7:9, 1:6) = zeros(3,6); 60 | 61 | F(7:9, 7:9) = -diag(1./tau); 62 | 63 | Phi = 1*eye(9) + F*T; 64 | 65 | 66 | 67 | L = [ zeros(6,3); RT]; 68 | 69 | G = [ zeros(3,6); 70 | -RT, zeros(3,3); 71 | zeros(3,3), eye(3,3)]; 72 | 73 | H = [ eye(3,3), zeros(3,3), zeros(3,3)]; 74 | 75 | 76 | E = diag(varQ); 77 | 78 | Q = G*E*G'*T; 79 | 80 | R = diag(varR); 81 | 82 | Mf = 10^-3.* [ones(6,1); zeros(3,1)]; 83 | Mh = 10^-3 .* [ones(3,1)]; 84 | 85 | Nf = 10^2*sum(F(1:6,1:9) .* T) / 6; 86 | Ng = 10^2*[sum(G(1:6,1:3))/6 , zeros(1,3)]; 87 | Nh = 0*sum(H(:,1:9))/3; 88 | 89 | sys.Phi = Phi; 90 | sys.L = L; 91 | sys.G = G; 92 | sys.H = H; 93 | sys.Q = Q; 94 | sys.Qeta = E; 95 | sys.R = R; 96 | sys.alfa = 1000; % Sayed 97 | % sys.alfa = 0.1; 98 | % sys.alfa = 1; 99 | sys.Ef = Nf; 100 | sys.Eg = Ng; 101 | sys.Eh = Nh; 102 | %sys.M = M; 103 | sys.Mf = Mf; 104 | sys.Mh = Mh; 105 | -------------------------------------------------------------------------------- /sysEuler_f.m: -------------------------------------------------------------------------------- 1 | function [sys] = sysEuler_f(rates, xhat, varQ, varR, g, tau, T) 2 | 3 | E = diag(varQ); 4 | 5 | 6 | R = diag(varR); 7 | 8 | % E = [ 0.0001 0 0 0 0 0; 9 | % 0 0.0001 0 0 0 0; 10 | % 0 0 0.01 0 0 0; 11 | % 0 0 0 0.0001 0 0; 12 | % 0 0 0 0 0.0001 0; 13 | % 0 0 0 0 0 0.0001]; 14 | 15 | G = eye(6); 16 | 17 | Q = G*E*G'*T; 18 | 19 | 20 | % R = [ 6 0 0 ; 21 | % 0 6 0 ; 22 | % 0 0 6 ]; 23 | 24 | 25 | H = [ 1 0 0 0 0 0; 26 | 0 1 0 0 0 0; 27 | 0 0 1 0 0 0]; 28 | 29 | 30 | A = zeros(3, 3); 31 | 32 | phi = xhat(1); 33 | theta = xhat(2); 34 | 35 | bp = xhat(4); 36 | bq = xhat(5); 37 | br = xhat(6); 38 | 39 | ps = rates(1); 40 | qs = rates(2); 41 | rs = rates(3); 42 | 43 | A(1,1) = qs*cos(phi)*tan(theta) - rs*sin(phi)*tan(theta) - bq*cos(phi)*tan(theta) + br*sin(phi)*tan(theta); 44 | A(1,2) = qs*sin(phi)*sec(theta)^2 + rs*cos(phi)*sec(theta)^2 - bq*sin(phi)*sec(theta)^2 - br*cos(phi)*sec(theta)^2; 45 | A(1,3) = 0; 46 | A(1,4) = -1; 47 | A(1,5) = sin(phi)*tan(theta); 48 | A(1,6) = cos(phi)*tan(theta); 49 | 50 | A(2,1) = - qs*sin(phi) - rs*cos(phi) + bq*cos(phi) - br*cos(phi); 51 | A(2,2) = 0; 52 | A(2,3) = 0; 53 | A(2,4) = 0; 54 | A(2,5) = - cos(phi); 55 | A(2,6) = - sin(phi); 56 | 57 | 58 | A(3,1) = qs*cos(phi)*sec(theta) - rs*sin(phi)*sec(theta) - bq*cos(phi)*sec(theta) + br*sin(phi)*sec(theta); 59 | A(3,2) = qs*sin(phi)*sec(theta)*tan(theta) + rs*cos(phi)*sec(theta)*tan(theta) - bq*sin(phi)*sec(theta)*tan(theta) - br*cos(phi)*sec(theta)*tan(theta); 60 | A(3,3) = 0; 61 | A(3,4) = 0; 62 | A(3,5) = - sin(phi)*sec(theta); 63 | A(3,6) = - cos(phi)*sec(theta); 64 | 65 | A(4,1) = 0; 66 | A(4,2) = 0; 67 | A(4,3) = 0; 68 | A(4,4) = -1/tau(1); 69 | A(4,5) = 0; 70 | A(4,6) = 0; 71 | 72 | 73 | A(5,1) = 0; 74 | A(5,2) = 0; 75 | A(5,3) = 0; 76 | A(5,4) = 0; 77 | A(5,5) = -1/tau(2); 78 | A(5,6) = 0; 79 | 80 | A(6,1) = 0; 81 | A(6,2) = 0; 82 | A(6,3) = 0; 83 | A(6,4) = 0; 84 | A(6,5) = 0; 85 | A(6,6) = -1/tau(3); 86 | 87 | Phi = eye(6) + A*T; 88 | 89 | sys.Phi = Phi; 90 | sys.G = G; 91 | sys.H = H; 92 | sys.Q = Q; 93 | sys.Qeta = E; 94 | sys.R = R; 95 | 96 | Ef = 0.001*ones(6,6); 97 | Eg = 0.001*ones(6,6); 98 | Eh = 0.001*ones(3,6); 99 | Mf = eye(6); 100 | Mh = eye(3); 101 | sys.alfa = 2000; % Sayed 102 | sys.Ef = Ef; 103 | sys.Eg = Eg; 104 | sys.Eh = Eh; 105 | % sys.M = M; 106 | sys.Mf = Mf; 107 | sys.Mh = Mh; 108 | 109 | sys.gamma = 100000; -------------------------------------------------------------------------------- /functions/sysquat_m.m: -------------------------------------------------------------------------------- 1 | function [sys] = sysquat_m(omega, q, varQ, varR, g, tau, T) 2 | q0 = q(1); 3 | q1 = q(2); 4 | q2 = q(3); 5 | q3 = q(4); 6 | 7 | r = rot(q); 8 | 9 | Omega = Omegacalc(omega); 10 | Psi = Psicalc(q); 11 | 12 | F = [0.5*Omega, -0.5*Psi; 13 | zeros(3,4) , -diag(1./tau)]; 14 | 15 | Phi = eye(7) + F*T; 16 | 17 | 18 | omegap = sqrt(omega(1)^2 + omega(2)^2 + omega(3)^2); 19 | Phi(1:4,1:4) = (eye(4)*cos(1/2*omegap*T) + Omega*inv(omegap)*sin(1/2*omegap*T)); 20 | 21 | G = [-0.5*Psi, zeros(4,3); 22 | zeros(3,3), eye(3,3)]; 23 | 24 | H = [eye(4), zeros(4,3)]; 25 | H(5,1) = (2*q3*r(1,1) - 2*q0*r(1,2))/(r(1,1)^2); 26 | H(5,2) = (2*q2*r(1,1) - 2*q1*r(1,2))/(r(1,1)^2); 27 | H(5,3) = (2*q1*r(1,1) + 2*q2*r(1,2))/(r(1,1)^2); 28 | H(5,4) = (2*q0*r(1,1) + 2*q3*r(1,2))/(r(1,1)^2); 29 | H(5,5:7) = zeros(1,3); 30 | 31 | E = diag(varQ); 32 | 33 | Q = G*E*G'*T; 34 | 35 | R = diag(varR); 36 | 37 | 38 | Ef = 0.01*[ 1.4803399819097904e-005 0.0014590105834444845 0.0017107285399275364 0.0020022809722185539 0.00099777056214160197 0.0010263839752163097 0.028261295273978856 ; 39 | 0.0014590105834444845 1.4803399819097904e-005 0.0020022809722185539 0.0017107285399275364 0.026246343289360544 0.028261295273978856 0.0010263839752163097 ; 40 | 0.0017107285399275364 0.0020022809722185539 1.4803399819097904e-005 0.0014590105834444845 0.028261295273978856 0.026246343289360544 0.00099777056214160197 ; 41 | 0.0020022809722185539 0.0017107285399275364 0.0014590105834444845 1.4803399819097904e-005 0.0010263839752163097 0.00099777056214160197 0.026246343289360544 ; 42 | 0 0 0 0 1.6787360226480613e-009 0 0 ; 43 | 0 0 0 0 0 1.9745792003171669e-009 0 ; 44 | 0 0 0 0 0 0 4.3422240198188913e-009 ]; 45 | 46 | Eg = 0.001*[ 0.0042620984684868758 0.0043488658300001575 0.12045557728903865 0 0 0 ; 47 | 0.11402512126384462 0.12045557728903865 0.0043488658300001575 0 0 0 ; 48 | 0.12045557728903865 0.11402512126384462 0.0042620984684868758 0 0 0 ; 49 | 0.0043488658300001575 0.0042620984684868758 0.11402512126384462 0 0 0 ; 50 | 0 0 0 0 0 0 ; 51 | 0 0 0 0 0 0 ; 52 | 0 0 0 0 0 0 ]; 53 | 54 | Eh = 0.01*[ 9.4416920243637286 172.71773757701732 179.67905596416497 8.6243952278318989 0 0 0 ; 55 | 8.6243952278318989 179.67905596416497 1072.71773757701732 9.4416920243637286 0 0 0 ; 56 | 179.67905596416497 8.6243952278318989 9.4416920243637286 172.71773757701732 0 0 0 ; 57 | 2.0736036134272466 0.10522561984381455 0.12891363679983572 2.0735239135853307 0 0 0 ]; 58 | 59 | M = [ 1 0 0 0 0 0 0 ; 60 | 0 1 0 0 0 0 0 ; 61 | 0 0 1 0 0 0 0 ; 62 | 0 0 0 1 0 0 0 ; 63 | 0 0 0 0 1 0 0 ; 64 | 0 0 0 0 0 1 0 ; 65 | 0 0 0 0 0 0 1 ]; 66 | 67 | Mf = M; 68 | Mh = eye(4); 69 | % a = -1; 70 | % b = 1; 71 | % delta = a + (b-a).*rand; 72 | % deltaPhi = M*delta*Ef; 73 | % deltaG = M*delta*Eg; 74 | % 75 | % Phi = Phi + deltaPhi; 76 | % G = G + deltaG; 77 | 78 | sys.Phi = Phi; 79 | sys.G = G; 80 | sys.H = H; 81 | sys.Q = Q; 82 | sys.Qeta = E; 83 | sys.R = R; 84 | 85 | sys.alfa = 2000; % Sayed 86 | % sys.alfa = 0.1; 87 | % sys.alfa = 1000; 88 | sys.Ef = Ef; 89 | sys.Eg = Eg; 90 | sys.Eh = Eh; 91 | sys.M = M; 92 | sys.Mf = Mf; 93 | sys.Mh = Mh; 94 | 95 | sys.gamma = 100000; -------------------------------------------------------------------------------- /plot_fig.m: -------------------------------------------------------------------------------- 1 | a = figure(1); 2 | plot(t_v(2:n),psiIMU_v(2:n)*r2d,'.b'); 3 | hold on 4 | % plot(t_v(2:n),psiGyro_v(2:n)*r2d,'.k') 5 | %plot(tGPS_v,psiGPS_v*r2d,'.g','MarkerSize',10) 6 | plot(t_v(2:n),psiHat_v(2:n)*r2d,'.r'); 7 | hold off 8 | ylim([-360,360]) 9 | ylabel('\psi (^o)') 10 | xlabel('t (s)') 11 | % legend('IMU',filter) 12 | legend('IMU', filter) 13 | set( gca , ... 14 | 'FontName' , 'Helvetica' ); 15 | 16 | set(gca, ... 17 | 'Box' , 'off' , ... 18 | 'TickDir' , 'out' , ... 19 | 'TickLength' , [.02 .02] , ... 20 | 'XMinorTick' , 'on' , ... 21 | 'YMinorTick' , 'on' , ... 22 | 'YGrid' , 'on' , ... 23 | 'XColor' , [.3 .3 .3], ... 24 | 'YColor' , [.3 .3 .3], ... 25 | 'LineWidth' , 1 ); 26 | %savefig(a,'figure1') 27 | 28 | a = figure(2); 29 | plot(t_v(2:n),thetaIMU_v(2:n)*r2d,'.b'); %'-.g','LineWidth',2.5); 30 | hold on 31 | % plot(t_v(2:n),thetaGyro_v(2:n)*r2d,'.k') 32 | %plot(t_v,thetaAccel_v*r2d,'.g') %'--c','LineWidth',2.5) 33 | plot(t_v(2:n),thetaHat_v(2:n)*r2d,'.r'); %'-r','LineWidth',2); 34 | hold off 35 | ylim([-360,360]) 36 | ylabel('\theta (^o)') 37 | xlabel('t (s)') 38 | % legend('IMU','Rate Gyro','Accelerometer',filter) 39 | legend('IMU',filter) 40 | set( gca , ... 41 | 'FontName' , 'Helvetica' ); 42 | 43 | set(gca, ... 44 | 'Box' , 'off' , ... 45 | 'TickDir' , 'out' , ... 46 | 'TickLength' , [.02 .02] , ... 47 | 'XMinorTick' , 'on' , ... 48 | 'YMinorTick' , 'on' , ... 49 | 'YGrid' , 'on' , ... 50 | 'XColor' , [.3 .3 .3], ... 51 | 'YColor' , [.3 .3 .3], ... 52 | 'LineWidth' , 1 ); 53 | %savefig(a,'figure2') 54 | 55 | a=figure(3); 56 | plot(t_v(2:n),phiIMU_v(2:n)*r2d,'.b'); 57 | hold on 58 | % plot(t_v(2:n),phiGyro_v(2:n)*r2d,'.k') 59 | %plot(t_v,phiAccel_v*r2d,'.g') 60 | plot(t_v(2:n),phiHat_v(2:n)*r2d,'.r'); 61 | hold off 62 | ylim([-360,360]) 63 | ylabel('\theta (^o)') 64 | xlabel('t (s)') 65 | % legend('IMU','Rate Gyro','Accelerometer',filter) 66 | legend('IMU',filter) 67 | set( gca , ... 68 | 'FontName' , 'Helvetica' ); 69 | 70 | set(gca, ... 71 | 'Box' , 'off' , ... 72 | 'TickDir' , 'out' , ... 73 | 'TickLength' , [.02 .02] , ... 74 | 'XMinorTick' , 'on' , ... 75 | 'YMinorTick' , 'on' , ... 76 | 'YGrid' , 'on' , ... 77 | 'XColor' , [.3 .3 .3], ... 78 | 'YColor' , [.3 .3 .3], ... 79 | 'LineWidth' , 1 ); 80 | 81 | 82 | figure(6); 83 | plot(tGPS_v(2:end),hHat_v(2:end)*r2d,'-b','LineWidth',2) 84 | hold on 85 | plot(tGPS_v(2:end),p_GPS_v(3,2:end)*r2d,'--r','LineWidth',2); %'-.g','LineWidth',2.5); 86 | xlim([0,125]) 87 | ylabel('Altitude (m)','FontSize',18) 88 | xlabel('t (s)','FontSize',18) 89 | legend(filter, 'GPS') 90 | set( gca , ... 91 | 'FontName' , 'Helvetica' ,'FontSize',14); 92 | set(gca, ... 93 | 'Box' , 'on' , ... 94 | 'TickDir' , 'out' , ... 95 | 'TickLength' , [.02 .02] , ... 96 | 'XMinorTick' , 'on' , ... 97 | 'YMinorTick' , 'on' , ... 98 | 'YGrid' , 'off' , ... 99 | 'LineWidth' , 1.5 ); 100 | -------------------------------------------------------------------------------- /EulerEKF_bg.m: -------------------------------------------------------------------------------- 1 | function [phi, theta, psi, bg] = EulerEKF_bg(z, rates, dt) 2 | % 3 | % 4 | persistent H Q R 5 | persistent x P 6 | persistent tauR 7 | persistent firstRun 8 | 9 | 10 | if isempty(firstRun) 11 | H = [ 1 0 0 0 0 0; 12 | 0 1 0 0 0 0; 13 | 0 0 1 0 0 0]; 14 | 15 | Q = [ 0.0001 0 0 0 0 0; 16 | 0 0.0001 0 0 0 0; 17 | 0 0 0.01 0 0 0; 18 | 0 0 0 0.0001 0 0; 19 | 0 0 0 0 0.0001 0; 20 | 0 0 0 0 0 0.0001]; 21 | 22 | % R = [ 6 0 0 0; 23 | % 0 6 0 0; 24 | % 0 0 6 0; 25 | % 0 0 0 0.06]; 26 | 27 | R = [ 6 0 0 ; 28 | 0 6 0 ; 29 | 0 0 6 ]; 30 | 31 | x = [0 0 0 0.01 0.01 0.01]'; 32 | P = 10*eye(6); 33 | 34 | 35 | firstRun = 1; 36 | end 37 | 38 | 39 | A = Ajacob(x, rates, dt); 40 | 41 | xp = fx(x, rates, dt); 42 | Pp = A*P*A' + Q; 43 | 44 | K = Pp*H'*inv(H*Pp*H' + R); 45 | 46 | % x = xp + K*(z - H*xp); 47 | x = xp + K*normalize_angle_f(z-H*xp,-pi); 48 | 49 | P = Pp - K*H*Pp; 50 | 51 | 52 | phi = x(1); 53 | theta = x(2); 54 | psi = x(3); 55 | 56 | bg = x(4:6); 57 | 58 | 59 | %------------------------------ 60 | function xp = fx(xhat, rates, dt) 61 | % 62 | % 63 | tauR(1,1) = 626.8115; 64 | tauR(2,1) = 6468.0515; 65 | tauR(3,1) = 602.5784; 66 | 67 | phi = xhat(1); 68 | theta = xhat(2); 69 | 70 | ps = rates(1); 71 | qs = rates(2); 72 | rs = rates(3); 73 | 74 | bp = xhat(4); 75 | bq = xhat(5); 76 | br = xhat(6); 77 | 78 | 79 | xdot = zeros(3, 1); 80 | xdot(1) = ps + qs*sin(phi)*tan(theta) + rs*cos(phi)*tan(theta) -bp - bq*sin(phi)*tan(theta) - br*cos(phi)*tan(theta); 81 | xdot(2) = qs*cos(phi) - rs*sin(phi) - bq*cos(phi) - br*sin(phi); 82 | xdot(3) = qs*sin(phi)*sec(theta) + rs*cos(phi)*sec(theta) - bq*sin(phi)*sec(theta) - br*cos(phi)*sec(theta); 83 | xdot(4) = -1/tauR(1)*bp; 84 | xdot(5) = -1/tauR(2)*bq; 85 | xdot(6) = -1/tauR(3)*br; 86 | xp = xhat + xdot*dt; 87 | 88 | 89 | %------------------------------ 90 | function A = Ajacob(xhat, rates, dt) 91 | % 92 | % 93 | A = zeros(3, 3); 94 | 95 | tauR(1,1) = 626.8115; 96 | tauR(2,1) = 6468.0515; 97 | tauR(3,1) = 602.5784; 98 | 99 | 100 | phi = xhat(1); 101 | theta = xhat(2); 102 | 103 | bp = xhat(4); 104 | bq = xhat(5); 105 | br = xhat(6); 106 | 107 | ps = rates(1); 108 | qs = rates(2); 109 | rs = rates(3); 110 | 111 | A(1,1) = qs*cos(phi)*tan(theta) - rs*sin(phi)*tan(theta) -bq*cos(phi)*tan(theta) + br*sin(phi)*tan(theta); 112 | A(1,2) = qs*sin(phi)*sec(theta)^2 + rs*cos(phi)*sec(theta)^2 -bq*sin(phi)*sec(theta)^2 -br*cos(phi)*sec(theta)^2; 113 | A(1,3) = 0; 114 | A(1,4) = -1; 115 | A(1,5) = sin(phi)*tan(theta); 116 | A(1,6) = cos(phi)*tan(theta); 117 | 118 | A(2,1) = -qs*sin(phi) - rs*cos(phi) + bq*cos(phi)-br*cos(phi); 119 | A(2,2) = 0; 120 | A(2,3) = 0; 121 | A(2,4) = 0; 122 | A(2,5) = -cos(phi); 123 | A(2,6) = -sin(phi); 124 | 125 | 126 | A(3,1) = qs*cos(phi)*sec(theta) - rs*sin(phi)*sec(theta) - bq*cos(phi)*sec(theta) + br*sin(phi)*sec(theta); 127 | A(3,2) = qs*sin(phi)*sec(theta)*tan(theta) + rs*cos(phi)*sec(theta)*tan(theta) - bq*sin(phi)*sec(theta)*tan(theta) - br*cos(phi)*sec(theta)*tan(theta); 128 | A(3,3) = 0; 129 | A(3,4) = 0; 130 | A(3,5) = -sin(phi)*sec(theta); 131 | A(3,6) = -cos(phi)*sec(theta); 132 | 133 | A(4,1) = 0; 134 | A(4,2) = 0; 135 | A(4,3) = 0; 136 | A(4,4) = -1/tauR(1); 137 | A(4,5) = 0; 138 | A(4,6) = 0; 139 | 140 | 141 | A(5,1) = 0; 142 | A(5,2) = 0; 143 | A(5,3) = 0; 144 | A(5,4) = 0; 145 | A(5,5) = -1/tauR(2); 146 | A(5,6) = 0; 147 | 148 | A(6,1) = 0; 149 | A(6,2) = 0; 150 | A(6,3) = 0; 151 | A(6,4) = 0; 152 | A(6,5) = 0; 153 | A(6,6) = -1/tauR(3); 154 | 155 | A = eye(6) + A*dt; 156 | -------------------------------------------------------------------------------- /functions/syspos_m.m: -------------------------------------------------------------------------------- 1 | function [sys] = syspos_m(p, v, q, varQ, varR, tau, T) 2 | 3 | lambda = p(1); 4 | phi = p(2); 5 | h = p(3); 6 | 7 | vN = v(1); 8 | vE = v(2); 9 | vD = v(3); 10 | 11 | Rlambda = Rlambda_m(lambda); 12 | Rphi = Rphi_m(lambda); 13 | RT = rot(q)'; 14 | 15 | F(1,1) = 0; 16 | F(1,2) = 0; 17 | F(1,3) = -(vN)/(Rlambda + h)^2; 18 | F(2,1) = (vE*sin(lambda))/((Rphi + h)*cos(lambda)^2); 19 | F(2,2) = 0; 20 | F(2,3) = -(vE)/((Rphi + h)^2*cos(lambda)); 21 | F(3,1) = 0; 22 | F(3,2) = 0; 23 | F(3,3) = 0; 24 | 25 | F(1,4) = inv(Rlambda + h); 26 | F(1,5) = 0; 27 | F(1,6) = 0; 28 | F(2,4) = 0; 29 | F(2,5) = inv((Rphi + h)*cos(lambda)); 30 | F(2,6) = 0; 31 | F(3,4) = 0; 32 | F(3,5) = 0; 33 | F(3,6) = -1; 34 | 35 | F(1:3, 7:9) = zeros(3,3); 36 | 37 | F(4,1) = -(vE)^2/(Rphi + h) - (vE^2*sin(lambda)^2)/((Rphi + h)*cos(lambda)^2); 38 | F(4,2) = 0; 39 | F(4,3) = (vE^2*sin(lambda))/((Rphi + h)^2*cos(lambda)) -(vN*vD)/((Rlambda + h)^2); 40 | F(5,1) = (vE*vN) /(Rphi + h) + (vE*vN*(sin(lambda)^2))/((Rphi + h)*(cos(lambda)^2)); 41 | F(5,2) = 0; 42 | F(5,3) = -((vE*vN)*sin(lambda))/((Rphi + h)^2*cos(lambda)) - (vE*vD)/((Rphi + h)^2); 43 | F(6,1) = 0; 44 | F(6,2) = 0; 45 | F(6,3) = (vE^2)/((Rphi + h)^2) + (vN^2)/((Rlambda + h)^2); 46 | 47 | F(4,4) = vD/(Rlambda + h); 48 | F(4,5) = -(2*vE*sin(lambda))/((Rphi + h)*cos(lambda)); 49 | F(4,6) = vN/(Rlambda + h); 50 | F(5,4) = (vE*sin(lambda))/((Rphi + h)*cos(lambda)); 51 | F(5,5) = (vN*sin(lambda))/((Rphi + h)*cos(lambda)) + vD/(Rphi + h); 52 | F(5,6) = vE/(Rphi + h); 53 | F(6,4) = -(2*vN)/(h); 54 | F(6,5) = -(2*vE)/(Rphi + h); 55 | F(6,6) = 0; 56 | 57 | F(4:6, 7:9) = -RT; 58 | 59 | F(7:9, 1:6) = zeros(3,6); 60 | 61 | F(7:9, 7:9) = -diag(1./tau); 62 | 63 | Phi = 1*eye(9) + F*T; 64 | 65 | 66 | 67 | L = [ zeros(6,3); RT]; 68 | 69 | G = [ zeros(3,6); 70 | -RT, zeros(3,3); 71 | zeros(3,3), eye(3,3)]; 72 | 73 | H = [ eye(6,6), zeros(6,3)]; 74 | 75 | 76 | E = diag(varQ); 77 | 78 | Q = G*E*G'*T; 79 | 80 | R = diag(varR); 81 | 82 | Ef = [ 0 0 8.6518448575533743e-029 6.9770885612170754e-017 0 0 0 0 0 ; 83 | 1.4287232607684019e-015 0 1.2447232512488922e-028 0 8.0235026059619446e-017 0 0 0 0 ; 84 | 0 0 0 0 0 0.0048467315716272576 0 0 0 ; 85 | 1.3419798263717305e-013 0 2.6307940197713262e-028 8.7953645040105456e-016 4.9124972178940882e-015 6.0101195411087961e-015 0.15833602031753119 0.055399136600360326 0.020535612493901633 ; 86 | 1.5752745071652574e-014 0 5.0230881702614319e-029 1.2281243044735221e-015 1.5702369259163797e-015 7.5189289898952656e-015 0.055441611614859838 0.16725670231452638 0.0021191568441892034 ; 87 | 0 0 1.7660288223281591e-027 2.4340445898035342e-006 3.0075715959581062e-014 0 0.0026292483570886692 0.0021191568441892034 0.0043854455861159387 ; 88 | 0 0 0 0 0 0 8.2592255044468662e-008 0 0 ; 89 | 0 0 0 0 0 0 0 2.8384398844185659e-010 0 ; 90 | 0 0 0 0 0 0 0 0 2.4536583488697674e-010 ]; 91 | 92 | Eg = [ 0 0 0 0 0 0 ; 93 | 0 0 0 0 0 0 ; 94 | 0 0 0 0 0 0 ; 95 | 0.6755166781275751 0.23380865986478375 0.086803728739753533 0 0 0 ; 96 | 0.23511438553072692 0.71236956254659978 0.0084971391152168316 0 0 0 ; 97 | 0.008621263625382708 0.0084971391152168316 0.0012944780053323755 0 0 0 ; 98 | 0 0 0 0 0 0 ; 99 | 0 0 0 0 0 0 ; 100 | 0 0 0 0 0 0 ]; 101 | 102 | Eh = [ 0 0 0 0 0 0 0 0 0 ; 103 | 0 0 0 0 0 0 0 0 0 ; 104 | 0 0 0 0 0 0 0 0 0 ; 105 | 0 0 0 0 0 0 0 0 0 ; 106 | 0 0 0 0 0 0 0 0 0 ; 107 | 0 0 0 0 0 0 0 0 0 ]; 108 | 109 | M = 1e-8*[ 1 0 0 0 0 0 0 0 0 ; 110 | 0 1 0 0 0 0 0 0 0 ; 111 | 0 0 1 0 0 0 0 0 0 ; 112 | 0 0 0 1 0 0 0 0 0 ; 113 | 0 0 0 0 1 0 0 0 0 ; 114 | 0 0 0 0 0 1 0 0 0 ; 115 | 0 0 0 0 0 0 1 0 0 ; 116 | 0 0 0 0 0 0 0 1 0 ; 117 | 0 0 0 0 0 0 0 0 1 ]; 118 | 119 | Mf = M; 120 | Mh = 0*eye(6); 121 | 122 | % a = -1; 123 | % b = 1; 124 | % delta = a + (b-a).*rand; 125 | % deltaPhi = M*delta*Ef; 126 | % deltaG = M*delta*Eg; 127 | % 128 | % Phi = Phi + deltaPhi; 129 | % G = G + deltaG; 130 | 131 | sys.Phi = Phi; 132 | sys.L = L; 133 | sys.G = G; 134 | sys.H = H; 135 | sys.Q = Q; 136 | sys.Qeta = E; 137 | sys.R = R; 138 | sys.alfa = 1000; % Sayed 139 | % sys.alfa = 0.1; 140 | % sys.alfa = 1; 141 | sys.Ef = Ef; 142 | sys.Eg = Eg; 143 | sys.Eh = Eh; 144 | sys.M = M; 145 | sys.Mf = Mf; 146 | sys.Mh = Mh; 147 | -------------------------------------------------------------------------------- /functions/sys_ati.m: -------------------------------------------------------------------------------- 1 | function [sys] = sys_ati(sys,omega, q, sigma, R, g,m, tau, T, opt) 2 | % q0 = q(1); 3 | % q1 = q(2); 4 | % q2 = q(3); 5 | % q3 = q(4); 6 | % 7 | % r = rot(q); 8 | 9 | Omega = Omegacalc(omega); 10 | Psi = Psicalc(q); 11 | 12 | F = [0.5*Omega, -0.5*Psi; 13 | zeros(3,4) , -diag(1./tau)]; 14 | 15 | % F = [0.5*Omega, -0.5*Psi, zeros(4,3); 16 | % zeros(3,4) , -diag(1./tau(1:3)), zeros(3,3); 17 | % zeros(3,4) , -diag(1./tau(4:6)), zeros(3,3)]; 18 | 19 | Phi = eye(7) + F*T; 20 | 21 | %deltaPhi = 22 | omegap = sqrt(omega(1)^2 + omega(2)^2 + omega(3)^2); 23 | Phi(1:4,1:4) = (eye(4)*cos(1/2*omegap*T) + Omega*inv(omegap)*sin(1/2*omegap*T)); 24 | 25 | 26 | G = [-0.5*Psi, zeros(4,3); 27 | zeros(3,3), eye(3,3)]; 28 | 29 | % G = [-0.5*Psi, zeros(4,3), zeros(4,3); 30 | % zeros(3,3), eye(3,3), zeros(3,3); 31 | % zeros(3,3), zeros(3,3), eye(3,3)]; 32 | 33 | Gd = G*sqrt(T); 34 | %Gd = G*T; 35 | 36 | %Gd = (T*eye(10) + T^2*Phi)*G; 37 | % H = [ -2*g*q2 2*g*q3 -2*g*q0 2*g*q1 0 0 0; 38 | % 2*g*q1 2*g*q0 2*g*q3 2*g*q2 0 0 0; 39 | % g*q0 -g*q1 -g*q2 g*q3 0 0 0];%2*g*q0 -2*g*q1 -2*g*q2 2*g*q3, 4*g*q0 0 0 4*g*q3 40 | % 41 | % H(4,1) = (2*q3*r(1,1) - 2*q0*r(1,2))/(r(1,1)^2 + r(1,2)^2); 42 | % H(4,2) = (2*q2*r(1,1) - 2*q1*r(1,2))/(r(1,1)^2 + r(1,2)^2); 43 | % H(4,3) = (2*q1*r(1,1) + 2*q2*r(1,2))/(r(1,1)^2 + r(1,2)^2); 44 | % H(4,4) = (2*q0*r(1,1) + 2*q3*r(1,2))/(r(1,1)^2 + r(1,2)^2); 45 | % H(4,5:7) = zeros(1,3); 46 | H = Hquat_m(q,g,m,opt); 47 | 48 | Eeta1 = [sigma(1)^2, 0 , 0; 49 | 0 , sigma(2)^2, 0; 50 | 0 , 0 , sigma(3)^2]; 51 | 52 | Eeta2 = [sigma(4)^2, 0 , 0; 53 | 0 , sigma(5)^2, 0; 54 | 0 , 0 , sigma(6)^2]; 55 | 56 | % Eeta3 = [sigma(7)^2, 0 , 0; 57 | % 0 , sigma(8)^2, 0; 58 | % 0 , 0 , sigma(9)^2]; 59 | 60 | Eeta1eta2 = [Eeta1 , zeros(3); 61 | zeros(3), Eeta2]; 62 | % Eeta1eta2 = [Eeta1 , zeros(3), zeros(3); 63 | % zeros(3), Eeta2, zeros(3), 64 | % zeros(3), zeros(3), Eeta3]; 65 | 66 | Q = G*Eeta1eta2*G'*T; 67 | %Q = G*Eeta1eta2*G'*T^2; 68 | % if ~exist('sys.Q') 69 | % Q = G*Eeta1eta2*G'*T; 70 | % else 71 | % Q = sys.Q; 72 | % Q = Phi*Q*Phi' + G*Eeta1eta2*G'; 73 | % end 74 | 75 | if opt == 0 76 | R = R(1:3,1:3); 77 | end 78 | [p,n] = size(H); 79 | [n,m] = size(G); 80 | 81 | sys.Phi = Phi; 82 | sys.G = G; 83 | sys.H = H; 84 | sys.Q = Q; 85 | sys.Qeta = Eeta1eta2; 86 | sys.R = R; 87 | sys.Phi = Phi; 88 | sys.G = G; 89 | sys.Gd = Gd; 90 | sys.G_v = zeros(n,p); 91 | sys.H = H; 92 | sys.E = eye(n); 93 | sys.K_w = zeros(p,m); 94 | sys.K_v = eye(p); 95 | sys.J = zeros(p,n); 96 | sys.Q = Q; 97 | sys.S = zeros(m,p); 98 | % sys.Nf = 0.01*[0.5505 0.5505 0.5505 0.5505 0.1076 0.1076 0.1076 0 0 0]; sys.Ng_w = 0.01*[0.6721 0.6721 0.6721 0 0 0 0 0 0]; 99 | % sys.Nh = [0.0040 0.0281 0.0283 0.0288 0 0 0 0 0 0]; 100 | % sys.Nh = [0.0040 0.0284 0.0284 0.0284 0 0 0 0 0 0]; 101 | sys.Nf = (Phi - eye(7)); 102 | sys.Nf = [mean(abs(sys.Nf(1:4,1:7)))]; 103 | % sys.Nf = 100*[sys.Nf(1:4) 0.001*sys.Nf(5:7)]; 104 | % sys.Ng_w = 100*[mean(abs(Gd(1:4,1:3))) zeros(1,3)]; 105 | % sys.Nh = mean(H); 106 | sys.Nf = 100*[sys.Nf(1:4) 0.001*sys.Nf(5:7)]; 107 | sys.Ng_w = 100*[mean(abs(Gd(1:4,1:3))) zeros(1,3)]; 108 | sys.Nh = mean(H); 109 | %sys.Nh = [0.0040 0.0284 0.0284 0.0284 0 0 0 0 0 0]; 110 | 111 | sys.Nj = zeros(1,n); 112 | sys.Ng_v = zeros(1,p); 113 | sys.Ne = zeros(1,n);% 1e-12*ones(1,n); 114 | sys.Nk_w = zeros(1,m); 115 | sys.Nk_v = zeros(1,p); 116 | sys.M1 = [1;1;1;1;0;0;0]; 117 | switch opt 118 | case 1 119 | %sys.M2 = [1;0;0;1;1;1]; 120 | sys.M2 = [1;1;1;1;1;1]; 121 | case 0 122 | sys.M2 = [1;1;1]; 123 | end 124 | 125 | sys.M1 = 0.001*sys.M1; 126 | sys.M2 = 0.001*sys.M2; 127 | 128 | % sys.alfa = 5; 129 | %sys.alfa = 4; 130 | %sys.S_HInf = R; 131 | sys.S_HInf = eye(7); 132 | 133 | sys.L = eye(7); 134 | % switch opt 135 | % case 1 136 | % sys.S_HInf = eye(6); 137 | % case 0 138 | % sys.S_HInf = eye(3); 139 | % end 140 | sys.theta = 0.1; 141 | sys.gamma = 500000; 142 | -------------------------------------------------------------------------------- /main_INS.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc 4 | 5 | path(path,'functions') 6 | 7 | n = 3100; 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | %% Syncronization data parameters 10 | % GPS 11 | deltaGPS = 1; 12 | stepGPS = 100; 13 | % IMU 14 | deltaIMU = 79; 15 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 16 | %% GPS and IMU data 17 | GPS_data 18 | IMU_data 19 | %% Angle conversion 20 | r2d = 180/pi; 21 | d2r = pi/180; 22 | 23 | %% Filter selection 24 | opt = 0; 25 | if opt == 0 26 | filter = 'Extended Kalman filter'; 27 | end 28 | 29 | %% Plot range 30 | range = 100; 31 | 32 | %% Simulation or actual experiment selection 33 | % state = 1 simulation, state = 0 actual 34 | state = 0; 35 | %% System initial conditions 36 | fIMU = 100; % Hz 37 | TIMU = 1/fIMU; % Sample time [s]. 38 | 39 | fGPS = 1; % Hz 40 | TGPS = 1/fGPS; % Sample time [s]. 41 | 42 | g = -9.81; % gravity[m/s^2]. 43 | %% Correlation time [s]. 44 | % Rate gyros 45 | tauR(1,1) = 626.8115; 46 | tauR(2,1) = 6468.0515; 47 | tauR(3,1) = 602.5784; 48 | % Accelerometers 49 | tauA(1,1) = 1438.6558; 50 | tauA(2,1) = 3807.8042; 51 | tauA(3,1) = 1883.8307; 52 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 53 | %% Weighting of the Kalman filter 54 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 55 | % Variances of the Q matrice 56 | % AHRS - Attitude and heading reference system 57 | varQ1(1,1) = 2;%0.0001; 58 | varQ1(2,1) = 2;%0.0001; 59 | varQ1(3,1) = 2;%0.01; 60 | varQ1(4,1) = 2;%0.0001; 61 | varQ1(5,1) = 2;%0.0001; 62 | varQ1(6,1) = 2;%0.0001; 63 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 64 | % INS aided GPS - Inertial Navigation system aided GPS 65 | varQ2(1,1) = 10^3%0.00059539; 66 | varQ2(2,1) = 10^3%0.00062077; 67 | varQ2(3,1) = 200000%0.001261; 68 | varQ2(4,1) = 0.200000%1.5151e-6; 69 | varQ2(5,1) = 0.200000%2.181e-6; 70 | varQ2(6,1) = 0.200000%6.853e-6; 71 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 72 | % Variances of the R matrice 73 | % AHRS - Attitude and heading reference system 74 | varR1(1,1) = 5; 75 | varR1(2,1) = 5; 76 | varR1(3,1) = 5; 77 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 78 | % INS aided GPS - Inertial navigation system aided GPS 79 | varR2(1,1) = 0.1%2.591128492879236e008^2; 80 | varR2(2,1) = 0.1%2.591128492879236e008^2; 81 | varR2(3,1) = 0.000000001%0.00033^2; 82 | %varR2(4,1) = 0.1%0.5144^2; 83 | %varR2(5,1) = 0.1%0.5144^2; 84 | %varR2(6,1) = 0.1%0.5144^2; 85 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 86 | P1 = eye(6); 87 | P2 = eye(9)*1e-10; 88 | % load P_matrices 89 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 90 | sumDeltaz1= 0; 91 | sumDeltaz2= 0; 92 | magDec = 0.3604127; % Magnetic declination 93 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 94 | GPScont = 0; 95 | for k=1:n 96 | %% System time 97 | k 98 | t = (k-1)*TIMU; 99 | t_v(k) = t; 100 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 101 | %% IMU reading 102 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 103 | % Euler angles - IMU 104 | eangles.phi = e_imu_v(1,k+deltaIMU); 105 | eangles.theta = e_imu_v(2,k+deltaIMU); 106 | eangles.psi = e_imu_v(3,k+deltaIMU); 107 | eangles.phi = eangles.phi; 108 | eangles.theta = -eangles.theta; 109 | eangles.psi = -normalize_angle_f(eangles.psi +(magDec + pi/2),-pi); 110 | % Changing quaternion 111 | qIMU = euler2quat_f(eangles); 112 | qIMU_v(:,k) = qIMU; 113 | % Linear acceleration 114 | a = lin_accel_v(:,k+deltaIMU); 115 | a(2:3,1) = -a(2:3,1); % Changing axis 116 | a_a = a/g; 117 | a_a_v(:,k) = a_a; 118 | % Angular velocity 119 | omega_g = ang_vel_v(:,k+deltaIMU); 120 | omega_g(2:3,1) = -omega_g(2:3,1); % Changing axis 121 | omega_g_v(:,k) = omega_g; 122 | % Roll and pitch angles - accelerometers 123 | thetaAccel = normalize_angle_f(atan2(-a_a(1),sqrt(a_a(2)^2 + a_a(3)^2)),-pi); % Pitch accelerometer 124 | thetaAccel_v(k) = thetaAccel; % Pitch accelerometer vector 125 | phiAccel = normalize_angle_f(atan2(-a_a(2),-a_a(3))+pi,-pi); % Roll accelerometer 126 | phiAccel_v(k) = phiAccel; % Roll accelerometer vector 127 | 128 | % Euler angles normalization 129 | psiIMU = normalize_angle_f(eangles.psi,-pi); % Yaw IMU 130 | psiIMU_v(k) = psiIMU; % Yaw IMU vector 131 | thetaIMU = normalize_angle_f(eangles.theta,-pi); % Pitch IMU 132 | thetaIMU_v(k) = thetaIMU; % Pitch IMU vector 133 | phiIMU = normalize_angle_f(eangles.phi,-pi); % Roll IMU 134 | phiIMU_v(k) = phiIMU; % Roll IMU vector 135 | if k == 1 136 | qhat = euler2quat_f(eangles); 137 | eangleshat = eangles; 138 | 139 | psiHat = eangleshat.psi; 140 | thetaHat = eangleshat.theta; 141 | phiHat = eangleshat.phi; 142 | 143 | phiGyro = phiHat; 144 | psiGyro = psiHat; 145 | thetaGyro = thetaHat; 146 | end 147 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 148 | %% GPS reading 149 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 150 | %% Yaw angle obtained by the GPS 151 | if mod(k-1,stepGPS) == 0 152 | GPSstate = 1; % There is GPS data 153 | GPScont = GPScont +1; 154 | tGPS = t; 155 | tGPS_v(GPScont) = t; 156 | % Reading GPS data 157 | %psiGPS = normalize_angle_f(heading_v(GPScont + deltaGPS),-pi); % Yaw GPS 158 | %deltapsi = 0%psiGPS - eangles.psi; 159 | 160 | %psiGPS_v(GPScont) = psiGPS; % Yaw GPS vector 161 | latGPS = lat_v(GPScont + deltaGPS); 162 | latGPS_v(GPScont) = latGPS; 163 | lonGPS = lon_v(GPScont + deltaGPS); 164 | lonGPS_v(GPScont) = lonGPS; 165 | altGPS = alt_v(GPScont + deltaGPS); 166 | altGPS_v(GPScont) = altGPS; 167 | 168 | p = [latGPS;lonGPS;altGPS]; 169 | p_v(:,GPScont) = p; 170 | 171 | if GPScont == 1 172 | deltaT = 0; 173 | dlat = 0; 174 | dlon = 0; 175 | dalt = 0; 176 | 177 | latGPSprevious = latGPS; 178 | lonGPSprevious = lonGPS; 179 | altGPSprevious = altGPS; 180 | else 181 | %deltaT = TGPS; 182 | deltaT = (stamp_gps(GPScont + deltaGPS) - stamp_gps(GPScont + deltaGPS -1))/10^9; 183 | dlat = (latGPS - latGPSprevious)/deltaT; 184 | dlon = (lonGPS - lonGPSprevious)/deltaT; 185 | dalt = (altGPS - altGPSprevious)/deltaT; 186 | 187 | latGPSprevious = latGPS; 188 | lonGPSprevious = lonGPS; 189 | altGPSprevious = altGPS; 190 | end 191 | 192 | lambda = latGPS; 193 | h = altGPS ; 194 | 195 | Rlambda = Rlambda_m(lambda); 196 | Rphi = Rphi_m(lambda); 197 | 198 | vCalc = [(Rlambda + h) 0 0; 199 | 0 (Rphi + h)*cos(lambda) 0; 200 | 0 0 -1]* [dlat; dlon; dalt]; 201 | vCalc_v (:,k) = vCalc; 202 | 203 | v = [vCalc(1);vCalc(2);vCalc(3)]; 204 | 205 | v_v(:,GPScont) = v; 206 | 207 | pIG = p; 208 | vIG = v; 209 | else 210 | GPSstate = 0; % There is not GPS data 211 | end 212 | 213 | %% Filters 214 | if k == 1 215 | %Bias initial conditions 216 | bg = zeros(3,1); % Bias of the rate gyros 217 | ba = zeros(3,1); % Bias of the accelerometers 218 | 219 | % State initial conditions 220 | % x1 = [phit; theta; psi; bg]; 221 | 222 | x1 = [eangles.phi; eangles.theta; eangles.psi; bg] 223 | x2 = [p; v; ba]; 224 | end 225 | % AHRS - Attitude and heading reference system 226 | 227 | [sys_a] = sysEuler_f(omega_g, x1, varQ1, varR1, g, tauR, TIMU); 228 | if opt == 0 229 | 230 | [x1p, P1p] = ekf_p (x1,P1,sys_a); 231 | end 232 | 233 | z1 = [eangles.phi; eangles.theta; eangles.psi]; 234 | 235 | if opt == 0 236 | delta_z1 = normalize_angle_f(z1-sys_a.H*x1p,-pi); 237 | [x1,P1] = ekf_u(x1p, P1p, delta_z1, sys_a,1); 238 | end 239 | 240 | 241 | eangleshat.phi = normalize_angle_f(x1(1),-pi); 242 | eangleshat.theta = normalize_angle_f(x1(2),-pi); 243 | eangleshat.psi = normalize_angle_f(x1(3),-pi); 244 | bg = x1(4:6); 245 | 246 | % Quaternion 247 | qhat = euler2quat_f(eangleshat); 248 | qhat = qhat/norm(qhat); 249 | % INS aided GPS - Inertial Navigation system aided GPS 250 | %if GPSstate == 0 251 | % [pINS,vINS] = INS(x2(1:3), x2(4:6), qhat, a-ba, g, TIMU); 252 | % [p_INS,v_INS] = INS(x2(1:3,1),v, x1(1:4), a_a-ba, g,T); 253 | % [p_IG,v_IG] = GPS_IMU(p_IG,v_IG, x1(1:4), a-ba, g, T); 254 | % x2(1:3) = pINS; 255 | % x2(4:6) = vINS; 256 | %end 257 | if GPSstate == 1 258 | %%%%%%%%%%%%%%%%%%%%%% Position Reference System %%%%%%%%%%%%%%%%%%% 259 | [pHat,vHat] = INS(x2(1:3),x2(4:6), qhat, a-ba, g,TIMU); 260 | sysP = syspos4_m(pHat, vHat, qhat, varQ2, varR2, tauA, TIMU); 261 | if opt == 0 262 | [xp2, Pp2] = ekf_p (x2,P2,sysP); % Prediction 263 | end 264 | % hhat2 = xp2(1:6); 265 | %hhat2 = [pHat;vHat]; 266 | 267 | %z2(1:6,1) = [p; v]; % Position observation 268 | hhat2 = [pHat]; 269 | z2 = [p]; 270 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 271 | % Position reference system observation error 272 | deltaz2 = z2 - hhat2; 273 | deltaz2(1) = subtr_ang(z2(1),hhat2(1)); 274 | deltaz2(2) = subtr_ang(z2(2),hhat2(2)); 275 | 276 | sumDeltaz2 = deltaz2'*deltaz2*TIMU + sumDeltaz2; 277 | deltaz2_v(:,k) = deltaz2; % Observation position error 278 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 279 | if opt == 0 280 | [x2,P2] = ekf_u(xp2, Pp2, deltaz2, sysP,1); % Filtering 281 | end 282 | end 283 | p_IG_v(:,GPScont) = pIG; 284 | v_IG_v(:,GPScont) = vIG; 285 | % Accelerometer bias estimated 286 | ba = x2(7:9); 287 | ba_v(:,GPScont) = ba; 288 | latHat_v(GPScont) = x2(1); 289 | lonHat_v(GPScont) = x2(2); 290 | hHat_v(GPScont) = x2(3); 291 | vnHat_v(GPScont) = x2(4); 292 | veHat_v(GPScont) = x2(5); 293 | vdHat_v(GPScont) = x2(6); 294 | % Rate gyros bias estimated 295 | bg_v(:,k) = bg; 296 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 297 | psiHat = normalize_angle_f(eangleshat.psi, -pi); 298 | thetaHat = normalize_angle_f(eangleshat.theta, -pi); 299 | phiHat = normalize_angle_f(eangleshat.phi, -pi); 300 | psiHat_v(k) = psiHat; 301 | thetaHat_v(k) = thetaHat; 302 | phiHat_v(k) = phiHat; 303 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 304 | if k >1 305 | % % Bias correction 306 | % phiGyro = normalize_angle_f((omega_g(1) - bg(1))*TIMU + phiGyro,-pi); 307 | % thetaGyro = normalize_angle_f((omega_g(2) - bg(2))*TIMU + thetaGyro,-pi); 308 | % psiGyro = normalize_angle_f((omega_g(3) - bg(3))*TIMU + psiGyro,-pi); 309 | % Whithout bias correction 310 | phiGyro = normalize_angle_f((omega_g(1))*TIMU + phiGyro,-pi); 311 | thetaGyro = normalize_angle_f((omega_g(2))*TIMU + thetaGyro,-pi); 312 | psiGyro = normalize_angle_f((omega_g(3))*TIMU + psiGyro,-pi); 313 | 314 | end 315 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 316 | phiGyro_v(k) = phiGyro; 317 | thetaGyro_v(k) = thetaGyro; 318 | psiGyro_v(k) = psiGyro; 319 | 320 | 321 | end 322 | p_GPS_v = p_v; 323 | v_GPS_v = v_v; 324 | plot_fig 325 | drawnow 326 | -------------------------------------------------------------------------------- /malaga-urban-dataset-extract-04_all-sensors_GPS.txt: -------------------------------------------------------------------------------- 1 | % Time Lat Lon Alt fix #sats speed dir Local X Local Y Local Z rawlog ID Geocen X Geocen Y Geocen Z GPS X GPS Y GPS Z GPS VX GPS VY GPS VZ Local VX Local VY Local VZ SAT Time 2 | 1261229022.0000 0.6409001729632727 -0.0781901105352869 62.900000 1 9 0.000000 0.000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 306 5102986.0869294023141265 -399818.1670427707722411 3792647.0018258248455822 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229022.0000 3 | 1261229023.0000 0.6409011328943615 -0.0781901105352869 64.400000 1 9 0.000000 0.000000 -0.0000000000592960 6.1083373422671396 1.4803601157525099 697 5102983.6473257681354880 -399817.9759002051432617 3792652.7910442338325083 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229023.0000 4 | 1261229024.0000 0.6409020637366291 -0.0781899360023617 64.700000 1 9 0.000000 0.000000 0.8933675858635053 12.0278611474366350 1.7613156901985025 1087 5102980.4288184484466910 -399816.8276252091163769 3792657.7145063211210072 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229024.0000 5 | 1261229025.0000 0.6409028491347926 -0.0781899941800034 66.100000 1 9 0.000000 0.000000 0.5955781737173643 17.0261512107906192 3.1452368166572180 1475 5102978.5475369580090046 -399816.9789291300112382 3792662.5544140529818833 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229025.0000 6 | 1261229026.0000 0.6409035472664933 -0.0781899941800034 66.700000 1 9 0.000000 0.000000 0.5955779208623551 21.4670028728674964 3.7309444945151498 1868 5102976.3809789372608066 -399816.8091798993409611 3792666.4712276188656688 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229026.0000 7 | 1261229027.0000 0.6409043035758357 -0.0781898487358991 65.900000 1 9 0.000000 0.000000 1.3400494014480555 26.2732573219629302 2.9154650505114343 2256 5102972.9333176501095295 -399815.7923031458049081 3792669.8474562331102788 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229027.0000 8 | 1261229028.0000 0.6409050017075366 -0.0781897905582574 65.600000 1 9 0.000000 0.000000 1.6378372311563558 30.7112110354762962 2.6011708682406365 2640 5102970.0708188889548182 -399815.2693263665423729 3792673.2261375910602510 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229028.0000 9 | 1261229029.0000 0.6409055543951331 -0.0781897614694365 65.300000 1 9 0.000000 0.000000 1.7867307075488306 34.2243894267053506 2.2898527830918525 3030 5102967.7479440579190850 -399814.9379797012079507 3792675.8635553685016930 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229029.0000 10 | 1261229030.0000 0.6409059034609835 -0.0781897614694365 65.000000 1 9 0.000000 0.000000 1.7867301603602965 36.4428826644742685 1.9827041174019229 3417 5102966.1851969705894589 -399814.8155392585904337 3792677.4632060891017318 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229030.0000 11 | 1261229031.0000 0.6409060779939086 -0.0781897323806156 64.700000 1 9 0.000000 0.000000 1.9356239984404815 37.5516459475411111 1.6791302295204531 3810 5102965.2955875732004642 -399814.5964883544947952 3792678.1733429208397865 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229031.0000 12 | 1261229032.0000 0.6409061361715503 -0.0781897614694365 64.500000 1 9 0.000000 0.000000 1.7867297116030143 37.9209114431484693 1.4779394672054806 4198 5102964.9036337034776807 -399814.7151294258655980 3792678.3502629534341395 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229032.0000 13 | 1261229033.0000 0.6409061652603713 -0.0781897905582574 64.200000 1 9 0.000000 0.000000 1.6378354566228199 38.1048997640686196 1.1773451349412021 4588 5102964.5420206338167191 -399814.8361476622521877 3792678.3191385529935360 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229033.0000 14 | 1261229034.0000 0.6409061652603713 -0.0781897905582574 64.200000 1 9 0.000000 0.000000 1.6378354566228199 38.1048997640686196 1.1773451349412021 4982 5102964.5420206338167191 -399814.8361476622521877 3792678.3191385529935360 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229034.0000 15 | 1261229035.0000 0.6409064852374009 -0.0781897032917948 64.500000 1 9 0.000000 0.000000 2.0845174564141056 40.1403710057458625 1.4707878641481713 5365 5102963.6038776468485594 -399814.3145933798514307 3792680.1292892335914075 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229035.0000 16 | 1261229036.0000 0.6409068343032512 -0.0781895287588696 64.200000 1 9 0.000000 0.000000 2.9778811686521669 42.3588639609595319 1.1636367795886287 5755 5102962.1109098857268691 -399813.3015180256916210 3792681.7289382871240377 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229036.0000 17 | 1261229037.0000 0.6409072997243851 -0.0781890924265566 64.400000 1 9 0.000000 0.000000 5.2112904070464801 45.3187888576182729 1.3540967169040670 6148 5102960.6811582287773490 -399810.9492433427367359 3792684.2205564808100462 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229037.0000 18 | 1261229038.0000 0.6409077942343397 -0.0781884524724975 64.700000 1 9 0.000000 0.000000 8.4869559444498535 48.4639928234052562 1.6439564393713262 6537 5102959.3024727404117584 -399807.5555192332249135 3792686.9202183764427900 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229038.0000 19 | 1261229039.0000 0.6409083469219362 -0.0781876961631550 65.000000 1 9 0.000000 0.000000 12.3581945143365584 51.9791091286104034 1.9326191328557378 6926 5102957.7497994918376207 -399803.5507649525534362 3792689.9163835570216179 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229039.0000 20 | 1261229040.0000 0.6409089286983536 -0.0781869689426333 65.400000 1 9 0.000000 0.000000 16.0805363182768133 55.6795047049044385 2.3206809470490910 7318 5102956.1551502421498299 -399799.6920749415876344 3792693.1205922062508762 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229040.0000 21 | 1261229041.0000 0.6409096268300545 -0.0781862417221116 65.200000 1 8 0.000000 0.000000 19.8028717996685018 60.1177870789576332 2.1063553051038468 7713 5102953.6400263961404562 -399795.7612722309422679 3792696.5590525357984006 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229041.0000 22 | 1261229042.0000 0.6409104413170387 -0.0781857472121569 65.200000 1 8 0.000000 0.000000 22.3340524283977580 65.2965327970354963 2.0896407864051909 8099 5102950.7506738482043147 -399792.9959540886338800 3792700.7101029315963387 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229042.0000 23 | 1261229043.0000 0.6409113139816647 -0.0781855435904108 65.500000 1 8 0.000000 0.000000 23.3762941548996608 70.8461532310937798 2.3717313593728093 8488 5102947.7642483916133642 -399791.7165322823566385 3792705.3370320107787848 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229043.0000 24 | 1261229044.0000 0.6409122157351115 -0.0781856308568735 65.900000 1 8 0.000000 0.000000 22.9295994534182448 76.5810480560472513 2.7532248814735993 8875 5102944.6311817783862352 -399791.9191206657560542 3792710.1720029166899621 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229044.0000 25 | 1261229045.0000 0.6409130593109166 -0.0781859217450821 66.100000 1 7 0.000000 0.000000 21.4406516616091203 81.9453851768038106 2.9359122614178688 9263 5102941.4773886790499091 -399793.1655323666054755 3792714.5908828550018370 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229045.0000 26 | 1261229046.0000 0.6409138156202592 -0.0781865326103203 66.600000 1 8 0.000000 0.000000 18.3138811070400180 86.7558231224897014 3.4203901885460652 9651 5102938.7661540675908327 -399796.0894605774665251 3792718.7443846175447106 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229046.0000 27 | 1261229047.0000 0.6409144264854973 -0.0781873180084837 66.500000 1 8 0.000000 0.000000 14.2937541548702090 90.6395480641870392 3.3078555100546936 10042 5102936.0569391427561641 -399799.9096412554499693 3792721.7978708655573428 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229047.0000 28 | 1261229048.0000 0.6409150373507356 -0.0781881034066471 66.900000 1 9 0.000000 0.000000 10.2736317810286089 94.5248902275919534 3.6953133136162037 10432 5102933.7472695922479033 -399803.7611216590739787 3792725.1503201858140528 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229048.0000 29 | 1261229049.0000 0.6409155609495111 -0.0781888888048105 66.600000 1 9 0.000000 0.000000 6.2535122713203446 97.8531076289824568 3.3845628959773109 10826 5102931.2089835004881024 -399807.5946871193009429 3792727.6394640835933387 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229049.0000 30 | 1261229050.0000 0.6409161427259285 -0.0781895869365113 66.400000 1 7 0.000000 0.000000 2.6800754456144222 101.5515599789343071 3.1726119592551285 11215 5102928.5649876622483134 -399810.9719186258735135 3792730.4849020270630717 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229050.0000 31 | 1261229051.0000 0.6409168990352710 -0.0781901105352869 67.400000 1 6 0.000000 0.000000 -0.0000000000255032 106.3636229782868554 4.1570632482520580 11604 5102926.2881696652621031 -399813.4818192365346476 3792734.9373617717064917 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229051.0000 32 | 1261229052.0000 0.6409177716998969 -0.0781903723346747 68.100000 1 5 0.000000 0.000000 -1.3400364583063702 111.9145382851373824 4.8391194830329027 11995 5102923.4352760314941406 -399814.6024386686622165 3792739.8034453266300261 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229052.0000 33 | 1261229053.0000 0.6409186443645231 -0.0781904305123164 68.800000 1 5 0.000000 0.000000 -1.6378214558411806 117.4654547930424684 5.5211709430912990 12388 5102920.6637886045500636 -399814.6839906646055169 3792744.6695270040072501 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 1261229053.0000 34 | --------------------------------------------------------------------------------