diff --git a/examples/allan-variance/stim300.mat b/examples/allan-variance/stim300.mat index 7ed80ef..c2540b4 100755 Binary files a/examples/allan-variance/stim300.mat and b/examples/allan-variance/stim300.mat differ diff --git a/examples/real-data/mpu6000_imu.mat b/examples/real-data/mpu6000_imu.mat index 7113f8f..830d521 100755 Binary files a/examples/real-data/mpu6000_imu.mat and b/examples/real-data/mpu6000_imu.mat differ diff --git a/examples/real-data/navego_example_real_mpu6000.m b/examples/real-data/navego_example_real_mpu6000.m index 925294a..c247317 100755 --- a/examples/real-data/navego_example_real_mpu6000.m +++ b/examples/real-data/navego_example_real_mpu6000.m @@ -107,7 +107,7 @@ load ekinox_gnss % ekinox_gnss contains the lever arm with respect to Ekinox IMU. -% ekinox_gnss.larm has to be changed for MPU-6000 IMU +% ekinox_gnss.larm has to be changed for MPU-6000 IMU. ekinox_gnss.larm = [-0.369, 0.0, -0.219]'; ekinox_gnss.eps = mean(diff(mpu6000_imu.t)) / 2; % A rule of thumb for choosing eps. @@ -126,7 +126,7 @@ % Execute INS/GNSS integration % --------------------------------------------------------------------- - nav_mpu6000 = ins_gnss(mpu6000_imu, ekinox_gnss, 'quaternion'); + nav_mpu6000 = ins_gnss(mpu6000_imu, ekinox_gnss, 'dcm'); % --------------------------------------------------------------------- save nav_mpu6000.mat nav_mpu6000 diff --git a/examples/synthetic-data/navego_example_synth.m b/examples/synthetic-data/navego_example_synth.m index 0d9017d..225bfda 100755 --- a/examples/synthetic-data/navego_example_synth.m +++ b/examples/synthetic-data/navego_example_synth.m @@ -1,6 +1,6 @@ % navego_example_synth: example of how to use NaveGo to generate -% both IMU and GNSS synthetic (simulated) data. Then, synthetic data is -% fused. +% both IMU and GNSS synthetic (simulated) data. Then, sensors synthetic +% data is fused. % % The main goal is to compare two INS/GNSS systems performances, one using % a synthetic ADIS16405 IMU and synthetic GNSS, and another using a diff --git a/ins-gnss/F_update.m b/ins-gnss/F_update.m index 0e666a3..04f3aa7 100755 --- a/ins-gnss/F_update.m +++ b/ins-gnss/F_update.m @@ -174,15 +174,15 @@ % Eq. 14.63 from Groves F = [F11 F12 F13 DCMbn Z ; - F21 F22 F23 Z -DCMbn ; + F21 F22 F23 Z DCMbn ; F31 F32 F33 Z Z ; Z Z Z Fgg Z ; Z Z Z Z Faa ; ]; -% Eq. Eq. 11.108 from Farrell +% Eq. 11.108 from Farrell G = [DCMbn Z Z Z ; - Z -DCMbn Z Z ; + Z DCMbn Z Z ; Z Z Z Z ; Z Z Fbg Z ; Z Z Z Fba ; diff --git a/ins-gnss/ins_gnss.m b/ins-gnss/ins_gnss.m index 0612a16..7265338 100755 --- a/ins-gnss/ins_gnss.m +++ b/ins-gnss/ins_gnss.m @@ -97,13 +97,16 @@ % Groves, P.D. (2013), Principles of GNSS, Inertial, and % Multisensor Integrated Navigation Systems (2nd Ed.). Artech House. % +% Crassidis, J.L. and Junkins, J.L. (2011). Optimal Esti- +% mation of Dynamic Systems, 2nd Ed. Chapman and Hall/CRC, USA. +% % ZUPT algothim based on Groves, Chapter 15, "INS Alignment, Zero Updates, % and Motion Constraints". % % ins_gps.m, ins_gnss function is based on that previous NaveGo function. % -% Version: 010 -% Date: 2022/03/06 +% Version: 011 +% Date: 2022/04/0 % Author: Rodrigo Gonzalez % URL: https://github.com/rodralez/navego @@ -367,11 +370,15 @@ %% INS/GNSS CORRECTIONS - % Quaternion corrections - % Crassidis. Eq. 7.34 and A.174a. - antm = [0.0 qua(3) -qua(2); -qua(3) 0.0 qua(1); qua(2) -qua(1) 0.0]; - qua = qua + 0.5 .* [qua(4)*eye(3) + antm; -1.*[qua(1) qua(2) qua(3)]] * kf.xp(1:3); - qua = qua / norm(qua); % Brute-force normalization + % Quaternion correction + qua_skew = -skewm(qua(1:3)); % According to Crassidis, qua_skew should be + % positive, but if positive NaveGo diverges. + % Crassidis A.174a + Xi = [qua(4)*eye(3) + qua_skew; -qua(1:3)']; + + % Crassidis. Eq. 7.34 + qua = qua + 0.5 .* Xi * kf.xp(1:3); + qua = qua / norm(qua); % Brute-force normalization % DCM correction DCMbn = qua2dcm(qua); diff --git a/ins/qua_update.m b/ins/qua_update.m index e9614ba..ff4a0cc 100755 --- a/ins/qua_update.m +++ b/ins/qua_update.m @@ -29,17 +29,12 @@ % References: % -% R. Gonzalez, J. Giribet, and H. Patiño. An approach to -% benchmarking of loosely coupled low-cost navigation systems, -% Mathematical and Computer Modelling of Dynamical Systems, vol. 21, -% issue 3, pp. 272-287, 2015. Eq. 13. -% % Crassidis, J.L. and Junkins, J.L. (2011). Optimal Esti- % mation of Dynamic Systems, 2nd Ed. Chapman and Hall/CRC, USA. -% Eq. 7.39 and 7.40, p. 458. +% Eq. 7.39 to 7.41, p. 458. % -% Version: 004 -% Date: 2021/03/18 +% Version: 005 +% Date: 2022/04/07 % Author: Rodrigo Gonzalez % URL: https://github.com/rodralez/navego @@ -53,20 +48,14 @@ co = cos(0.5*wnorm*dt); si = sin(0.5*wnorm*dt); - n1 = wb(1) / wnorm; - n2 = wb(2) / wnorm; - n3 = wb(3) / wnorm; - - qw1 = n1*si; - qw2 = n2*si; - qw3 = n3*si; - qw4 = co; + % Eq. 7.41 + psi = (si / wnorm) * wb; - Om=[ qw4 qw3 -qw2 qw1; - -qw3 qw4 qw1 qw2; - qw2 -qw1 qw4 qw3; - -qw1 -qw2 -qw3 qw4]; + % Eq. 7.40 + Om = [ (co*eye(3)-skewm(psi)) psi; % 3x4 + -psi' co]; % 1x4 + % Eq. 7.39 qua = Om * qua; end