Moving Horizon Estimation for GNSS-IMU sensor fusion

The aim of this work is to develop a Global Navigation Satellite System (GNSS) and Inertial Measurement Unit (IMU) sensor fusion system. To achieve this objective, we introduce a Moving Horizon Estimation (MHE) algorithm to estimate the position, velocity orientation and also the accelerometer and gyroscope bias of a simulated unmanned ground vehicle. The obtained results are compared with the true values of the system and with an Extended Kalman filter (EKF). The use of CasADi and Ipopt provide efficient numerical solvers that can obtain fast solutions. The quality of MHE estimated values enable us to consider MHE as a viable replacement for the popular Kalman Filter, even on real time systems.


I. Introduction
Navigation aims to solve the problem of determining the position, velocity and orientation of an object in space using different sources of information. If we want to control efficiently an unmanned vehicle, its position, velocity and orientation should be known as accurately as possible. The integration of Global Navigation Satellite Systems (GNSS) and Inertial Measurement Units (IMU) is the state of the art among navigation systems (Polóni et al., 2015), (Vandersteen et al.,2013). It involves non-linear measurement equations combined with rotation matrices, expressed through Euler angles or quaternions, along with the cinematic models for the rigid body's translation and rotation in space. Traditionally, the Extended Kalman Filter (EKF) (Lefferts et al., 1982;MarkleyandSedlak, 2008;Roumeliotis and Bekey, 1999), Unscented Kalman Filter (UKF) (Crassidis and Markley, 2003;Rhudyet al., 2013) or the Particle Filter (PF) (Carmi and Oshman,2009;Cheng and Crassidis, 2004) are used to solve the navigation problem.
Recently, the use of non-linear observers have been proposed as an alternative to the different types of Kalman filters and statistical methods. However, there is still little literature on the subject. Grip et al. (2012) present an observer for estimating position, velocity, attitude, and gyro bias, by using inertial measurements of accelerations and angular velocities, magnetometer measurements, and satellite-based measurements of position and (optionally) velocity. Vandersteen et al. (2013) use a Moving Horizon Estimation (MHE) algorithm in real-time to estimate the orientation and the sensor calibration parameters applied to two space mission scenarios. In the first scenario, the attitude is estimated from three-axis magnetometer and gyroscope measurements. In the second scenario, a star tracker is used to jointly estimate the attitude and gyroscope calibration parameters. In order to solve this constrained optimization problem in real time, an efficient numerical solution method based on the iterative Gauss-Newton scheme has been implemented and specific measures are taken to speed up the calculations by exploiting the sparsity and band structure of matrices to be inverted. In Poloni et al. (2015) a nonlinear numerical observer for accurate position, velocity and attitude estimation including the accelerometer bias and gyro bias estimation is presented. A Moving Horizon Observer (MHO) processes the accelerometer, gyroscope and magnetometer measurements from the IMU and the position and velocity measurements from the GNSS. The MHO is tested off-line in the numerical experiment involving the experimental flight data from a light fixed-wing aircraft.
Both EKF and MHE are based on the solution of a least-squares problem. While EKF use recursive updates to obtain the estimates and the error covariance matrix, MHE use a finite horizon window and solve a constrained optimization problem to find the estimates. In this way, the physical limits of the system states and parameters can be modeled through the optimization problem's constraints. The omission of this information can degrade the estimation algorithm performance (Haseltine and Rawlings, 2005). Unfortunately, the Kalman based filters do not explicitly incorporate restrictions in the estimates (states and/or parameters) and, because of this, several ad-hoc methods have been developed (Simon, 2010;Simon and Chia, 2002;Hall and McMullen, 2004;Teixeiraet Guido Sánchez, et al. (112-122) Universidad Tecnológica Nacional ABRIL 2020 / Año 18-Nº 37 Revista Tecnología y Ciencia DOI: https://doi.org/10. 33414/rtyc.37.112-122.202033414/rtyc.37.112-122. -ISSN 166633414/rtyc.37.112-122. -6933 . al., 2008Simon and Simon, 2010;Ko and Bitmead,2007). These methods lead to sub-optimal solutions at best and can obtain non-realistic solutions under certain conditions, specially when the statistics of the unknown variables are chosen poorly. On the other hand, MHE solves an optimization problem to find the system estimates on each sample step, providing a theoretical framework for theoretic frame for constrained state and parameter estimation.
In this work it will be assumed that the reader is familiar with some of the many coordinate frames used for navigation. If needed, the work of Bekir (2007) provides an excellent introduction to these topics. In particular, these coordinate frames will be used: 1. Body reference frame, referred as Body and by the superindex . 2. Earth-Centered Earth-Fixed, referred as ECEF and by the superindex . 3. East-North-Up, referred as ENU and by the superindex . This work is organized as follows: in Section II the problem formulation is presented. Section III describes the aspects of the Moving Horizon Estimation algorithm and the Extended Kalman Filter implementation. In order tocompare the proposed method, a test simulation example is given in Section IV. Finally, in Section V conclusions of this work are stated.

II. Problem Formulation
The system equations -for a detailed description, see Poloni et al. (2015) and Bekir(2007)-that describe the rigid body dynamics in ECEF coordinates are given by: where is the position in ECEF coordinates, is the linear velocity in ECEF coordinates, is the linear acceleration in ECEF coordinates and is the gravity vector in ECEF coordinates. The gravity vector is a function of the position and is modeled using the 2 gravity model (Hsu, 1996). The known Earth's angular velocity around the ECEF z-axis is represented by vector and ~= [0; ] is the quaternion representation of the angular velocities in body frame. The quaternion determines the orientation of the rigid body in space and and are the gyroscope and accelerometer bias, respectively.
The measurement equations with measurement noise are given by: where is a known vector that contains the values of the magnitude of the terrestrial magnetic field given our current latitude and longitude 1 , and are the angular velocity and linear acceleration vectors in body and Guido Sánchez, Universidad Tecnológica Nacional ABRIL 2020 / Año 18-Nº 37 Revista Tecnología y Ciencia DOI: https://doi.org/10. 33414/rtyc.37.112-122.202033414/rtyc.37.112-122. -ISSN 1666 ECEF coordinates, respectively. The matrix ( ) is the rotation matrix associated with the current orientation quaternion.
In order to use GNSS data with Eqs.
(1)-(5), we need to convert it to ECEF coordinates. This can be done usingthefollowingequations: where = 2 √ 2 2 + 2 2 is the Earth's east-west radius of curvature, is the latitude in radians, is the longitude in radians, ℎ is the altitude in meters, = 6378137 and = 6356752.3142 are the major and minor axes of the Earth reference ellipsoid, respectively.
The set of Eqs.
(1)-(5) model the position, velocity and orientation of a vehicle in ECEF coordinates. However, if we wish to travel short distances it is convenient to use ENU coordinates and work in a local reference frame. The steps to convert from ECEF to ENU are the following: 1. Determine the latitude, longitude and altitude of the initial reference position ( 0 , 0 , ℎ 0 ) and calculate its ECEF coordinates using equation (11) to obtain vector 0 = [ 0 , 0 , 0 ] . This position will be the origin of the ENU coordinate system. 2. Transform the incoming GNSS measurements to ECEF coordinates using equation (11) to obtain and compute the relative displacements in ENU coordinates using the following: where ( 0 , 0 ) is the ENU to ECEF rotation matrix and depends on the initial latitude and longitude ( 0 , 0 ).
The ENU to ECEF rotation matrix is given by two rotations (J. Sanz-Subirana& Hernández-Pajares, 2011): 1. A clockwise rotation over east-axis by an angle 90 − to align the up-axis with the z-axis.
Where rotation matrices are defined as follows: in matrixform, weobtain where we assume that the x-axis points to the East when using ENU coordinates. Taking into account the properties of rotation matrices, the ECEF to ENU transformation is obtained through the transpose of the matrix given by the previous equation. In this way, equation (16) gives a formula to convert coordinates from ENU to ECEF and from ECEF to ENU. By using ENU, we establish a local coordinate system relative to the reference position 0 . We must replace our orientation quaternion from to . Besides, we must be very careful and know exactly in which frame of reference each of the parameters, constants and sensor measurements are given in order to apply the corresponding rotations to them.

A. MHE implementation
The MHE implementation follows the algorithm presented in the work of Rao et al. (2001Rao et al. ( , 2003. In our case, the vector of differential and algebraic states are defined as

B. EKF implementation
The implementation of the Extended Kalman Filter follows the standard procedure; however, there are a couple of subtleties. Firstly, gyroscope and accelerometer readings are treated as control inputs instead of as measurements. To that end, and , which were previously regarded as algebraic states, are expressed as functions of the inputs and and subsequently eliminated from the problem formulation. The differential states remain the same as in the MHE formulation, while the measurement vector is comprised of the remaining data readings, namely,

= [ ]
And secondly, the quaternion ^ must be renormalized at each time step, given that there is no way to take this constraint into account in the EKF, as was done in the MHE implementation.

IV. Example
In the following example, we will perform a manoeuvre using Gazebo and ROS to run a simulation of the Husky unmanned ground vehicle moving to the following set of waypoints: 1 = [5; 0; 0] , 2 = [15; 10; 0] , 3 = [20; 10; 0] , 4 = [30; 0; 0] and 5 = [35; 0; 0] . As stated before, the vehicle is equipped with GNSS and IMU sensors, which will be used to estimate the position and orientation in ENU coordinates using MHE and EKF. Both of these estimated values will be compared to the true values. Figures 1, 3 and 5 show the true position and its estimates ^ in ENU coordinates. It can be seen that both the MHE and EKF provide good estimates. Figures 1 and 3 show that both estimators are able to follow the changes on the x and y axis. Since the vehicle is moving on flat terrain, the z coordinate is only affected by noise (see Fig.   5). Figures 2, 4 and 6 show the difference ^− . It can be seen that MHE error is slightly smaller on the x and y axis, while EKF filters slightly better the noise on the z axis. The orientation quaternion and its estimates ^ are shown in Figures 7, 9, 11 and 13, where it can be seen a similar behaviour than the one obtained from the position. MHE performs a better estimation of the states that change through time --0 and 3 --, as it can be seen in Figures 7, 8, 13 and 14, while EKF is able to do a slightly better job at filtering the noise on 1 and 2 , as it can be seen in Figures 9, 10, 11 and 12. The results obtained by MHE can be attributed to the fact that: i) MHE uses more measurements to obtain the current estimate; ii) MHE does not assume Gaussian distribution for the process and measurement noises within the estimation horizon, such as the EKF.
Finally, Table 1 shows the mean and the standard deviation of the squared error between the real state and the estimated state over 50 realizations of the same experiment. It can be seen that in average, the position estimates show a smaller mean squared error with EKF, while the velocity estimates show a smaller error with MHE. Finally, the orientation quaternion shows the same behaviour as commented earlier, where MHE performs a better estimation of the states that change through time --0 and 3 --, while EKF does a better job at filtering the noise on 1 and 2 . The average execution time for each sample was 5.293 milliseconds for each MHE iteration and 0.211 milliseconds for each EKF iteration on an Intel i5 desktop computer.

V. Conclusion
In this work we employed MHE to estimate the position, velocity and orientation of an unmanned ground vehicle by fusing data from GNSS and IMU sensors. These estimates are compared with the classic benchmark Guido Sánchez, Universidad Tecnológica Nacional ABRIL 2020 / Año 18-Nº 37 Revista Tecnología y Ciencia DOI: https://doi.org/10. 33414/rtyc.37.112-122.202033414/rtyc.37.112-122. -ISSN 1666 algorithm, the EKF and the true values. MHE is able to perform as good as EKF, even at a fast rate, which indicates that it can easily be used for real time estimation. Since MHE solves a non linear optimization problem on each iteration, the addition of constraints and bounds such as the ones described by Eq. (21), is straight forward.
Since the solution of the navigation problem requires a very specific set of knowledge and the use of different coordinate systems often leads to confusion, we also showed all the necessary steps to perform position, velocity and orientation estimation either in ECEF or ENU coordinates. One of the issues that remains open is how to tune both MHE and EKF weight matrices in order to provide better results.
If we want to run these algorithms with real sensors, special care must be taken in order to account for different sampling rates, especially when typical GNSS receivers sampling rate is around 1 Hz to 10 Hz and commercial IMUs sampling rate is around 500 Hz.

VI. Acknowledgment
The authors wish to thank the Universidad Nacional de Litoral (with CAI+D Jóven 500 201501 00050 LI and CAI+D 504 201501 00098 LI), the Agencia Nacional de PromociónCientífica y Tecnológica (with PICT 2016-0651) and the Consejo Nacional de InvestigacionesCientíficas y Técnicas (CONICET) from Argentina, for their support. We also would like to thank the groups behind the development of CasADi, Ipopt and the HSL solvers (The HSLMathematical Software Library, 2016).