A generic multi-sensor fusion scheme for localization of autonomous platforms using moving horizon estimation

Abstract

In this paper, a generic multi-sensor fusion framework is developed for the localization of intelligent vehicles and mobile robots. The localization framework is based on moving horizon estimation (MHE). Unlike the commonly used probabilistic filtering algorithms – for example, extended Kalman filter (EKF) and unscented Kalman filter (UKF) – MHE relies on solving successive least squares optimization problems over the innovation of multiple sensors’ measurements and a specific estimation horizon. In this paper, we present an efficient and generic multi-sensor fusion scheme, based on MHE. The proposed multi-sensor fusion scheme is capable of operating with different sensors’ rates, missing measurements, and outliers. Moreover, the proposed scheme is based on a multi-threading architecture to reduce its computational cost, making it more feasible for practical applications. The MHE fusion method is tested using simulated data as well as real experimental data sequences from an intelligent vehicle and a mobile robot combining measurements from different sensors to get accurate localization results. The performance of MHE is compared against that of UKF, where the MHE estimation results show superior performance.

Introduction

Localization is one of the main modules for autonomous systems; for example, vehicles and mobile robots. It acts as the basis for decision making and navigation control. The research effort on developing more reliable and accurate localization methods has been increasing in the past few years from both academia and industry. This problem can be addressed by the use of highly accurate sensors such as the differential global positioning system (D-GPS). However, such sensors are usually costly and their accuracy degrades in remote areas or in GPS denied environments. An alternative solution is to use different low cost sensors of lower accuracy, and to apply a sensor fusion technique (Khaleghi et al., 2013) in order to get a better pose estimate of the autonomous system (Al-Kaff et al., 2018; Duan et al., 2014; Kelly and Sukhatme, 2011; Lu et al., 2017; Luo and Chang, 2012; Magrin and Todt, 2016; Osman et al., 2019a; Urmson et al., 2008).

The localization problem can be addressed by two different approaches: recursive filtering (through linearizing the nonlinear systems models) and optimization-based approaches (without the need for linearization). Recursive filtering methods are based on the Bayesian estimation methods such as extended Kalman filter (EKF) and unscented Kalman filter (UKF) (Julier and Uhlmann, 1997, 2004), which rely on the sensor measurements combined with a motion model for computing the prior distribution of the pose to estimate the posterior probability of the pose (Thrun et al., 2005). Optimization-based techniques are based on directly solving the maximum a posteriori (MAP) optimization problem without the need of any linearization. The optimization problem aims at maximizing the posterior probability over the whole estimated trajectory; this is achieved through solving a least squares optimization problem over the innovations of the measurements of different sensors (Cadena et al., 2016).

Solving the optimization problem over the whole history of states is commonly referred to as a full information estimation (Rao et al., 2003). The full information estimation becomes intractable for longer duration of operation, due to the increase in optimization variables (unknown poses in case of localization). In practice, the contribution of old states over the future ones decreases with time. This means that optimizing over the whole history of states may become unnecessary and computationally expensive. Moving horizon estimation (MHE) has the ability to deal directly with the nonlinear system models and measurement models. The computational cost of MHE can be controlled by choosing the size of the estimation horizon.

In order to solve the intractability problem of full information estimation, MHE solves a similar estimation problem with the difference being that it includes a fixed number of the most recent measurements, while older measurements are marginalized in order to keep the computational time nearly constant (Ali and Tahir, 2020; Flaus and Boillereaux, 1997; Zou et al., 2020).

In this paper, a generic multi-sensor fusion framework is developed for the localization of mobile platforms using MHE. The main contributions of this paper can be summarized as follows:

• The development of a generic multi-sensor fusion scheme based on MHE for the localization of autonomous systems, which is capable of handling different sensor rates as well as missing measurements and outliers.

• Incorporation of a multi-threading framework into the MHE algorithm to reduce its computational cost to become more feasible for real-time applications.

In conclusion, the paper proposes a proof-of-concept design of a generic and practical multi-sensor fusion scheme based on MHE for the localization of autonomous platforms given the historical reputation of the MHE as computationally expensive and only suitable for certain applications and configurations.

The MHE fusion scheme is tested using both numerically simulated data as well as experimental data sequences. The MHE estimation output is compared with that of a UKF. Finally, the implementation of the proposed estimation scheme is made available as an open-source package for the scientific community.

The remainder of this paper is organized as follows. An overview of the related work is presented in Section 2. The MHE problem is formulated in Section 3. The generic fusion scheme developed for fusing different odometries using MHE is presented in Section 4. Section 5 presents the numerical simulation and experimental sequences used for validating the proposed scheme. In Section 6, the MHE localization results are compared with those of the UKF multi-sensor fusion algorithm. Finally, concluding remarks and future works are presented in Section 7.

Related work

In Chen et al. (2012), the authors introduced an EKF-based localization algorithm for autonomous mobile robots, which uses a map of landmarks consisting of corner features. In this work, based on the assumption of Gaussian white noises and a Markov stochastic process, the location of the robot is estimated using the odometry information and exteroceptive observations. The exteroceptive observations are then compared with the stored map of the landmarks in order to estimate the robot location. In Marín et al. (2013), an event-based Kalman filter was presented for multi-sensor fusion of robots with limited computation resources for indoor-outdoor localization of mobile robots. In Lynen et al. (2013), a robust generic sensor fusion algorithm for the localization of mobile robots based on the EKF was proposed, which is able to process delayed, relative and absolute measurements from theoretically unlimited number of different sensors and sensor types.

In Al Hage et al. (2017), a method for rejecting outliers in the measurements of sensors was presented using Kullback-Leibler divergence (KLD). The authors integrated the proposed method to an information filter and applied it to the localization of multi-robot systems for validation.

In Moore and Stouch (2016), a generic multi-sensor fusion scheme was developed for localization using filtering methods (EKF and UKF), which can take an arbitrary number of heterogeneous sensor measurements.

In Yousuf and Kadri (2018), an information fusion scheme for GPS, Inertial Navigation System, and odometry sensors is proposed to improve the accuracy of the localization of autonomous vehicles. Additionally, the authors uses an artificial neural network as a pseudo-sensor to predict the measurement of the GPS in case the GPS signal is lost. Furthermore, in Yousuf and Kadri (2020), the authors also add a fuzzy logic system in order to reduce the wheel slippage errors that can result in an increased error in position estimation from the odometer.

In Kim and Lee (2018), an EKF fusion scheme is implemented that fuses the measurements of a GPS and cameras along with the odometers. The scheme uses the odometers in order to perform the prediction step of the filter while the GPS and the cameras are used for the measurement step.

UKF-based localization is discussed in Giannitrapani et al. (2011) and was compared with EKF-based localization for a spacecraft. It was shown that the UKF performs better in terms of the average localization accuracy and consistency of the estimates. This is because the UKF approximation, which uses the unscented transform of the prior and measurement distributions, is a better in approximating the original distribution than the EKF, which is based on linear approximation.

In Nada et al. (2018), a UKF is used for estimating the position of a wheelchair in indoors environments. The fusion scheme is based on fusing the data from two odometers, a magnetometer, and an accelerometer. The work compares between two different fusion architecture, namely, state vector fusion and measurement fusion. The paper concludes that the measurement fusion gives better accuracy. Here, we use the measurement fusion scheme to develop our fusion scheme.

In Jain and Roy (2020), a navigation system is proposed based on UKF for pose estimation of the vehicle. The UKF is used to fuse the measurements of a GPS, an IMU, and wheel encoders while using a Stanley controller for navigation control.

In Liu et al. (2020), a multi-innovation UKF is used to fuse the measurement from different sensors in order to estimate the pose and velocity of skid-steer mobile robots. Therein, a dual antenna GNSS, two encoders, and an IMU are used to localize the robot while estimating the slip error components.

MHE was used in several studies for localization: Mehrez et al. (2014) used nonlinear MHE for the relative localization of a multi-robot system. An efficient algorithm based on real-time iteration scheme was used to improve the computational efficiency. In Simonetto et al. (2011), a distributed MHE scheme for a mobile robot localization problem was proposed using sensor networks. The stability of the estimator and the approximation of the arrival costs were discussed.

Wang et. al (2014). combined the EKF with the MHE for the localization of a three dimensional underwater vehicle. It was claimed that the algorithm achieves a compromise between better accuracy and lower computational cost. In Kimura et al. (2014), MHE was used to fuse a laser sensor range measurements and the odometry information of a vehicle to perform localization, which is more robust to the outliers in the laser measurements. In Qayyum et al. (2019), a receding horizon state observer for linear time-varying systems was proposed considering known deterministic inputs.

Liu et al. (2017) proposed a multi-rate MHE for the localization of mobile robots and provided a stability analysis of the estimator. The proposed MHE scheme uses an inertial sensor and a camera with data sampled at different rates. A binary switching sequence was introduced to model the multi-rate sampling process and the input-to-state stability was investigated assuming bounded disturbances and noises. Similarly, in Dubois et al. (2018), a multi-rate MHE sensor fusion algorithm in the presence of time-delayed and missing measurements was proposed. Additionally, a computationally efficient implementation of linear MHE was introduced. The proposed approach is only applicable when an analytical solution of the linear MHE problem can be found.

In most of the aforementioned studies, the proposed algorithms used different filtering techniques, or were based on linear measurement and system models to avoid the high computational load required for the implementation of the MHE. Furthermore, most of the solutions proposed are specific to each case and cannot be generalized to arbitrary number of sensor measurements.

In this paper, a generic multi-sensor fusion scheme based on nonlinear MHE is proposed. The proposed fusion scheme is generic in the sense that it can take care of multiple sample rates (Lin and Sun, 2019), missing measurements, outlier rejection, and real-time requirements.

Notations

$\mathbb{N}$ and $\mathbb{R}$ denote the sets of natural and real numbers, respectively; ${\mathbb{N}}_0$ denotes the positive natural numbers; $||x|{|}_A^2$ is the squared $l_2$ norm weighted by $A$ and is calculated as $x^{\mathrm{T}}\mathrm{Ax}$ ; ${\mathbf{I}}_{n\times n}$ is the $n\times n$ identity matrix; $0_{n\times k}$ denotes an $n\times k$ matrix with all entries of zeros; and $\mathrm{SO}(3)$ denotes the special orthogonal group, respectively.

MHE

In this section, the mathematical formulation of the MHE problem is presented following the way shown in Rawlings and Bakshi (2006). We consider the following (disturbed) discrete nonlinear dynamics

$${\mathbf{x}}_k=\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{u}}_k)+{\mathbf{w}}_k,$$

$${\mathbf{z}}_k=\mathbf{h}({\mathbf{x}}_k)+{\mathbf{v}}_k,$$

where $k\in {\mathbb{N}}_0$ is the current time-step, the subscript $(·{)}_k$ indicates the time index, $\mathbf{x}\in X\subseteq {\mathbb{R}}^n$ is the state vector, $\mathbf{u}\in U\subseteq {\mathbb{R}}^p$ is the vector of control inputs, $\mathbf{z}\in Z\subseteq {\mathbb{R}}^m$ is the measurement vector. $X$ , $U$ and $Z$ denote the feasible sets for state, control, and measurements, respectively. $\mathbf{w}\in {\mathbb{R}}^n$ and $\mathbf{v}\in {\mathbb{R}}^m$ are the zero-mean additive white random process and measurement noises. $\mathbf{f}:{\mathbb{R}}^n\times {\mathbb{R}}^p\to {\mathbb{R}}^n$ and $\mathbf{h}:{\mathbb{R}}^n\to {\mathbb{R}}^m$ are the nonlinear process and measurement models, respectively. The sequence of the states and that of the measurements for the whole state history are denoted as

$$\mathbf{X}:=({\mathbf{x}}_0,{\mathbf{x}}_1,\dots ,{\mathbf{x}}_k)\in X^{k+1},$$

$$\mathbf{Z}:=({\mathbf{z}}_1,\dots ,{\mathbf{z}}_k)\in Z^k.$$

A full information MAP estimation approach solves a least squares optimization problem over the history of states by minimizing the cost function

$$\begin{array}{c}\multicolumn{1}{c}{J(\mathbf{X},\mathbf{Z})=\frac{1}{2}||{\mathbf{e}}_{\mathbf{x}}({\mathbf{x}}_0,{\hat{\mathbf{x}}}_0)|{|}_{{\mathbf{P}}_0^{-1}}^2+\frac{1}{2}\sum _{i=1}^k}\\ \multicolumn{1}{c}{\{||{\mathbf{e}}_{\mathbf{x}}({\mathbf{x}}_i,{\hat{\mathbf{x}}}_i)|{|}_{{\mathcal{Q}}_i^{-1}}^2+||{\mathbf{e}}_{\mathbf{z}}({\mathbf{z}}_i,\mathbf{h}({\mathbf{x}}_i))|{|}_{{\mathcal{R}}_i^{-1}}^2\},}\end{array}$$

(1)

where (.) denotes the estimated states of the system (optimization variables), ${\hat{\mathbf{x}}}_0\in {\mathbb{R}}^n$ and ${\mathbf{P}}_0\in {\mathbb{R}}^{n\times n}$ are the initial state and its covariance matrix, respectively, ${\widehat{\mathbf{x}}}_i=\mathbf{f}({\stackrel{⌣}{\mathbf{x}}}_{i-1},{\mathbf{u}}_i)$ is the state prediction at the $i$ -th time-step using the model, ${\mathbf{e}}_{\mathbf{x}}:{\mathbb{R}}^n\times {\mathbb{R}}^n\to {\mathbb{R}}^n$ is an error function over the state estimates, ${\mathbf{e}}_{\mathbf{z}}:{\mathbb{R}}^m\times {\mathbb{R}}^n\to {\mathbb{R}}^m$ is an error function over the sensor measurements, and ${\mathcal{Q}}_i\in {\mathbb{R}}^{n\times n}$ and ${\mathcal{R}}_i\in {\mathbb{R}}^{m\times m}$ are the positive definite process and measurement covariance matrices (and their inverse matrices are the information matrices), respectively.

Minimizing the cost function stated in (1) becomes, in general, intractable for long duration of operation due to the growth of the number of optimization variables. Moreover, the effect of older states effect on current states decreases with time. Instead, MHE strategy can be adapted as an alternative solution to a full information MAP. To this end, we define the estimation horizon length $N\in \mathbb{N}\backslash \{0\}$ for the following finite horizon cost function

$$J_{MHE}({\stackrel{⌣}{\mathbf{X}}}_N,{\mathbf{Z}}_N)=\frac{1}{2}\left|\right|{\mathbf{e}}_{\mathbf{x}}({\stackrel{⌣}{\mathbf{x}}}_{k-N},{\widehat{\mathbf{x}}}_{k-N}{)||}_{{\mathbf{P}}_{k-N}^{-1}}^2+\frac{1}{2}\sum _{i=k-N+1}^k\{\left|\right|{\mathbf{e}}_{\mathbf{x}}({\stackrel{⌣}{\mathbf{x}}}_i,{\widehat{x}}_i{)||}_{{\mathcal{Q}}_i^{-1}}^2+||{\mathbf{e}}_{\mathbf{z}}({\mathbf{z}}_i,\mathbf{h}({\stackrel{⌣}{\mathbf{x}}}_i{))\left|\right|}_{{ℛ}_i^{-1}}^2\},$$

(2)

where $N$ is the estimation horizon.

$${\stackrel{⌣}{\mathbf{X}}}_N:=({\stackrel{⌣}{\mathbf{x}}}_{k-N},{\stackrel{⌣}{\mathbf{x}}}_{k-N+1},\dots ,{\stackrel{⌣}{\mathbf{x}}}_k)\in X^{N+1},\mathrm{and}$$

$${\mathbf{Z}}_N:=({\mathbf{z}}_{k-N+1},\dots ,{\mathbf{z}}_k)\in Z^N$$

are the history of the estimated robot states and measurements over the estimation horizon, $\left|\right|{\mathbf{e}}_{\mathbf{x}}({\stackrel{⌣}{\mathbf{x}}}_{k-N},{\widehat{\mathbf{x}}}_{k-N}{)||}_{{\mathbf{P}}_{k-N}^{-1}}^2$ is the arrival cost of the moving horizon problem, and the weighting matrix ${\mathbf{P}}_{k-N}$ is the estimation posterior covariance of the $(k-N)$ -th state and can be computed using the EKF Riccati equation Rao et al. (2003), that is

$${\mathbf{P}}_{k+1}={\mathcal{Q}}_k+F_k{\mathbf{P}}_k{F}_k^{\mathrm{T}}-F_k{\mathbf{P}}_k{H}_k^{\mathrm{T}}({\mathcal{R}}_k+H_k{\mathbf{P}}_k{H}_k^{\mathrm{T}}{)}^{-1}{H}_k{\mathbf{P}}_k{F}_k^{\mathrm{T}},$$

where $F_k\in {\mathbb{R}}^{n\times n}$ and $H_k\in {\mathbb{R}}^{m\times n}$ are the process model and the measurement model Jacobian matrices given by

$$F_k:=\frac{\partial \mathbf{f}}{\partial \mathbf{x}}{|}_{\mathbf{x}={\stackrel{⌣}{\mathbf{x}}}_k},H_k:=\frac{\partial \mathbf{h}}{\partial \mathbf{x}}{|}_{\mathbf{x}={\stackrel{⌣}{\mathbf{x}}}_k}.$$

(3)

The moving horizon optimization problem can now be stated as

$$\underset{{\stackrel{⌣}{\mathbf{X}}}_N\in X^{N+1}}{\min }{J}_{MHE}({\stackrel{⌣}{\mathbf{X}}}_N,{\mathbf{Z}}_N)$$

(4a)

$$\mathrm{subject}\mathrm{to}{\stackrel{⌣}{\mathbf{x}}}_i=\mathbf{f}({\stackrel{⌣}{\mathbf{x}}}_{i-1},u_i)\forall k-N⩽i⩽k,$$

(4b)

$${\stackrel{⌣}{\mathbf{x}}}_i\in X\forall k-N⩽i⩽k,$$

(4c)

$${\mathbf{u}}_i\in U\forall k-N+1⩽i⩽k.$$

(4d)

We remark that, in general, the MHE problem can be defined for any noise distribution, that is, $\mathbf{w}$ and $\mathbf{v}$ . However, note that MHE becomes a least squares problem if and only if additive zero mean Gaussian noises are assumed (Osman et al., 2019b). Furthermore, the MHE deals directly with nonlinearity in the optimization problem without the need for linearization. Finally, unlike recursive filtering techniques, the MHE uses a window of previous states for estimation, this, in turn, leads to an increased robustness and more accurate and smoother trajectory estimation (as will be shown in the results (Section 6).

Multi-sensor fusion using MHE for localization

In this section, we present the main components of the proposed multi-sensor fusion scheme. The generic motion model used for fusion is presented first; secondly, the measurement model and the specific (measurement) data handling and processing methodologies are stated. Finally, the proposed multi-threading fusion algorithm is presented.

Motion model

The states of general mobile robots can be defined as

$$\mathbf{x}:={\left[\begin{array}{ccccc}\mathbf{p}\hfill & \theta \hfill & \stackrel{·}{\mathbf{p}}\hfill & \omega \hfill & \mathbf{a}\hfill \end{array}\right]}^{\mathrm{T}},$$

(5)

where $\mathbf{p}:=[x,y,z{]}^{\mathrm{T}}\in {\mathbb{R}}^3$ is the robot position in Cartesian coordinates, $\theta :=[\alpha ,\beta ,\gamma {]}^{\mathrm{T}}\in {\mathbb{R}}^3$ is the orientation of the robot represented in Euler angles (Lynch and Park, 2017: Chapter 3), $\stackrel{·}{\mathbf{p}}:=[v_x,v_y,v_z{]}^{\mathrm{T}}\in {\mathbb{R}}^3$ , $\omega :=[{\omega }_x,{\omega }_y,{\omega }_z{]}^{\mathrm{T}}\in {\mathbb{R}}^3$ , and $\mathbf{a}:=[a_x,a_y,a_z{]}^{\mathrm{T}}\in {\mathbb{R}}^3$ are the linear velocity, angular velocity, and linear acceleration vectors of the robot with regards to the robot body frame, respectively.

For $k\in {\mathbb{N}}_0$ , the following generic 3D omni-directional motion model is used as the prediction model for the fusion scheme (Siegwart et al., 2011: Chapter 3)

$$\left[\begin{array}{c}{\mathbf{p}}_{k+1}\hfill \\ \multicolumn{1}{c}{{\theta }_{k+1}}\\ \multicolumn{1}{c}{{\stackrel{·}{\mathbf{p}}}_{k+1}}\\ \multicolumn{1}{c}{{\omega }_{k+1}}\\ \multicolumn{1}{c}{{\mathbf{a}}_{k+1}}\end{array}\right]=\left[\begin{array}{ccccc}{\mathbf{I}}_{3\times 3}\hfill & 0_{3\times 3}\hfill & \tau R_k\hfill & 0_{3\times 3}\hfill & \frac{{\tau }^2}{2}{R}_k\hfill \\ \multicolumn{1}{c}{0_{3\times 3}}& {\mathbf{I}}_{3\times 3}\hfill & 0_{3\times 3}\hfill & \tau {\mathbf{I}}_{3\times 3}\hfill & 0_{3\times 3}\hfill \\ \multicolumn{1}{c}{0_{3\times 3}}& 0_{3\times 3}\hfill & {\mathbf{I}}_{3\times 3}\hfill & 0_{3\times 3}\hfill & \tau {\mathbf{I}}_{3\times 3}\hfill \\ \multicolumn{1}{c}{0_{3\times 3}}& 0_{3\times 3}\hfill & 0_{3\times 3}\hfill & {\mathbf{I}}_{3\times 3}\hfill & 0_{3\times 3}\hfill \\ \multicolumn{1}{c}{0_{3\times 3}}& 0_{3\times 3}\hfill & 0_{3\times 3}\hfill & 0_{3\times 3}\hfill & {\mathbf{I}}_{3\times 3}\hfill \end{array}\right]\left[\begin{array}{c}{\mathbf{p}}_k\hfill \\ \multicolumn{1}{c}{{\theta }_k}\\ \multicolumn{1}{c}{{\stackrel{·}{\mathbf{p}}}_k}\\ \multicolumn{1}{c}{{\omega }_k}\\ \multicolumn{1}{c}{{\mathbf{a}}_k}\end{array}\right]$$

(6)

where $\tau >0$ is the sampling time, $0_{3\times 3}$ and ${\mathbf{I}}_{3\times 3}$ are $3\times 3$ zero and identity matrices with respective dimensions defined by the subscript, and finally $R_k:=R({\alpha }_k,{\beta }_k,{\gamma }_k)\in \mathrm{SO}(3)$ is the 3D rotation matrix of the robot relative to the world frame, where $\mathrm{SO}(3)$ is the special orthogonal group (Lynch and Park, 2017: Chapter 3).

Measurements model

The localization of a mobile robotic platform can be accomplished through odometry computation (odometry-based); for example, LiDAR, visual, or wheel odometries, through map-based localization, for example, landmark-based and grid-based localization (Thrun et al., 2005), or by exteroceptive sensors such as GPS or magnetometer.

In this study, we assume that each of the robot’s on-board sensors provides a measurement for a subset of the states $\mathbf{x}$ ; thus, the measurement model for each sensor used for the localization scheme is linear as follows

$${\mathbf{z}}_k^j=C^j{\mathbf{x}}_k^j,$$

(7)

where $j\in \{1,\dots M\}$ is the sensor index, $M$ is the number of sensors used for localization, $C^j\in {\mathbb{B}}^{m_j\times n}$ is the measurement matrix of the $j$ -th sensor, where $\mathbb{B}:=\{0,1\}$ .

Assuming linear measurement models is acceptable, because most of the sensors used in the localization of autonomous platforms, in general, provide a subset of the vehicle states. For example, a visual or LiDAR odometry of a ground mobile robot provides a measurement of $[x,y,\gamma ]$ , a barometer can be used to provide the altitude $z$ of a quad-copter, and a gyroscope can provide a measurement of the angles $[\alpha ,\beta ,\gamma ]$ .

Error definition

As explained in Section 3, the MHE cost function (2) embodies the error in the model and the measurement. The error for the position, velocity, angular velocity, and acceleration in the Cartesian space as

$${\mathbf{e}}_{\mathbf{p}}(\stackrel{⌣}{\mathbf{p}},\hat{\mathbf{p}}):=\stackrel{⌣}{\mathbf{p}}-\hat{\mathbf{p}},$$

$${\mathbf{e}}_{\mathbf{v}}(\stackrel{⌣}{\mathbf{v}},\hat{\mathbf{v}}):=\stackrel{⌣}{\mathbf{v}}-\hat{\mathbf{v}},$$

$${\mathbf{e}}_{\omega }(\stackrel{⌣}{\omega },\hat{\omega }):=\stackrel{⌣}{\omega }-\hat{\omega },$$

$${\mathbf{e}}_{\mathbf{a}}(\stackrel{⌣}{\mathbf{a}},\hat{\mathbf{a}}):=\stackrel{⌣}{\mathbf{a}}-\hat{\mathbf{a}}.$$

As for the orientation, we employ the formulation of the rotation error given by Campa and De La Torre (2009)

$${\mathbf{e}}_{\theta }(\stackrel{⌣}{\theta },\hat{\theta }):=\left[\begin{array}{c}{\tilde{r}}_{32}-{\tilde{r}}_{23}\hfill \\ \multicolumn{1}{c}{{\tilde{r}}_{13}-{\tilde{r}}_{31}}\\ \multicolumn{1}{c}{{\tilde{r}}_{21}-{\tilde{r}}_{12}}\end{array}\right],$$

where ${\tilde{r}}_{\mathrm{ij}}$ is the $i$ -th row and $j$ -th column element of the error rotation matrix $\tilde{R}\in \mathrm{SO}(3)$ defined as

$$\tilde{R}:=R({\stackrel{⌣}{\alpha }}_k,{\stackrel{⌣}{\beta }}_k,{\gamma }_k)R^{\mathrm{T}}({\hat{\alpha }}_k,{\hat{\beta }}_k,{\hat{\gamma }}_k).$$

Finally, the error on the states ${\mathbf{e}}_{\mathbf{x}}$ can be defined as

$${\mathbf{e}}_{\mathbf{x}}(\stackrel{⌣}{\mathbf{x}},\hat{\mathbf{x}}):=\left[\begin{array}{c}{\mathbf{e}}_{\mathbf{p}}(\stackrel{⌣}{\mathbf{p}},\hat{\mathbf{p}})\hfill \\ \multicolumn{1}{c}{{\mathbf{e}}_{\theta }(\stackrel{⌣}{\theta },\hat{\theta })}\\ \multicolumn{1}{c}{{\mathbf{e}}_{\mathbf{v}}(\stackrel{⌣}{\mathbf{v}},\hat{\mathbf{v}})}\\ \multicolumn{1}{c}{{\mathbf{e}}_{\omega }(\stackrel{⌣}{\omega },\hat{\omega })}\\ \multicolumn{1}{c}{{\mathbf{e}}_{\mathbf{a}}(\stackrel{⌣}{\mathbf{a}},\hat{\mathbf{a}})}\end{array}\right],$$

(8)

and the error on the measurements of each sensor ${\mathbf{e}}_{\mathbf{z}}^j(\mathbf{z},\mathbf{h}(\stackrel{⌣}{\mathbf{x}}))$ can be defined based on the measurements of the sensor as

$${\mathbf{e}}_{\mathbf{z}}^j(\mathbf{z},\mathbf{h}(\stackrel{⌣}{\mathbf{x}})):=(C^j\circ {\mathbf{e}}_{\mathbf{x}})(\mathbf{z},\mathbf{h}(\stackrel{⌣}{\mathbf{x}})).$$

(9)

where the ° operator denotes the function composition.

Multi-sensor fusion scheme

In order to develop a generic framework for the MHE localization, the following considerations need to be addressed:

1. Different measurement rates of the sensors as well as the missed inter-sample values need to be properly handled.

2. The localization algorithm needs to be computationally efficient so that it can be implemented in real-time.

3. The algorithm needs to be adjustable to different kinematic constraints depending on the platform type; for example, holonomic, non-holonomic.

4. Outliers need to be properly handled to prevent the inclusion of spurious measurements in the optimization because they can significantly deteriorate the estimation performance.

Each of the aforementioned considerations is addressed through the proposed multi-sensor fusion scheme as follows.

Multi-rate sensor fusion

The proposed localization algorithm does not impose any limits on the update rates of the sensors being fused. Here, the localization scheme relies on solving the optimization problem (4) using a fixed but configurable sampling rate. The different sensor measurements are received at different times, that is, no synchronization is required, and depending on the measurement received during the sampling time duration, a new iteration of the MHE problem is formulated using the available data (see Figure 1 for an illustration of the idea). The different queues of the data over the estimation horizon are updated with the new measurements; then, the measurements older than the estimation horizon are discarded. The cost function (2) is then updated by the new available measurements.

Figure 1.

Multi-rate MHE-based localization.Note: Diamond shapes represent the available measurements produced from each sensor.

Notice that the multi-rate sensor fusion approach along with the MHE scheme that does not only rely on the previous state and measurements but also takes a window of them, adds robustness to the localization scheme. This will be highlighted in Section 6. Although the performance and accuracy of filtering-based schemes can deteriorate if measurements are missing even for one timestep, MHE can be robust to missing measurements as long as there is some measurements in the estimation horizon $N$ .

Multi-threading architecture

The computational cost of solving the MHE problem is highly dependent on the estimation horizon length $N$ . Nonetheless, some considerations can be taken to reduce the computational cost of the data handling as well as the computation of the information matrices (the inverse of the covariance matrix). Consequently, in order to keep the computational cost as low as possible given the estimation horizon $N$ , the proposed localization scheme is developed in a multi-threading architecture. Specifically, the measurement data is received and handled in a thread separated from the main thread of solving the MHE optimization problem (4) (see Figure 2).

Figure 2.

The multi-threading architecture of the MHE localization scheme.

Thread 1 is responsible for computing the information matrices for each sensor, building the measurement matrices, that is

$${\mathbf{z}}_k:=\left[\begin{array}{c}{\mathbf{z}}_k^1\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{{\mathbf{z}}_k^M}\end{array}\right],C_k:=\left[\begin{array}{c}{C}_k^1\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{C_k^M}\end{array}\right],\mathrm{and}{\mathcal{R}}_k:=\left[\begin{array}{ccc}{\mathcal{R}}_k^1\hfill & \dots \hfill & 0\hfill \\ \multicolumn{1}{c}{⋮}& \ddots \hfill & ⋮\hfill \\ \multicolumn{1}{c}{0}& \dots \hfill & {\mathcal{R}}_k^M\hfill \end{array}\right],$$

as well as propagating the motion model through the next time-step to compute the posterior covariance matrix. The measurement vector ${\mathbf{z}}_k$ and the information matrix ${\mathcal{R}}_k^{-1}$ are pushed into the estimation horizon queue and the oldest measurement in the queue is discarded while only keeping the posterior covariance (calculated using equation (3)) for the arrival cost calculation. In Thread 2, the optimization problem (4) is updated and solved.

This architecture reduces the computational cost of the MHE through dividing the required computations on two threads, which can work in parallel. The first thread can receive the measurements, compute the new prediction, invert the covariance matrices, and form the measurements vector while the other thread is solving the previous optimization problem. This results in a reduction in the overall computational time of the MHE and increases lower bound on the sampling time as shown in Figure 3.

Figure 3.

Visualization of the computational time for the MHE scheme in the multi-threading case as well as the unthreaded case.Note: The computational time decrease in case of the multi-threading case as new measurements can be received and processed before the previous optimization problem is solved.

Compensation for the vehicle type

The proposed MHE localization scheme uses a generic omni-directional motion model (equation (6)) for state prediction. Therefore, when solving the MHE optimization problem, certain kinematic constraint are further induced over the optimization problem to compensate for the vehicle type. For example, a differential mobile robot has a constraint on lateral motion, that is, $v_y=0$ . Such constraint is not directly modeled in the aforementioned motion model, but it can be imposed as a constraint in the MHE framework.

Outlier rejection

Measurement outliers are detected and removed using the Mahalanobis’ distance thresholding method (Mahalanobis, 1936) as follows

$${\mathcal{E}}_k^j={\mathbf{e}}_{\mathbf{z}}^T({\mathbf{z}}_k^j,{\hat{\mathbf{z}}}_k^j){\mathcal{R}}^{-1}{\mathbf{e}}_{\mathbf{z}}({\mathbf{z}}_k^j,{\hat{\mathbf{z}}}_k^j)⩽{\mathcal{G}}^j,j\in \{1,\dots M\},$$

(10)

where ${\mathcal{E}}_k^j$ is the Mahalanobis’ distance, and ${\mathcal{G}}^j\in {\mathbb{R}}_{>0}$ is the configurable Mahalanobis’ threshold. Algorithm 1 shows the proposed MHE-based localization framework. Its implementation using robot operating system ( ROS) is available online. ¹

Algorithm 1.

MHE generic multi-sensor fusion scheme for autonomous vehicles localization.

1 Set The estimation horizon $N$ , number of sensors $M$ , processcovariance $\mathcal{Q}$ , sampling time $\tau$ , measurement matrices $C^i$ , and thethreshold ${\mathcal{G}}^i$ for $i\in \{1,\dots ,M\}$ 2 Set the optimization constraints. 3 for $k\in \{1,...,\infty \}$ do 4 Thread 1: 5 { 6 Propagate the states ${\stackrel{⌣}{\mathbf{x}}}_{k-1}$ through the motion model. 7 for $i\in \{1,\dots ,M\}$ do 8  Receive the available sensors’ measurements and covariance matrices ${\mathbf{z}}_k^i,{\mathcal{R}}_k^i$ . 9  Calculate the information matrix ${\mathcal{R}}_k^{i-1}$ for each measurement. 10  Calculate the Mahalanobis distance ${\mathcal{E}}_i$ for each measurement via equation (13). 11  if ${\mathcal{E}}_k^i<{\mathcal{G}}^i$ then 12   Push the sensor measurement and the information matrix to ${\mathbf{z}}_k$ and ${\mathcal{R}}_k^{-1}$ . 13  end 14 end 15 Push the measurement vector ${\mathbf{z}}_k$ and information matrix ${\mathcal{R}}_k^{-1}$ to the estimation horizon queue and remove the oldest measurement from the queue. 16 Calculate the posterior covariance using equation (3). 17 } 18 Thread 2: 19 { 20 Update the optimization problem (4) to match the numberand the type of the available sensor’s measurements. 21 Solve the optimization problem (4) 22 } 23 end

Data sets used for simulations and experiments

The proposed localization scheme (Algorithm 1) was developed using ROS (Quigley et al., 2009). Here, the optimization problem (4) is formulated symbolically using CasADi: an open-source software tool for numerical optimization (Andersson et al., 2019). The optimization problem is then solved using the interior point method (Helmberg et al., 1996).

In order to validate the MHE localization scheme, several numerical and experimental data sets are used: (1) a simulated data of 3D motion of an aerial vehicle; (2) several scenarios from EU long-term dataset for autonomous driving (Yan et al., 2019); and (3) experimental sequences generated using Summit-XL Steel omni-directional mobile robot, see Figure 4. All the experiments and dataset runs are performed using a computer with Intel i7-8850H 6-core processor running at 2.60 GHz and a 16 GB RAM. The computer is running Ubuntu version 16.04 and ROS Kinetic.

Figure 4.

The omni-directional mobile robot used for validating the MHE multi-sensor fusion.

Aerial vehicle simulation

A simulated aerial vehicle is used for the validation of the proposed MHE localization scheme. The simulated vehicle is commanded to perform a spiral motion and is assumed to be equipped with a camera running a visual odometry (data are simulated), a GPS sensor, a barometer to measure the altitude of the vehicle, and an inertial measurement unit (IMU). Each of the sensors is assumed to have additive Gaussian noise with zero mean; Table 1 shows the measurements of each of the sensors as well as their covariance matrices. The simulated motion is generated using the same omni-directional model stated in equation (6) with the constant velocities $v_x=0.2$ m/s, $v_z=0.3$ m/s, and ${\omega }_z=0.1$ rad/s.

Table 1.

The simulated sensors measurements and covariances for the aerial vehicle simulation.

Sensor	Measurements	Covariance
Camera (VO)	$x,y,z,\alpha ,\beta ,\gamma$	$0.1{\mathbf{I}}_{6\times 6}$
GPS	$x,y$	$0.1{\mathbf{I}}_{2\times 2}$
IMU	${\omega }_x,{\omega }_y,{\omega }_y,a_x,a_y,a_z$	$0.1{\mathbf{I}}_{6\times 6}$
Barometer	$z$	0.1

EU long-term dataset

The EU long-term dataset is an open-source dataset for autonomous driving, which contains the sensors data from multiple heterogeneous sensors. The data was collected using the University of Technology of Belfort Montbliard (UTBM) vehicle in human driving mode. The vehicle is driven in the downtown of Montpelier in France (Yan et al., 2019). For the long-term data, the driving distance is about 5.0 km per session driven in $16$ minutes. Here, nine driving data sequences from the dataset are used to validate the proposed localization algorithm.

The ground-truth for this dataset is generated by a GPS/RTK sensor that can achieve very accurate position measurements. ² Therefore, an artificial Gaussian noise $\mathcal{N}(0,{\mathcal{Q}}_{\mathrm{gps}})$ with ${\mathcal{Q}}_{\mathrm{gps}}=\mathrm{diag}(25[{\mathrm{m}}^2],25[{\mathrm{m}}^2],25[\stackrel{2}{\mathrm{deg}}])$ is added to the GPS data and used for fusion. Furthermore, an open-source implementation of the the LiDAR odometry and mapping (LOAM) algorithm (Zhang and Singh, 2014) is used as LiDAR odometry measurements. The proposed MHE localization algorithm is then validated by fusing the noisy GPS, the LOAM, and the IMU measurements. The algorithm is validated using nine sequences of the UTBM dataset with a total of about $45$ km driving distance. As for the covariance matrices needed for the fusion, the covariance estimation algorithm proposed in Osman et al. (2018) and Osman et al. (2019c) is used. This algorithm uses the measurements of a sensor which does not suffer from drift such as GPS in order to estimate the covariance of an odometry which suffer from drift such as LiDAR odometry.

Summit-XL Steel omnidirectional robot

Summit XL Steel is a ground mobile robot with meccanum wheels manufactured by Robotnik Inc. ³ The robot is equipped with a Velodyne LiDAR sensor ⁴ , an Astra RGB-D Camera ⁵ , a Pix-hawk 3 auto-pilot used as an IMU, ⁶ and wheel encoders. Several experiments were conducted using the robot to validate the proposed MHE localization algorithm while using a VICON motion capture system to generate the ground-truth data. The VICON system used here consists of 12 cameras. The VICON bridge package was used to couple VICON with ROS. ⁷ The sensor fusion was performed using measurements from the IMU, wheel odometry, and 2D LiDAR odometry (Censi, 2008).

Evaluation metrics

The performance of the proposed MHE localization algorithm is compared against a UKF-based localization scheme implemented in Moore and Stouch (2014). Both schemes are evaluated by, first, the mean and the maximum error percentages of the translation, calculated as shown in equation (11) and equation (12), respectively

$$T{E}_{mean}[\%]=\frac{1}{L\cdot D}\sum _{k=1}^N{||{\stackrel{⌣}{\mathbf{p}}}_k-{\mathbf{p}}_k||}_2,$$

(11)

$$T{E}_{max}[\%]=\frac{1}{D}\cdot \underset{k\in {\mathbb{N}}_0}{\max }{||{\stackrel{⌣}{\mathbf{p}}}_k-{\mathbf{p}}_k||}_2,$$

(12)

where $\mathrm{TE}$ stands for translation error, ${\stackrel{⌣}{\mathbf{p}}}_k$ is the estimated position of the vehicle, ${\mathbf{p}}_k$ is the true position at time-step $k$ , $L$ is the total number of estimated poses in the driving sequence, $D$ is the total distance of the driving sequence, and $||·|{|}_2$ is the $l_2$ -norm.

The performance of the orientation is evaluated by the mean and the maximum of the orientation error divided by $D$ , which are calculated as shown in equation (13) and equation (14), respectively.

$$O{E}_{mean}{[}^o/m]=\frac{1}{L\cdot D}\sum _{k=1}^N\parallel {\stackrel{⌣}{\theta }}_k-{\theta }_k{\parallel }_1,$$

(13)

$$O{E}_{max}{[}^o/m]=\frac{1}{D}\cdot \underset{k\in {\mathbb{N}}_0}{\max }\parallel {\stackrel{⌣}{\theta }}_k-{\theta }_k{\parallel }_1,$$

(14)

where $\mathrm{OE}$ stands for orientation error, ${\stackrel{⌣}{\theta }}_k$ is the estimated orientation of the vehicle and ${\theta }_k$ is the true orientation of the vehicle at time step $k$ , and $||·|{|}_1$ is the $l_1$ -norm.

UKF

UKF is a nonlinear extension of the linear Kalman filter algorithm (Julier and Uhlmann, 1997, 2004). However, in case of the UKF, the algorithm deals with the nonlinear system directly without the need for linearization that leads to increased accuracy of the state estimation. UKF uses the unscented transform to propagate the error through the nonlinear system directly but still in this case both the prior prediction and observation are assumed to be Gaussian random variables.

The UKF scheme used for comparison in this paper uses the same parameters used in the MHE. However, as the UKF uses the unscented transform for noise propagation, some extra parameters a present such as the number of sigma points and their weights. Notice that these parameters were defined exactly as stated in the original paper (Wan and Van Der Merwe, 2000).

Process noise and measurement covariances

To achieve good localization accuracy using MHE (and many other sensor fusion techniques), the process noise covariance needs to be chosen carefully. Although the determination of the process noise covariance $\mathcal{Q}$ is usually accomplished through tuning, the knowledge of the system can help achieve more accurate values especially due to the use of a generic kinematic model for prediction. For example, if the scheme is going to be used with a non-holonomic, then the lateral velocity $v_y$ variance should be enlarged because the model does not describe the non-holonomic constraints and consequently will estimate lateral motion, which is not accurate considered the actual system. The same is true in the case of a ground robot that cannot have a linear acceleration $a_z$ and so on. Furthermore, due to the difficulty in quantifying the cross-correlation terms of the covariance, the covariance matrix is commonly defined as a diagonal matrix of the variances.

As for the measurement covariance of the sensors, there are several methods of determining them: (i) The characteristics and specifications of the sensors could contain the variances directly; (ii) Through tuning based on multiple experiments using the sensor; (iii) The sensor itself could provide its measurements along with the covariance; (iv) Through using a covariance estimation algorithm such as Osman et al. (2019c) and Osman et al. (2018).

Results and discussion

In this section, the results of implementing Algorithm 1 are presented for the simulated aerial vehicle, UTBM dataset, and Summit-Xl Steel omnidirectional robot.

Simulated aerial vehicle

As mentioned earlier, a spiral motion of an aerial vehicle was simulated for validating the MHE localization scheme. Figure 5 shows the 3D path taken by the simulated drone as well as the MHE and UKF estimation outputs. It can be noticed that the MHE estimation is smoother and more accurate than that of the UKF. Figure 6 shows the error in the estimation of the MHE and UKF. The figure demonstrates the superior performance of the MHE compared with UKF for both translation as well as orientation estimation. Table 2 shows the quantitative results of the aerial vehicle simulation. The MHE estimation is almost as twice as accurate as the UKF. The results of this simulation concludes that the MHE localization algorithm outperforms UKF.

Figure 5.

The estimated trajectory by MHE and UKF as well as the ground truth of the simulated aerial vehicle spiral motion.

Figure 6.

The translation and orientation errors in MHE and UKF for the simulated aerial vehicle spiral motion.

Table 2.

Mean and maximum translation and orientation errors of MHE ( $N=10$ ) and UKF for the simulated spiral path (total distance = $90$ m).

Fusionalgorithm	$T{E}_{\mathrm{mean}}$	$T{E}_{\max }$	$O{E}_{\mathrm{mean}}$	$O{E}_{\max }$
MHE	0.076	0.2457	0.02796	0.0888
UKF	0.1257	0.3291	0.04579	0.20774

In order to further validate these results with real experimental data. The following two sections shows the results of the fusion using 2D sequences using the UTBM platform as well as the omnidirectional Summit-XL Steel robot. In order to show the feasibility of the algorithm for practical purposes, the computation time of the algorithm is also studied using one of the UTBM sequences.

EU long-term dataset

In Figure 7, the MHE output as well as the measurements from the GPS and LOAM are presented. The MHE achieves a very accurate pose estimation as well as smooth trajectory given the jumpy and discontinuous GPS measurements. As for LOAM measurement, in order to limit the amount of drift, caused by motion increments integration, the MHE or UKF output was used as a corrective feedback, in order to correct for the drift when fusing the odometry measurements. This correction along with the Mahalanobis’ thresholding outlier rejection led to better fusion results by mitigating the effect of drift from the LiDAR odometry on the fusion output. Moreover, throughout these sequences, the lateral velocity $v_y$ was constrained to a small value $|v_y|<0.1$ so that the prediction model of the MHE would be realistic for an Ackermann steering vehicle. Such a constraint enhances the orientation estimation accuracy of the vehicle.

Figure 7.

The estimated trajectory by MHE plotted with the GPS and the LOAM measurements (without correction) as well as the ground truth data for sequence 2018-05-02 (1) from EU long-term dataset.

Figure 8 and Figure 9 show the estimation output from the MHE and UKF as well as the translation and orientation errors along the path of the sequence 2018-05-02 (2). The MHE output is smoother and more accurate when compared with the output of the UKF (except for one instance where the maximum error of the path in MHE case is worse than that of the UKF). Figure 10 and Figure 11 confirms that the MHE outperforms the UKF in both translation and orientation errors. It also confirms that the MHE is able to produce smoother paths.

Figure 8.

The estimated trajectory by MHE and UKF as well as the ground truth for sequence 2018-05-02 (2) from EU long-term dataset.

Figure 9.

Translation and orientation errors in MHE and UKF pose estimation for sequence 2018-05-02 (2) from EU long-term dataset.

Figure 10.

The estimated trajectory by MHE and UKF as well as the ground truth for sequence 2018-07-19 from EU long-term dataset.

Figure 11.

Translation and orientation errors in MHE and UKF pose estimation for sequence 2018-07-19 from EU long-term dataset.

Figure 12 shows the MHE and UKF estimated paths for sequence 2019-01-31, which is slightly different and longer than the rest of the sequences. Note that during this sequence, there are missing GPS measurements as shown in the figure. At these instances, the MHE scheme managed to estimate the path correctly with minimal error while the UKF output diverged until the GPS measurements became available again. This is expected because the UKF algorithm depends on the latest measurements and prediction in order to estimate the pose while the MHE uses a window of old measurements for estimation ( $N=10$ in this case). This adds more robustness and stability against missing measurements and gives more room to rejecting outliers without degrading the performance of the estimator. Furthermore, again in this sequence, the MHE shows much smoother estimation compared with the UKF.

Figure 12.

The estimated trajectory by MHE and UKF as well as the ground truth and GPS measurements for sequence 2019-01-31 from EU long-term dataset.

The overall quantitative results of the MHE fusion are shown in Table 3. The results show that the MHE fusion outperforms UKF fusion in every driving sequence of the EU Long-term dataset (except for the maximum orientation error of sequence 2018-05-02 (2)). The overall percentage error over the nine sequences is $0.05949\%$ which is about $33\%$ better than the UKF estimation. This is also the case for orientation accuracy, which is better by $38\%$ . This is a considerable enhancement in the performance of the pose estimator of the sensor fusion module in any autonomous platform.

Table 3.

Mean and maximum translation and orientation errors of MHE ( $N=10$ ) and UKF for UTBM sequences.

Seq. date	MHE				UKF				Overall distance
	$T{E}_{\mathrm{mean}}$	$T{E}_{\max }$	$O{E}_{\mathrm{mean}}$	$O{E}_{\max }$	$T{E}_{\mathrm{mean}}$	$T{E}_{\max }$	$O{E}_{\mathrm{mean}}$	$O{E}_{\max }$
2018-05-02 (1)	0.0593	0.1579	0.00049	0.00253	0.0738	0.1928	0.00076	0.00296	5076
2018-05-02 (2)	0.0597	0.3546	0.0005	0.00640	0.0721	0.4007	0.00079	0.00311	5071
2018-07-13	0.0618	0.1897	0.0005	0.00203	0.0622	0.2337	0.00078	0.00316	5028
2018-07-16	0.0563	0.2428	0.00049	0.00242	0.0893	0.2920	0.00085	0.00609	5010
2018-07-17	0.0573	0.4965	0.00049	0.00224	0.0910	0.5557	0.00079	0.00312	5063
2018-07-19	0.0577	0.2081	0.0005	0.00241	0.0905	0.3431	0.00078	0.00348	4980
2018-07-20	0.0622	0.1939	0.0005	0.00224	0.0897	0.2677	0.00083	0.00313	5001
2019-01-31	0.0553	0.2149	0.00038	0.00184	0.1343	7.4508	0.00069	0.00465	6443
2019-04-18	0.0664	0.3356	0.0005	0.00276	0.0885	0.3409	0.00078	0.00312	5060
Overall	0.05949	0.2648	0.00048	0.00274	0.0893	1.3102	0.00078	0.00367	46740

Such enhancement in the pose estimation comes with the cost of increased computation time due to solving a constrained optimization problem every time-step. Table 4 shows the mean translation and orientation errors for different estimation horizon sizes as well as the average computation time for each time-step. Notice that the estimation accuracy increases alongside the estimation horizon size. However, this in turn increases the computation cost of the algorithm. The results stated above, in Table 3, were gathered while both algorithms (UKF and MHE) were running with a frequency of $5$ Hz. The average computational time of UKF was $51$ ms, which is lower than the computational time of MHE even for a prediction horizon of $N=2$ . However, although the MHE fusion increases the computational time, the values indicated in Table 4 are still acceptable for practical purposes since, for example for $N=10$ , the estimation algorithm can work with a frequency up to $6$ Hz. Furthermore, for higher estimation frequencies, lower estimation horizons could be used to increase the speed of the fusion algorithm while maintaining better estimation accuracy compared with filtering techniques such as UKF.

Table 4.

Average translational error and computation time for different estimation horizons in MHE estimation in sequence 2018-08-02 (1) from EU long-term dataset.

Horizon $N$	$T{E}_{\mathrm{mean}}$	$O{E}_{\mathrm{mean}}$	Mean comp.time (ms)
2	0.1076	0.01496	54
3	0.09953	0.00790	58
4	0.07571	0.00119	73
5	0.06984	0.00078	89
10	0.05938	0.00049	148
20	0.06471	0.00059	325

Although theoretically speaking, increasing the estimation horizon $N$ should lead to better accuracy, the accuracy of MHE is limited with the computation power of the processing unit used because increasing the prediction horizon also increases the computation time of the MHE. Notice that in the case of $N=20$ , the error started increasing again due to higher computational cost that leads to lower sampling times resulting in reduced accuracy of the estimation.

Furthermore, another factor that affects the accuracy of the MHE scheme is the relation between the number of sensors used, and the computational cost of the optimization problem. As the number of sensors used increase, the computational cost of the scheme increases which might affect the performance of the MHE. Consequently, with the increase of the the number of sensors, the estimation horizon $N$ might need to be decreased to avoid bad performance due to high computational cost (for more information about the interior point optimization computational complexity and its relation with the number of optimization variables, see Potra and Wright (2000)). For a more thorough discussion of the effect of the estimation horizon $N$ , see Rawlings and Bakshi (2006).

Summit-XL steel omnidirectional robot

Using the Summit-XL Steel robot shown in Figure 4, three sequences of data were generated for validating the MHE localization algorithm; the generated data are from executing two rectangular paths and one circular path.

In Figure 13, the ground-truth taken by the VICON is plotted as well as each individual sensor measurement and the pose estimation using the proposed localization scheme. As can be seen, the MHE output is more accurate compared with the different odometries. Furthermore, the output of the estimation is smooth enough, which makes it suitable for navigation and control purposes.

Figure 13.

The estimated trajectory by MHE as well as the sensors measurement and the ground truth for rectangle sequence (2) executed by Summit-XLS omnidirectional robot.

In Figure 14, another rectangular path is plotted by the ground-truth data and the estimation output from MHE and UKF algorithms. The figure shows that the accuracy of the MHE algorithm is higher than that of the UKF. Furthermore, the estimation of the UKF shows oscillations in the estimated path while the estimated path by MHE is much smoother.

Figure 14.

The estimated trajectory by MHE and UKF as well as the ground truth for a rectangle (1) executed by Summit-XLS omnidirectional robot.

In Figure 15, the translation and orientation errors for the rectangular path (1) are reported. As can be seen, the performance of MHE is better than that of UKF. Furthermore, notice that although at some instances the estimation error in UKF seems to be better than that of the UKF, this can be attributed to the fact that the oscillations in the UKF shifted the estimation closer to the ground truth. This may show that the results of the UKF at some instances are more accurate compared with MHE; however, such oscillations indicated the lower stability and robustness of the UKF output.

Figure 15.

Euclidean error (top) and absolution orientation error (bottom) in MHE and UKF pose estimation for rectangle sequence(1) Summit-XLS robots.

For further validation of the MHE algorithm using a different scenario, a circular path was executed using the robot. As can be seen in Figure 16 and Figure 17, the MHE estimation output is more accurate compared with that of different sensors odometries as well as the output of UKF fusion algorithm. Furthermore, in this scenario, the stability of the MHE algorithm is much more obvious as the output of UKF suffered from large amount of oscillations unlike the MHE, which did not suffer from any considerable oscillations in its output.

Figure 16.

The estimated trajectory by MHE as well as the sensors measurement and the ground truth for a circle sequence executed by Summit-XLS omnidirectional robot.

Figure 17.

The estimated trajectory by MHE as well as the sensors measurement and the ground truth for a circle sequence executed by Summit-XLS omnidirectional robot.

Finally, Table 5 shows the quantitative results from Summit-XL Steel robot sequences. The translation and orientation errors from both MHE and UKF estimation output is reported for the three scenarios. It can be seen that MHE outperforms UKF in all cases and for all evaluation metrics.

Table 5.

Mean and maximum translation errors of MHE ( $N=10$ ) and UKF for Summit-XLS sequences.

Scenario Name	MHE				UKF				Distance [m]
	$T{E}_{\mathrm{mean}}$	$T{E}_{\max }$	$O{E}_{\mathrm{mean}}$	$O{E}_{\max }$	$T{E}_{\mathrm{mean}}$	$T{E}_{\max }$	$O{E}_{\mathrm{mean}}$	$O{E}_{\max }$
Rectangle (1)	0.4547	0.9722	0.06267	0.3041	0.5769	1.2801	0.0930	0.2976	12.6
Rectangle (2)	0.3152	0.7072	0.05891	0.2711	0.3504	0.8376	0.09490	0.3134	12.5
Circle	0.3446	1.3476	0.08496	0.8477	0.4184	5.0123	0.1516	2.8466	6.5
Overall	0.3768	0.9446	0.06577	0.4028	0.4547	1.8727	0.1058	0.8282	31.6

Conclusion

In this paper, a generic multi-sensor fusion scheme using MHE was proposed for the localization of autonomous vehicles. The proposed scheme takes into account sensors with different update rates, missed measurements and outlier rejection; different vehicle types; and real-time applicability. The proposed strategy was tested using data generated numerically and experimentally; that is, autonomous driving sequences from EU Long-term dataset as well as experimental sequences using an omni-directional mobile robot. The MHE localization output was compared against that of the UKF. The MHE results showed superior accuracy, better stability, and robustness in all cases.

While the proposed localization scheme meet the computational requirements up to certain levels, the future work aims at further reducing the computational demand of the proposed algorithm. This will be done by investigating improved code-generation techniques to reduce the optimization problem solution time. Furthermore, the stability of the estimator will be studied based on the findings of Rao and Rawlings (2000).