Merge pull request #148 from rodralez/develop

Master branch update

README.md

@@ -180,37 +180,39 @@ Please, check the folder `examples/visual-data/`.
 
 # Researchers who are using NaveGo
 
-The following 15 papers have expressed the use of NaveGo for research:
+The following 16 papers have expressed the use of NaveGo for research:
 
-1. Ren, X.; Sun, M.; Zhang, X.; Liu, L.; Wang, X.; Zhou, H. An AR Geo-Registration Algorithm for UAV TIR Video Streams Based on Dual-Antenna RTK-GPS. Remote Sens. 2022, 14, 2205. https://doi.org/10.3390/rs14092205. This paper use NaveGo to improve the geographic registration (geo-registration) accuracy by fusing the positioning and heading data from the dual-antenna real-time kinematic global positioning system (RTK-GPS) with onboard internal measurement unit (IMU) data.
+1. Paul Rawiel (2022). Positioning of Pedelecs for a Pedelec Sharing System with Free-Floating Bikes. In: Coors, V., Pietruschka, D., Zeitler, B. (eds) iCity. Transformative Research for the Livable, Intelligent, and Sustainable City. Springer, Cham. [link](https://doi.org/10.1007/978-3-030-92096-8_5)
 
-2. Johann Diep et al. Investigating the Performance of LCNS with Visual-Inertial Odometry for Lunar Rover Navigation. NAVITEC 2022 conference, April 2022. NaveGo is used as part of a VISUAL/INS/GNSS navigation system. [Youtube video](https://youtu.be/sPnPMBs1gSU).
+2. Ren, X.; Sun, M.; Zhang, X.; Liu, L.; Wang, X.; Zhou, H. An AR Geo-Registration Algorithm for UAV TIR Video Streams Based on Dual-Antenna RTK-GPS. Remote Sens. 2022, 14, 2205. https://doi.org/10.3390/rs14092205. This paper use NaveGo to improve the geographic registration (geo-registration) accuracy by fusing the positioning and heading data from the dual-antenna real-time kinematic global positioning system (RTK-GPS) with onboard internal measurement unit (IMU) data.
 
-3. Canhui Tao, Zhiping Song, and Zhenping Weng. MCTLS-Assisted Completed SINS/GPS Integrated and Applied to Low-Cost Attitude and Heading Reference System. Mathematical Problems in Engineering 2021 (2021). [Link](https://www.hindawi.com/journals/mpe/2021/4260162/). NaveGo is used as a benchmark for comparing a proposed heading determination approach.
+3. Johann Diep et al. Investigating the Performance of LCNS with Visual-Inertial Odometry for Lunar Rover Navigation. NAVITEC 2022 conference, April 2022. NaveGo is used as part of a VISUAL/INS/GNSS navigation system. [Youtube video](https://youtu.be/sPnPMBs1gSU).
 
-4. Nagui, N., Attallah, O., Zaghloul, M.S. et al. Improved GPS/IMU Loosely Coupled Integration Scheme Using Two Kalman Filter-based Cascaded Stages. Arab J Sci Eng 46, 1345–1367 (2021). [Link](https://doi.org/10.1007/s13369-020-05144-8). Authors use NaveGo as a benchmark for a new proposed integrated navigation scheme.
+4. Canhui Tao, Zhiping Song, and Zhenping Weng. MCTLS-Assisted Completed SINS/GPS Integrated and Applied to Low-Cost Attitude and Heading Reference System. Mathematical Problems in Engineering 2021 (2021). [Link](https://www.hindawi.com/journals/mpe/2021/4260162/). NaveGo is used as a benchmark for comparing a proposed heading determination approach.
 
-5. Benjamin Noack, Christopher Funk, Susanne Radtke,and Uwe D. Hanebeck. State Estimation with Event-Based Inputs Using Stochastic Triggers. First Virtual IFAC World Congress (IFAC-V 2020). Germany, July 11-17, 2020 [Link](https://isas.iar.kit.edu/pdf/IFAC20_Noack.pdf).
+5. Nagui, N., Attallah, O., Zaghloul, M.S. et al. Improved GPS/IMU Loosely Coupled Integration Scheme Using Two Kalman Filter-based Cascaded Stages. Arab J Sci Eng 46, 1345–1367 (2021). [Link](https://doi.org/10.1007/s13369-020-05144-8). Authors use NaveGo as a benchmark for a new proposed integrated navigation scheme.
 
-6. W. Sun, J. Wu, W. Ding and S. Duan. A Robust Indirect Kalman Filter Based on the Gradient Descent Algorithm for Attitude Estimation During Dynamic Conditions, in IEEE Access, vol. 8, pp. 96487-96494, 2020, doi: 10.1109/ACCESS.2020.2997250.
+6. Benjamin Noack, Christopher Funk, Susanne Radtke,and Uwe D. Hanebeck. State Estimation with Event-Based Inputs Using Stochastic Triggers. First Virtual IFAC World Congress (IFAC-V 2020). Germany, July 11-17, 2020 [Link](https://isas.iar.kit.edu/pdf/IFAC20_Noack.pdf).
 
-7. R. Rabiee, X. Zhong, Y. Yan and W.P. Tay. LaIF: A Lane-Level Self-Positioning Scheme for Vehicles in GNSS-Denied Environments, in IEEE Transactions on Intelligent Transportation Systems, vol. 20, no. 8, pp. 2944-2961, August 2019. doi: 10.1109/TITS.2018.2870048. [Link](https://ieeexplore.ieee.org/document/8489926). NaveGo is used as a benchmark to assess a proposed fusion algorithm based on a particle filter to achieve lane-level tracking accuracy under a GNSS-denied environment.
+7. W. Sun, J. Wu, W. Ding and S. Duan. A Robust Indirect Kalman Filter Based on the Gradient Descent Algorithm for Attitude Estimation During Dynamic Conditions, in IEEE Access, vol. 8, pp. 96487-96494, 2020, doi: 10.1109/ACCESS.2020.2997250.
 
-8. O. Tokluoğlu and E. Çavuş. Study of Utilizing Multiple IMUs for Inertial Navigation Systems Without GPS Aid, 2019 1st Global Power, Energy and Communication Conference (GPECOM), Nevsehir, Turkey, June 2019, pp. 86-89. doi: 10.1109/GPECOM.2019.8778612.  [Link](https://ieeexplore.ieee.org/abstract/document/8778612). The purpose of NaveGo in this work is to test the performance of an INS/GNSS system with multiple IMUs.
+8. R. Rabiee, X. Zhong, Y. Yan and W.P. Tay. LaIF: A Lane-Level Self-Positioning Scheme for Vehicles in GNSS-Denied Environments, in IEEE Transactions on Intelligent Transportation Systems, vol. 20, no. 8, pp. 2944-2961, August 2019. doi: 10.1109/TITS.2018.2870048. [Link](https://ieeexplore.ieee.org/document/8489926). NaveGo is used as a benchmark to assess a proposed fusion algorithm based on a particle filter to achieve lane-level tracking accuracy under a GNSS-denied environment.
 
-9. Bac Nghia Vu, Khanh Nhu Nguyen and Mung Huy Vu. Practical Considerations of IMU Data Generator, 2019 3rd International Conference on Recent Advances in Signal Processing, Telecommunications & Computing (SigTelCom), Hanoi, Vietnam, March 2019, pp. 63-68. doi: 10.1109/SIGTELCOM.2019.8696196. [Link](https://ieeexplore.ieee.org/abstract/document/8696196/). NaveGo is used to simulate gyros data. Then, these data is compared to the output of a proposed method for the same goal.
+9. O. Tokluoğlu and E. Çavuş. Study of Utilizing Multiple IMUs for Inertial Navigation Systems Without GPS Aid, 2019 1st Global Power, Energy and Communication Conference (GPECOM), Nevsehir, Turkey, June 2019, pp. 86-89. doi: 10.1109/GPECOM.2019.8778612.  [Link](https://ieeexplore.ieee.org/abstract/document/8778612). The purpose of NaveGo in this work is to test the performance of an INS/GNSS system with multiple IMUs.
 
-10. Mohamed Atia. Design and simulation of sensor fusion using symbolic engines. Mathematical and Computer Modelling of Dynamical Systems 25.1 (2019): 40-62. February 2019. [Link](https://www.tandfonline.com/doi/abs/10.1080/13873954.2019.1566266). This work proposes a simulation framework for inertial sensors and an Attitude and Heading Reference System (AHRS). Atia uses the same true data that NaveGo provides (see Fig. 7) as true data input to simulate sensors. Unfortunately, Atias' simulator and NaveGo performances are not compared in this work.
+10. Bac Nghia Vu, Khanh Nhu Nguyen and Mung Huy Vu. Practical Considerations of IMU Data Generator, 2019 3rd International Conference on Recent Advances in Signal Processing, Telecommunications & Computing (SigTelCom), Hanoi, Vietnam, March 2019, pp. 63-68. doi: 10.1109/SIGTELCOM.2019.8696196. [Link](https://ieeexplore.ieee.org/abstract/document/8696196/). NaveGo is used to simulate gyros data. Then, these data is compared to the output of a proposed method for the same goal.
 
-11. Ren, X., Sun, M., Jiang, C., Liu, L., & Huang, W. (2018). An Augmented Reality Geo-Registration Method for Ground Target Localization from a Low-Cost UAV Platform. Sensors, October 2018, vol. 18, no 11, p. 3739. [Link](https://www.mdpi.com/1424-8220/18/11/3739/htm). NaveGo is used to process RTK GPS data in the context of an INS/GNSS system for geo-registration and target localization.
+11. Mohamed Atia. Design and simulation of sensor fusion using symbolic engines. Mathematical and Computer Modelling of Dynamical Systems 25.1 (2019): 40-62. February 2019. [Link](https://www.tandfonline.com/doi/abs/10.1080/13873954.2019.1566266). This work proposes a simulation framework for inertial sensors and an Attitude and Heading Reference System (AHRS). Atia uses the same true data that NaveGo provides (see Fig. 7) as true data input to simulate sensors. Unfortunately, Atias' simulator and NaveGo performances are not compared in this work.
 
-12. M. Pachwicewicz and J. Weremczuk. Accuracy Estimation of the Sounding Rocket Navigation System. 2018 XV International Scientific Conference on Optoelectronic and Electronic Sensors (COE), Warsaw, June 2018, pp. 1-4. doi: 10.1109/COE.2018.8435180. [Link](https://ieeexplore.ieee.org/abstract/document/8435180). In this paper NaveGo is used as a simulation framework for three types of IMUs.
+12. Ren, X., Sun, M., Jiang, C., Liu, L., & Huang, W. (2018). An Augmented Reality Geo-Registration Method for Ground Target Localization from a Low-Cost UAV Platform. Sensors, October 2018, vol. 18, no 11, p. 3739. [Link](https://www.mdpi.com/1424-8220/18/11/3739/htm). NaveGo is used to process RTK GPS data in the context of an INS/GNSS system for geo-registration and target localization.
 
-13. M.G. Deepika and A. Arun. Analysis of INS Parameters and Error Reduction by Integrating GPS and INS Signals. 2018 International Conference on Design Innovations for 3Cs Compute Communicate Control (ICDI3C), Bangalore, April 2018, pp. 18-23. doi: 10.1109/ICDI3C.2018.00013. [Link](https://ieeexplore.ieee.org/abstract/document/8436621). This work is completely based on the synthetic data example provided by NaveGo. It is not clear what the contribution of this paper is.
+13. M. Pachwicewicz and J. Weremczuk. Accuracy Estimation of the Sounding Rocket Navigation System. 2018 XV International Scientific Conference on Optoelectronic and Electronic Sensors (COE), Warsaw, June 2018, pp. 1-4. doi: 10.1109/COE.2018.8435180. [Link](https://ieeexplore.ieee.org/abstract/document/8435180). In this paper NaveGo is used as a simulation framework for three types of IMUs.
 
-14. P.K. Diamantidis. Attitude Navigation using a Sigma-Point Kalman Filter in an Error State Formulation. Dissertation for Master degree. Department of Space and Plasma Physics, School of Electrical Engineering, KTH Royal Institute of Technology, Stockholm, Sweden. 2017. [Link](http://www.diva-portal.org/smash/get/diva2:1141205/FULLTEXT01.pdf). A 30-minutes static measurement of a gyroscope was made with and its Allan Variance plot is presented by using the NaveGo functions.
+14. M.G. Deepika and A. Arun. Analysis of INS Parameters and Error Reduction by Integrating GPS and INS Signals. 2018 International Conference on Design Innovations for 3Cs Compute Communicate Control (ICDI3C), Bangalore, April 2018, pp. 18-23. doi: 10.1109/ICDI3C.2018.00013. [Link](https://ieeexplore.ieee.org/abstract/document/8436621). This work is completely based on the synthetic data example provided by NaveGo. It is not clear what the contribution of this paper is.
 
-15. Shaoxing Hu, Shike Xu 1, Duhu Wang and Aiwu Zhang. Optimization Algorithm for Kalman Filter Exploiting the Numerical Characteristics of SINS/GPS Integrated Navigation Systems.  Sensors 2015, 15(11), 28402-28420. [Link](https://www.mdpi.com/117278). The mathematical model of this work is base on NaveGo's proposed mathematical model.
+15. P.K. Diamantidis. Attitude Navigation using a Sigma-Point Kalman Filter in an Error State Formulation. Dissertation for Master degree. Department of Space and Plasma Physics, School of Electrical Engineering, KTH Royal Institute of Technology, Stockholm, Sweden. 2017. [Link](http://www.diva-portal.org/smash/get/diva2:1141205/FULLTEXT01.pdf). A 30-minutes static measurement of a gyroscope was made with and its Allan Variance plot is presented by using the NaveGo functions.
+
+16. Shaoxing Hu, Shike Xu 1, Duhu Wang and Aiwu Zhang. Optimization Algorithm for Kalman Filter Exploiting the Numerical Characteristics of SINS/GPS Integrated Navigation Systems.  Sensors 2015, 15(11), 28402-28420. [Link](https://www.mdpi.com/117278). The mathematical model of this work is base on NaveGo's proposed mathematical model.
 
 # Acknowledgments
 

simulation/acc_gen.m

@@ -41,8 +41,8 @@
 %   Thinking about accelerometers and gravity by Dave Redell
 % http://www.lunar.org/docs/LUNARclips/v5/v5n1/Accelerometers.html
 %
-% Version: 011
-% Date:    2022/01/25
+% Version: 012
+% Date:    2022/08/22
 % Author:  Rodrigo Gonzalez <[email protected]>
 % URL:     https://github.com/rodralez/navego
 
@@ -77,18 +77,18 @@
 %% SIMULATION OF GRAVITY AND CORIOLIS
 
 % Gravity and Coriolis in nav-ref
-grav_n = -gravity(ref.lat, ref.h);              % Accelerometer in Z axis senses an 
+g_n = -gravity(ref.lat, ref.h);              % Accelerometer in Z axis senses an 
                                                 % acceleration of 1.0 G straight up
 cor_n = coriolis(ref.lat, ref.vel, ref.h);
 
 % Gravity and Coriolis from nav-ref to body-ref
-grav_b = zeros(M);
+g_b = zeros(M);
 cor_b = zeros(M);
 for i = 1:N
-    dcm_nb = reshape(ref.DCMnb_m(i,:), 3, 3);
-    gb = dcm_nb * grav_n(i,:)';
-    corb =  dcm_nb * cor_n(i,:)';
-    grav_b(i,:) = gb';
+    dcmnb = reshape(ref.DCMnb_m(i,:), 3, 3);
+    gb = dcmnb * g_n(i,:)';
+    corb =  dcmnb * cor_n(i,:)';
+    g_b(i,:) = gb';
     cor_b(i,:) = corb';
 end
 
@@ -103,11 +103,11 @@
 % Simulation of white noise
 
 wn = randn(M);
-a_wn = zeros(M);
+acc_wn = zeros(M);
 
 for i=1:3
 
-    a_wn(:, i) = imu.a_std(i).* wn(:,i);
+    acc_wn(:, i) = imu.a_std(i).* wn(:,i);
 end
 
 % -------------------------------------------------------------------------
@@ -119,10 +119,10 @@
 % -------------------------------------------------------------------------
 % Simulation of rate random walk
 
-a_rrw = noise_rrw (imu.vrrw, dt, M);
+acc_rrw = noise_rrw (imu.vrrw, dt, M);
 
 % -------------------------------------------------------------------------
 
-fb_sim = acc_b + cor_b + grav_b + a_wn + ab_sta + ab_dyn + a_rrw;
+fb_sim = acc_b + cor_b + g_b + acc_wn + ab_sta + ab_dyn + acc_rrw;
 
 end

simulation/gyro_gen.m

@@ -7,7 +7,7 @@
 %	imu, data structure with IMU error profile.
 %
 % OUTPUT
-%	wb_sim, Nx3 matrix with simulated gryos in the body frame [X Y Z] 
+%	wb_sim, Nx3 matrix with simulated gyros in the body frame [X Y Z] 
 %     (rad, rad, rad).
 %
 %   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
@@ -38,8 +38,8 @@
 %   Aggarwal, P. et al. MEMS-Based Integrated Navigation. Artech
 % House. 2010.
 %
-% Version: 009
-% Date:    2022/01/25
+% Version: 010
+% Date:    2022/08/22
 % Author:  Rodrigo Gonzalez <[email protected]>
 % URL:     https://github.com/rodralez/navego
 
@@ -64,14 +64,15 @@
 
 %% SIMULATION OF TRANSPORTE AND EARTH RATES
 
-g_err_b = zeros(M);
+om_b = zeros(M);
 for i = 1:N
 
     dcmnb = reshape(ref.DCMnb_m(i,:), 3, 3);
     omega_ie_n = earth_rate(ref.lat(i));
     omega_en_n = transport_rate(ref.lat(i), ref.vel(i,1), ref.vel(i,2), ref.h(i));
-    omega_in_b = dcmnb * (omega_en_n + omega_ie_n );
-    g_err_b(i,:) = ( omega_in_b * gyro_b (i,:)' )';
+    om_ie_n = skewm_inv(omega_ie_n);
+    om_en_n = skewm_inv(omega_en_n);
+    om_b(i,:)  = (dcmnb * (om_ie_n + om_en_n))';
 end
 
 %% SIMULATION OF NOISES
@@ -85,11 +86,11 @@
 % Simulation of white noise
 
 wn = randn(M);
-g_wn = zeros(M);
+gyro_wn = zeros(M);
 
 for i=1:3
 
-    g_wn(:, i) = imu.g_std(i) .*  wn(:,i);
+    gyro_wn(:, i) = imu.g_std(i) .*  wn(:,i);
 end
 
 % -------------------------------------------------------------------------
@@ -101,10 +102,10 @@
 % -------------------------------------------------------------------------
 % Simulation of rate random walk
 
-g_rrw = noise_rrw (imu.arrw, dt, M);
+gyro_rrw = noise_rrw (imu.arrw, dt, M);
 
 % -------------------------------------------------------------------------
 
-wb_sim = gyro_b + g_err_b + g_wn + gb_sta + gb_dyn + g_rrw;
+wb_sim = gyro_b + om_b + gyro_wn + gb_sta + gb_dyn + gyro_rrw;
 
 end