Abstract
We propose a low-cost integrated navigation system to enhance navigation accuracy using multiple sensors such as the inertial measurement unit (IMU) and Global Positioning System (GPS). Due to the heterogeneity of the employed sensors, the suggested navigation system is equipped with a graph-based optimizer formulated as a maximum a posteriori estimator. As already known, graphic optimization methods are computationally more complex. To reduce this complexity, high-frequency IMU measurements are pre-integrated between lower-frequency GPS measurements. These pre-integrated measurements can be utilized in conjunction with the magnetometer and barometer sensor measurements to carry out navigation in case of GPS weak signal reception. The resulting optimization graph will be solved using the marginalization technique to attain a less computationally intensive optimization favorable in real-time applications. Incorporating internal sensors in navigation will come with problems like the initialization phase. This problem is tackled by integrating the inertial measurements between the required states in the body frame of the last state and then transforming them into the world frame. To evaluate the applicability of the proposed method in real-time situations, a dataset collected by a SkywalkerX8 Unmanned Aerial Vehicle is utilized. This dataset includes all the necessary maneuvers, including climb, descent, turn and cruise during the flight time. The results confirm that our proposed integrated system has superior statistical measures and temporal performance compared to the extended Kalman filter (EKF); it outperforms the conventional Factor Graph Optimization and EKF by approximately 45%.
This is a preview of subscription content, log in via an institution to check access.
Similar content being viewed by others
References
Carlone L, Kira Z, Beall C, Indelman V, Dellaert F (2014) Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors. In: 2014 IEEE International Conference on Robotics and Automation (ICRA), Hong Kong, China, June 1–7, pp. 4290–4297
Chesneau CI, Hillion M, Hullo JF, Thibault G, Prieur C (2017) Improving magneto-inertial attitude and position estimation by means of a magnetic heading observer. In: 2017 International Conference on Indoor Positioning and Indoor Navigation (IPIN), Sapporo, Japan, September 18–21, pp. 1–8
Dai B, He Y, Yang L, Su Y, Yue Y, Xu W (2020) SIMSF: a scale insensitive multi-sensor fusion framework for unmanned aerial vehicles based on graph optimization. IEEE Access 8:118273–118284
Dellaert F, Kaess M (2017) Factor graphs for robot perception. Found Trends Robot 6(1–2):1–139
Eltahier MMA, Hamid K (2017) Review of instrument landing system. IOSR J Electr Commun Eng 12(1):106–113
Forster C, Carlone L, Dellaert F, Scaramuzza D (2016) On-manifold pre-integration for real-time visual-inertial odometry. IEEE Trans Rob 33(1):1–21
Groves PD (2015) Principles of GNSS, inertial, and multi sensor integration navigation systems. IEEE Aerosp Electron Syst Mag 30(2):26–27
Gu X, Zhang F, Xu J, Yuan Q, Ma H, Liu X, Li Z (2019) Graph optimization based long-distance GPS/IMU integrated navigation. In: IEEE 2019 Chinese Control Conference (CCC), pp. 3976–3981
Indelman V, Williams S, Kaess M, Dellaert F (2013) Information fusion in navigation systems via factor graph based incremental smoothing. Robot Auton Syst 61(8):721–738
Jizhou LAI, Shiyu BAI, Xiaowei XU, Pin L (2019) A generic plug-and-play navigation fusion strategy for land vehicles in GNSS-denied environment. Trans Nanjing Univ Aeronaut Astronaut 36(2):197–204
Jones ES, Soatto S (2011) Visual-inertial navigation, mapping and localization: a scalable real-time causal approach. Int J Robot Res 30(4):407–430
Kaess M, Johannsson H, Roberts R, Ila V, Leonard JJ, Dellaert F (2012) ISAM2: incremental smoothing and mapping using the Bayes tree. Int J Robot Res 31(2):216–235
Kschischang FR, Frey BJ, Loeliger HA (2001) Factor graphs and the sum-product algorithm. IEEE Trans Inf Theory 47(2):498–519
Li W, Cui X, Lu M (2018) A robust graph optimization realization of tightly coupled GNSS/INS integrated navigation system for urban vehicles. Tsinghua Sci Technol 23(6):724–732
Lupton T, Sukkarieh S (2012) Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions. IEEE Trans Rob 28(1):61–76
Lynen S, Achtelik MW, Weiss S, Chli M, Siegwart R (2013) A robust and modular multi-sensor fusion approach applied to MAV navigation. In: 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, Tokyo, Japan, November 1–7, pp. 3923–3929
Mascaro R, Teixeira L, Hinzmann T, Siegwart R, Chli M (2018) GOMSF: Graph-Optimization based Multi-Sensor Fusion for robust UAV pose estimation. In: 2018 IEEE International Conference on Robotics and Automation (ICRA), Brisbane, QLD, Australia, May 21–25, pp. 1421–1428.
Mourikis AI, Roumeliotis SI (2007) A multi-state constraint Kalman filter for vision-aided inertial navigation. In: Proceedings 2007 IEEE International Conference on Robotics and Automation, Roma, Italy, April 10–14, pp. 3565–3572
Mu X, Chen J, Zhou Z, Leng Z, Fan L (2018) Accurate initial state estimation in a monocular visual–inertial SLAM system. Sensors 18(2):506
Mur-Artal R, Tardós JD (2017) Visual-inertial monocular SLAM with map reuse. IEEE Robot Autom Lett 2(2):796–803
Nezhadshahbodaghi M, Mosavi MR (2021) A loosely-coupled EMD-denoised stereo VO/INS/GPS integration system in GNSS-denied environments. Measurement 183:109895
Nezhadshahbodaghi M, Mosavi MR, Hajialinajar MT (2021) Fusing denoised stereo visual odometry, INS and GPS measurements for autonomous navigation in a tightly coupled approach. GPS Solut 25(2):1–18
Nourmohammadi H, Keighobadi J (2017) Decentralized INS/GNSS system with MEMS-grade inertial sensors using QR-factorized CKF. IEEE Sens J 17(11):3278–3287
Qin T, Cao S, Pan J, Shen S (2019) A general optimization-based framework for global pose estimation with multiple sensors. arXiv preprint, Cornel University, January 11, https://arxiv.org/abs/1901.03642.
Rogers RM (2007) Applied mathematics in integrated navigation systems. American Institute of Aeronautics and Astronautics, USA
Shen S, Mulgaonkar Y, Michael N, Kumar V (2014) Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft MAV. In: 2014 IEEE International Conference on Robotics and Automation (ICRA), Hong Kong, China, June 1–7, pp. 4974–4981
Sibley G, Matthies L, Sukhatme G (2010) Sliding window filter with application to planetary landing. J Field Robot 27(5):587–608
Titterton D, Weston JL (2004) Strap down inertial navigation technology, 2nd edn. Institution of Engineering Technology, Stevenage
Wei X, Li J, Zhang D, Feng K (2021) An improved integrated navigation method with enhanced robustness based on factor graph. Mech Syst Signal Process 155:107565
Wen W, Pfeifer T, Bai X, Hsu LT (2020) It is time for factor graph optimization for GNSS/INS integration: comparison between FGO and EKF. arXiv preprint, https://arxiv.org/abs/2004.10572.
Xu J, Yang G, Sun Y, Picek S (2021) A multi-sensor information fusion method based on factor graph for integrated navigation system. IEEE Access 9:12044–12054
Author information
Authors and Affiliations
Corresponding author
Additional information
Publisher's Note
Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.
Rights and permissions
About this article
Cite this article
Taghizadeh, S., Nezhadshahbodaghi, M., Safabakhsh, R. et al. A low-cost integrated navigation system based on factor graph nonlinear optimization for autonomous flight. GPS Solut 26, 78 (2022). https://doi.org/10.1007/s10291-022-01265-9
Received:
Accepted:
Published:
DOI: https://doi.org/10.1007/s10291-022-01265-9