Merge pull request #124 from rodralez/develop

Merging from develop to master
rodralez · Apr 16, 2022 · 24d9488 · 24d9488
2 parents c1afcb6 + 259f2d6
commit 24d9488
Show file tree

Hide file tree

Showing 6 changed files with 30 additions and 34 deletions.
diff --git a/examples/real-data/mpu6000_imu.mat b/examples/real-data/mpu6000_imu.mat
diff --git 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
@@ -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
@@ -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
@@ -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 <[email protected]>
 % 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
@@ -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 <[email protected]>
 % 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