articulo

Actas de las IX Jomadas Argentinas de Robótica 15-17 de noviembre, Córdoba, Argentina

Moving Horizon Estimation for GNSS-IMU sensor fusion

Estimación de Horizonte Móvil para fusión de GNSS-IMU

Instituto de Investigación en Señales, Sistemas e Inteligencia Computacional, UNL-CONICET - Santa Fe, Argentina

Abstract

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.

Keywords: State Estimation, Sensor Fusion, Moving Horizon Estimation, GNSS, IMU.

Resumen

El propósito de este trabajo es el desarrollo de un sistema de fusión de datos provenientes de un Sistema Global de Navegación por Satélite (GNSS, del inglés Global NavigationSatelliteSystem) y una Unidad de Medición Inercial (IMU, del inglés InertialMeasurementUnit). Para alcanzar este objetivo, presentamos un algoritmo de Estimación de Horizonte Móvil (MHE, del inglés MovingHorizonEstimation) para estimar mediante simulación la posición, velocidad, orientación y los sesgos (o bias) del acelerómetro y giróscopo de un vehículo terrestre no tripulado. Los resultados obtenidos serán comparados con los valores reales del sistema y con los obtenidos mediante el uso de un Filtro de Kalman Extendido (EKF, del inglés Extended Kalman Filter). La utilización de CasADi e Ipopt proveen solvers numéricos que permiten obtener soluciones sumamente rápido. La calidad de los estimados obtenidos por MHE nos permiten considerar a éste algoritmo como un reemplazo válido para el popular Filtro de Kalman, aún en sistemas que requieren operación en tiempo real.

Palabras claves: Estimación de Estados, Fusión de Sensores, Estimación de Horizonte Móvil, GNSS, IMU.

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 al., 2008; Simon 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:

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

gravity model (Hsu, 1996). The known Earth’s angular velocity around the ECEF z-axis is represented by vector

and

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.

where

is a known vector that contains the values of the magnitude of the terrestrial magnetic field given our current latitude and longitude,

and

are the angular velocity and linear acceleration vectors in body and 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:

is the Earth's east-west radius of curvature,

is the latitude in radians,

is the longitude in radians,

is the altitude in meters,

and

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:

where

is the ENU to ECEF rotation matrix and depends on the initial latitude and longitude

The ENU to ECEF rotation matrix is given by two rotations (J. Sanz-Subirana& Hernández-Pajares, 2011):

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

. We must replace our orientation quaternion from

. 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.

III. Implementation details

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

Given that the quaternion

must have unit norm, the constraint

could be included. However, to avoid the computational cost of this restriction, the first term of

, which penalizes its violation at

, and the following set of constraints

The horizon length

and the values of the weights

and

were chosen by a trial and error procedure as

and

. The resulting MHE constrained non-linear optimization problem is solved with CasADi (Andersson, 2013) and Ipopt (WächterandBiegler, 2006).

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.

The covariance matrices

and

of the EKF are chosen as the inverse of the weighting matrices employed in the MHE formulation, given that a smaller covariance in the former must correspond to a bigger weight, i.e., “trust”, in the latter. Therefore, the covariance matrices are defined as

and

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:

and

. 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 --

and

--, 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

and

, 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 --

and

--, while EKF does a better job at filtering the noise on

and

. 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 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.

	Mean (x 10e-3)		Std. Dev (x 10e-3)
	MHE	EKF	MHE	EKF
p_x [m]	0.30643	0.28912	0.36298	0.33779
p_y [m]	0.18676	0.17014	0.22421	0.20285
p_z [m]	0.02815	0.00870	0.03828	0.01201
v_x [m/s]	0.93519	1.70043	1.28402	2.32408
v_y [m/s]	0.92798	1.67139	1.26584	2.26883
v_z [m/s]	1.09800	2.12559	1.50401	2.90566
q₀	0.19255	1.08827	0.28763	1.17203
q₁	0.22829	0.06821	0.31084	0.09292
q₂	0.22064	0.07187	0.30205	0.09909
q₃	1.73780	5.35359	2.38613	5.35354

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).

Referencias

Andersson, J. (2013). A General-Purpose Software Framework for Dynamic Optimization. PhD thesis, Arenberg Doctoral School, KU Leuven, Department of Electrical Engineering (ESAT/SCD) and Optimization in Engineering Center, KasteelparkArenberg 10, 3001-Heverlee, Belgium.

Bekir, E. (2007). Introduction to modern navigation systems. World Scientific.

Carmi, A. &Oshman, Y. (2009). Adaptive Particle Filtering for Spacecraft Attitude Estimation from Vector Observations. Journal of Guidance, Control, and Dynamics, 32(1):232–241.

Cheng, Y. &Crassidis, J. (2004). Particle Filtering for Sequential Spacecraft Attitude Estimation. In AIAA Guidance, Navigation, and Control Conference and Exhibit, Reston, Virigina. American Institute of Aeronautics and Astronautics.

Crassidis, J. & Markley, F. (2003). Unscented Filtering for Spacecraft Attitude Estimation. Journal of Guidance, Control, and Dynamics, 26(4):536–542.

Grip, H. F., Fossero, T. I., Johansent, T. A., &Saberi, A. (2012). A nonlinear observer for integration of gnss and imu measurements with gyro bias estimation. In American Control Conference (ACC), 2012, pages 4607–4612. IEEE.

Hall, D. & McMullen, S. (2004). Mathematical techniques in multisensor data fusion. Artech House.

Haseltine, E. & Rawlings, J. (2005). Critical evaluation of extended kalman filtering and moving-horizon estimation. Industrial & engineering chemistry research, 44(8):2451–2460.

Hsu, D. (1996). Comparison of four gravity models. In Proceedings of Position, Location and Navigation Symposium - PLANS ’96, pages 631–635. IEEE.

J. Sanz-Subirana, J. Z. &Hernández-Pajares, M. (2011).Transformations between ECEF and ENU coordinates. http://www.navipedia.net/index.php/Transformations between ECEF and ENU coordinates. (Online; accessed 30-07-2017).

Ko, S. &Bitmead, R. (2007). State estimation for linear systems with state equality constraints. Automatica, 43(8):1363–1368.

Lefferts, E., Markley, F., and Shuster, M. (1982). Kalman filtering for spacecraft attitude estimation.

Markley, F. &Sedlak, J. (2008). Kalman Filter for Spinning Spacecraft Attitude Estimation. Journal of Guidance, Control, and Dynamics, 31(6):1750–1760.

Polóni, T., Rohal-Ilkiv, B., & Johansen, T. (2015). Moving Horizon Estimation for Integrated Navigation Filtering. IFACPapersOnLine, 48(23):519–526.

Rao, C. V., Rawlings, J. B., & Lee, J. H. (2001). Constrained linear state estimation—a moving horizon approach. Automatica, 37(10):1619–1628.

Rao, C. V., Rawlings, J. B., & Mayne, D. Q. (2003). Constrained state estimation for nonlinear discrete-time systems: Stability and moving horizon approximations. IEEE transactions on automatic control, 48(2):246–258.

Rhudy, M., Gu, Y., Gross, J., Gururajan, S., & Napolitano, M. R. (2013). Sensitivity Analysis of Extended and Unscented Kalman Filters for Attitude Estimation. Journal of Aerospace Information Systems, 10(3):131–143.

Roumeliotis, S. &Bekey, G. (1999). 3D Localization for a Mars Rover Prototype. In Artificial Intelligence, Robotics and Automation in Space, Proceedings of the Fifth International Symposium, ISAIRAS ’99, pages 441–448.

Simon, D. (2010). Kalman filtering with state constraints: a survey of linear and nonlinear algorithms. Control Theory & Applications, IET, 4(8):1303–1318.

Simon, D. & Chia, T. (2002). Kalman filtering with state equality constraints. Aerospace and Electronic Systems, IEEE Transactions on, 38(1):128–136.

Simon, D. & Simon, D. (2010). Constrained kalman filtering via density function truncation for turbofan engine health estimation. International Journal of Systems Science, 41(2):159–171.

Teixeira, B., Chandrasekar, J., PalanthandalamMadapusi, H., Tôrres, L., Aguirre, L., & Bernstein, D. (2008). Gainconstrainedkalman filtering for linear and nonlinear systems. Signal Processing, IEEE Transactions on, 56(9):4113–4123.

The HSL Mathematical Software Library (2016). HSL. A collection of Fortran codes for large scale scientific computation. http://www.hsl.rl.ac.uk. (Online; accessed 30-07-2017).

Vandersteen, J., Diehl, M., Aerts, C., &Swevers, J. (2013). Spacecraft Attitude Estimation and Sensor Calibration Using Moving Horizon Estimation. Journal of Guidance, Control, and Dynamics, 36(3):734–742.

Wächter, A. &Biegler, L. T. (2006). On the implementation of an interior-point filter line-search algorithm for largescale nonlinear programming. Mathematicalprogramming, 106(1):25–57.

This data is tabulated and can be obtained from https://www.ngdc.noaa.gov/geomag-web

Figure 1: position in the x-axis [m]	Figure 2: position error in the x-axis [m]
Figure 3: position in the y-axis [m]	Figure 4: position error in the y-axis [m]
Figure 5: position in the z-axis [m]	Figure 6: position error in the z-axis [m]

Figure 7:	Figure 8:
Figure 9:	Figure 10:
Figure 11:	Figure 12:
Figure 13:	Figure 14:

	(1)
	(2)
	(3)
	(4)
	(5)

	(6)
	(7)
	(8)
	(9)
	(10)

	(13)
	(14)
	(15)

	(17)
	(18)