Optimization-Based Sensor Fusion of GNSS and IMU Using a Moving Horizon Approach

The rise of autonomous systems operating close to humans imposes new challenges in terms of robustness and precision on the estimation and control algorithms. Approaches based on nonlinear optimization, such as moving horizon estimation, have been shown to improve the accuracy of the estimated solution compared to traditional filter techniques. This paper introduces an optimization-based framework for multi-sensor fusion following a moving horizon scheme. The framework is applied to the often occurring estimation problem of motion tracking by fusing measurements of a global navigation satellite system receiver and an inertial measurement unit. The resulting algorithm is used to estimate position, velocity, and orientation of a maneuvering airplane and is evaluated against an accurate reference trajectory. A detailed study of the influence of the horizon length on the quality of the solution is presented and evaluated against filter-like and batch solutions of the problem. The versatile configuration possibilities of the framework are finally used to analyze the estimated solutions at different evaluation times exposing a nearly linear behavior of the sensor fusion problem.

[1]  Mario Zanon,et al.  Nonlinear MPC and MHE for Mechanical Multi-Body Systems with Application to Fast Tethered Airplanes , 2012 .

[2]  MORITZ DIEHL,et al.  A Real-Time Iteration Scheme for Nonlinear Optimization in Optimal Feedback Control , 2005, SIAM J. Control. Optim..

[3]  Giovanni Ulivi,et al.  An outdoor navigation system using GPS and inertial platform , 2001, 2001 IEEE/ASME International Conference on Advanced Intelligent Mechatronics. Proceedings (Cat. No.01TH8556).

[4]  John L. Crassidis,et al.  Survey of nonlinear attitude estimation methods , 2007 .

[5]  Jay H. Lee,et al.  Constrained linear state estimation - a moving horizon approach , 2001, Autom..

[6]  Lars Imsland,et al.  Efficient implementation of solvers for linear model predictive control on embedded devices , 2014, 2014 IEEE Conference on Control Applications (CCA).

[7]  Moritz Diehl,et al.  Attitude estimation based on inertial and position measurements , 2012, 2012 IEEE 51st IEEE Conference on Decision and Control (CDC).

[8]  J.L. Crassidis,et al.  Sigma-point Kalman filtering for integrated GPS and inertial navigation , 2005, IEEE Transactions on Aerospace and Electronic Systems.

[9]  John C. Butcher,et al.  Integration processes based on Radau quadrature formulas , 1964 .

[10]  Ling Chen,et al.  An optimization based Moving Horizon Estimation with application to localization of Autonomous Underwater Vehicles , 2014, Robotics Auton. Syst..

[11]  Michael J. Rycroft,et al.  Understanding GPS. Principles and Applications , 1997 .

[12]  Tor Arne Johansen,et al.  Moving Horizon Estimation for Integrated Navigation Filtering , 2015 .

[13]  G. Bellusci,et al.  The Next Generation Xsens Motion Trackers for Industrial Applications , 2017 .

[14]  Dimitry Gorinevsky,et al.  Fault Tolerant Relative Navigation using Inertial and Relative Sensors , 2007 .

[15]  James B. Rawlings,et al.  Critical Evaluation of Extended Kalman Filtering and Moving-Horizon Estimation , 2005 .

[16]  Pascal Vasseur,et al.  Introduction to Multisensor Data Fusion , 2005, The Industrial Information Technology Handbook.

[17]  LI X.RONG,et al.  Survey of maneuvering target tracking. Part I. Dynamic models , 2003 .

[18]  Stefano Soatto,et al.  Visual-inertial navigation, mapping and localization: A scalable real-time causal approach , 2011, Int. J. Robotics Res..

[19]  John B. Moore,et al.  Direct Kalman filtering approach for GPS/INS integration , 2002 .

[20]  A. H. Mohamed,et al.  Adaptive Kalman Filtering for INS/GPS , 1999 .

[21]  Jan Swevers,et al.  Spacecraft Attitude Estimation and Sensor Calibration Using Moving Horizon Estimation , 2013 .

[22]  Zhou Chao,et al.  Collision-free UAV formation flight control based on nonlinear MPC , 2011, 2011 International Conference on Electronics, Communications and Control (ICECC).

[23]  Hans Bock,et al.  A Direct Multiple Shooting Method for Real-Time Optimization of Nonlinear DAE Processes , 2000 .

[24]  Mario Zanon,et al.  Time-optimal race car driving using an online exact hessian based nonlinear MPC algorithm , 2016, 2016 European Control Conference (ECC).

[25]  O. V. Stryk,et al.  Numerical Solution of Optimal Control Problems by Direct Collocation , 1993 .

[26]  P. Savage.  Strapdown Inertial Navigation Integration Algorithm Design Part 1: Attitude Algorithms , 1998 .

[27]  Dah-Jing Jwo,et al.  An Adaptive Sensor Fusion Method with Applications in Integrated Navigation , 2008, Journal of Navigation.

[28]  Moritz Diehl,et al.  Auto Generation of Implicit Integrators for Embedded NMPC with Microsecond Sampling Times , 2012 .

[29]  Xiaoji Niu,et al.  Analysis and Modeling of Inertial Sensors Using Allan Variance , 2008, IEEE Transactions on Instrumentation and Measurement.

[30]  Haris Ubaid Gul,et al.  An Optimal Moving Horizon Estimation for Aerial Vehicular Navigation Application , 2017 .

[31]  D. B. Cox,et al.  Integration of GPS with Inertial Navigation Systems , 1978 .

[32]  John Weston,et al.  Strapdown Inertial Navigation Technology , 1997 .

[33]  Shu-Li Sun,et al.  Multi-sensor optimal information fusion Kalman filter , 2004, Autom..

[34]  Robert E. Young,et al.  Moving-horizon state estimation applied to an industrial polymerization process , 1999, Proceedings of the 1999 American Control Conference (Cat. No. 99CH36251).

[35]  Moritz Diehl,et al.  Towards robust sensor fusion for state estimation in airborne applications using GNSS and IMU , 2017 .