Error State Extended Kalman Filter Localization for Underground Mining Environments

Abstract

The article addresses the issue of mobile robotic platform positioning in GNSS-denied environments in real-time. The proposed system relies on fusing data from an Inertial Measurement Unit (IMU), magnetometer, and encoders. To get symmetrical error gauss distribution for the measurement model and achieve better performance, the Error-state Extended Kalman Filter (ES EKF) is chosen. There are two stages of vector state determination: vector state propagation based on accelerometer and gyroscope data and correction by measurements from additional sensors. The error state vector is composed of the velocities along the x and y axes generated by combining encoder data and the orientation of the magnetometer around the axis z. The orientation angle is obtained from the magnetometer directly. The key feature of the algorithm is the IMU measurements’ isolation from additional sensor data, with its further summation in the correction step. Validation is performed by a simulation in the ROS (Robot Operating System) and the Gazebo environment on the grounds of the developed mathematical model. Trajectories for the ES EKF, Extended Kalman Filter (EKF), and Unscented Kalman Filter (UKF) algorithms are obtained. Absolute position errors for all trajectories are calculated with an EVO package. It is shown that using the simplified version of IMU’s error equations allows for the achievement of comparable position errors for the proposed algorithm, EKF and UKF.

1. Introduction

Despite the technological advancement in indoor positioning over recent years, real-time localization in underground mining environments remains an important issue [1]. The underground mining operation is an intensive process that requires constant updating of the layout plan, as well as roof subsidence monitoring. Automation of this work with the help of robotic equipment and novel technologies, such as computer vision [2], predictive model control [3], machine learning [4] and neural networks [5,6], will increase the efficiency of the mining process, in general, [7,8,9,10], and mine surveying, in particular, [11,12,13].

Sensor fusion is the de facto standard technique for outdoor and indoor localization and mapping [14,15]. This method increases the reliability of the system along with the accuracy of estimated parameters by merging information from a collection of sensors. On the other hand, by providing redundant measurements this method results in significant computational costs and requires the use of powerful CPUs or special optimization techniques [16,17].

GNSS/IMU systems [18] are one of the most common solutions for outdoor positioning as GNSS signals correct IMU observations prone to drift. This paper focuses on localization in GNSS-denied environments, such as underground mine work. The absence of GNSS signals leads to alternative sensor combinations for indoor positioning. There are three core categories of indoor sensor fusion techniques based on types of main sensors: LiDAR-based, visual-based, and multi-sensor approach.

The work [19] proposes LiDAR-IMU data integration in real-time for UAVs localization in environments with poor lighting conditions. In addition to IMU, LiDARs and cameras [20] are often used together for IMU data correction in indoor environments. If the primary objective is only localization IMU data can be corrected by means of magnetometer and wheel encoder observation [21].

In [22,23], a scheme of a single-channel vertical with an adjustable pendulum based on a three-axis accelerometer and one free gyroscope is proposed, which ensures the unperturbation of navigation reckoning. The main difference between the proposed scheme and the IMU is the accelerometers’ data compensation through an external linear speed source (e.g., a log designed to measure the ship’s speed). The possibility of using the proposed scheme to estimate the trajectory of dynamic objects seems scarcely probable for the author [23].

EKF is the most prevalent method for position estimation through sensor fusion although its convergence is not assured [24]. Issues of integrating data from IMU and encoders using the extended Kalman filter are presented in the paper [25]. Another common solution for localization is the sigma point approach which is the UKF algorithm [26,27]. UKF overcomes EKF linearization errors using a deterministic sampling approach by representing the state distribution as a set of sample points. Consequently, it achieves better accuracy [28]. However, study [29] is based on real data and shows that the performances of the EKF and UKF are substantially identical.

Indirect formulation of the Kalman filter, error-state extended Kalman filter (ES EKF) for attitude estimation is given in [30]. Several forms of ES EKF with GNSS and WiFi data have been presented recently for outdoor [31] and indoor applications [32]. The study [33] focuses on the ES EKF algorithm for orientation estimation by means of IMU and magnetometer data. The proposed method provides accurate results even when magnetic fluctuations occur. In [34], the authors combine ES EKF with the smooth variable structure filter for attitude estimation based on IMU observations. Attitude estimation of IMU under dynamic conditions using ES EKF is presented in [35]. Nevertheless, the amount of work aimed at ES EKF implementation for indoor environments is relatively small compared to EKF localization although it has significant potential. Low-cost indoor solutions based on IMU, wheel encoders, and magnetometer observations are studied in [36,37].

A comprehensive description of quaternion kinematics for the error-state Kalman filter is provided by [24]. Its findings served as a reference point in the formulation of the proposed approach. Previously, authors implemented a system for integrating data from IMU and encoders for a robotic platform using EKF and assessed linear and angular trajectory displacements [38]. The main contributions of this paper are as follows. The ES EKF is adapted to provide localization in a GNSS-denied environment by means of IMU, encoders, and magnetometer data fusion. In addition, tightly-coupled integration of encoders and magnetometer data is introduced to calculate the errors between prediction and measurements. An extensive description of the algorithm is also provided. Furthermore, the approach is verified through Gazebo simulation by obtaining trajectories from the ES EKF, EKF, and UKF localization algorithms based on the same sensors data.

2. Simulation Methodology

2.1. Reference Frame Decomposition

Before localization algorithm designing, the correlative coordinate frames should be described. According to Figure 1, there are four main coordinate frames which are commonly used for localisation purposes:

• Body frame $\mathit{B}$,
• Local frame $\mathit{L}$,
• Earth-North-UP frame $\mathit{ENU}$,
• Earth-Centered, Earth-Fixed frame $\mathit{ECEF}$

$\mathit{ECEF}$ is a cartesian coordinate frame with the origin at the center of the Earth’s mass. The X-axis in this frame represents the intersection of the Earth’s equator at zero degrees latitude and longitude accordingly. The Z-axis is orthogonal to X and points to the north pole. The Y-axis is orthogonal to X and the Z-axis is according to the right-hand rule. $\mathit{ENU}$ is a coordinate system with the origin at a specified point from the $\mathit{ECEF}$ frame, where the X and Y axes point to geodesic east and north respectively. The Z-axis faces upward according to the right-hand rule. $\mathit{L}$ is a local frame of reference with an origin at the starting point of system movement. $\mathit{B}$ is the local coordinate system fixed to the vehicle’s center of gravity. Therefore, all measurements from sensors, which are mounted on the robotic platform, must be transformed into the body frame. Then, to obtain sensor data in $\mathit{L}$, $\mathit{ENU}$, $\mathit{ECEF}$ reference frames, it must be converted by transformation matrices ${\mathit{T}}_B^L$, ${\mathit{T}}_L^{ENU}$,${\mathit{T}}_{ENU}^{ECEF}$ which are explicitly presented in [39,40].

Figure 1. Coordinate frames transformation.

Figure 1. Coordinate frames transformation.

As a matter of convenience in our system, the IMU is firmly attached to the platform and its coordinate frame is aligned to the body frame B, consequently, they have the same orientation and origin and are considered as ENU convention. Data from encoders are aligned to the body frame origin as well. Therefore, in the considered system frame $\mathit{L}$ is equal to $\mathit{ENU}$ frame.

2.2. ES EKF with IMU and Encoder Data Fusion

The main concept of the Error-state Extended Kalman filter (ES EKF) consists of the separation of the state vector into two parts: nominal state $\mathit{x}$ and error state $\delta \mathit{x}$.

$${\mathit{x}}_t=\mathit{x}+\delta \mathit{x}$$

2.3. Nominal State

The nominal state is represented by a motion model and in most cases composed of IMU observations. Errors related to process noise accumulation and motion model imperfections constitute the error state vector. Therefore, the nominal state vector has the following components:

$$\mathit{x}={\left[\begin{array}{ccc}\mathit{p}& \mathit{v}& \mathit{q}\end{array}\right]}^T,$$

where $\mathit{p}$ = $\left[\begin{array}{ccc}{p}_x& p_y& p_z\end{array}\right]$—represents position of the platform; $\mathit{v}=\left[\begin{array}{ccc}{v}_x& v_y& v_z\end{array}\right]$—platform’s velocity; $\mathit{q}=\left[\begin{array}{cccc}{q}_w& q_x& q_y& q_z\end{array}\right]$—orientation in the form of quaternion. They can be obtained with high-frequency IMU data. The IMU contains a collection of three-axis accelerometers and three-axis gyroscopes whose output data is linear acceleration $\mathit{a}=\left[\begin{array}{ccc}{a}_x& a_y& a_z\end{array}\right]$ and angular velocities $\mathbf{\omega }=\left[\begin{array}{ccc}{\omega }_x& {\omega }_y& {\omega }_z\end{array}\right]$ accordingly. Integration of the linear acceleration yields the velocity of the platform and angular velocity represents the orientation. With the known initial values of velocity, position, and orientation, the coordinates and attitude of the platform can be evaluated at any time. However, integration of the biases and sensor noise inherent to IMU measurements leads to the unlimited growth of errors with time [41] and as a consequence of this fact, position and orientation estimates are not precise. Hence, applying only mechanization equations does not guarantee satisfactory results in terms of accuracy.

The continuous motion model based on IMU observations, according to [24] is described by the following equations:

$$\dot{\mathit{p}}=\mathit{v},\dot{\mathit{v}}=\mathit{a},\dot{\mathit{q}}=\frac{1}{2}\mathit{q}\otimes \mathbf{\omega },$$

(1)

where ⊗—quaternion product operator.

Because of IMU sensor signal frequency limitations, Equation (1) need to be discretized according to the transmitting time interval $\Delta t$. Therefore, the discrete-time motion model can be defined by the equations:

$$\begin{array}{c}{\mathit{p}}_k={\mathit{p}}_{k-1}+{\mathit{v}}_{k-1}·\Delta t+(\mathit{R}\left\{{\mathit{q}}_{k-1}\right\}·{\mathit{a}}_k+\mathit{g})·\frac{\Delta t^2}{2},\\ {\mathit{v}}_k={\mathit{v}}_{k-1}+(\mathit{R}\left\{{\mathit{q}}_{k-1}\right\}·{\mathit{a}}_k+\mathit{g})·\Delta t,\\ {\mathit{q}}_k={\mathit{q}}_{k-1}\otimes \mathit{q}({\omega }_k·\Delta t),\end{array}$$

(2)

where k, $k-1$—indexes of current and previous sample step; $\mathit{g}$ = $\left[\begin{array}{ccc}0& 0& -9.81\end{array}\right]$—gravity vector; $\mathit{R}\left\{\mathit{q}\right\}$—quaternion to rotation matrix conversion which can be defined as:

$$\mathbf{R}=\left[\begin{array}{ccc}{q}_w^2+q_x^2-q_y^2-q_z^2& 2(q_x{q}_y-q_w{q}_z)& 2(q_x{q}_z+q_w{q}_y)\\ 2(q_x{q}_y+q_w{q}_z)& q_w^2-q_x^2+q_y^2-q_z^2& 2(q_y{q}_z-q_w{q}_x)\\ 2(q_x{q}_z-q_w{q}_y)& 2(q_y{q}_z+q_w{q}_x)& q_w^2-q_x^2-q_y^2+q_z^2\end{array}\right].$$

2.4. Error State

The error state kinematic equations are considered linear due to small magnitudes of error dynamics. Error state vector consists of the following vectors:

$$\delta \mathit{x}={\left[\begin{array}{ccc}\delta \mathit{p}& \delta \mathit{v}& \delta \mathbf{\theta }\end{array}\right]}^T,$$

where $\delta \mathit{p}$ = $\left[\begin{array}{ccc}\delta p_x& \delta p_y& \delta p_z\end{array}\right]$—represents the position error of the platform; $\delta \mathit{v}=\left[\begin{array}{ccc}\delta v_x& \delta v_y& \delta v_z\end{array}\right]$—velocity error of the platform; $\delta \mathbf{\theta }=\left[\begin{array}{ccc}\delta {\theta }_x& \delta {\theta }_y& \delta {\theta }_z\end{array}\right]$—angular error of the platform. Conversion between quaternion and angular representations is obtained by the following equations [42]:

$$\mathit{q}\left\{\mathbf{\theta }\right\}=\left[\begin{array}{c}cos(\varphi /2)·cos(\theta /2)·cos(\psi /2)+sin(\varphi /2)·sin(\theta /2)·sin(\psi /2)\\ sin(\varphi /2)·cos(\theta /2)·cos(\psi /2)-cos(\varphi /2)·sin(\theta /2)·sin(\psi /2)\\ cos(\varphi /2)·sin(\theta /2)·cos(\psi /2)+sin(\varphi /2)·cos(\theta /2)·sin(\psi /2)\\ cos(\varphi /2)·cos(\theta /2)·sin(\psi /2)-sin(\varphi /2)·sin(\theta /2)·cos(\psi /2)\end{array}\right],$$

$$\mathbf{\theta }\left\{\mathit{q}\right\}=\left[\begin{array}{c}arctan\left(\frac{2·(q_w·q_x+q_y·q_z)}{1-2·(q_x^2+q_y^2)}\right)\\ arcsin(2·(q_w·q_y-q_z·q_x))\\ arctan\left(\frac{2·(q_w·q_z+q_x·q_y)}{1-2·(q_y^2+q_z^2)}\right)\end{array}\right].$$

(3)

The simplified version of the IMU error state equations with excluded sensor biases in the continuous state is represented by

$$\delta \dot{\mathit{p}}=\delta \mathit{v},\delta \dot{\mathit{v}}=-\mathit{R}{\left[\mathit{a}\right]}_{\times }\delta \mathbf{\theta },\delta \dot{\mathbf{\theta }}=-{\left[\mathbf{\omega }\right]}_{\times }\delta \mathbf{\theta },$$

(4)

where ${\left[\mathit{a}\right]}_{\times }$—skew symmetric matrix operator, which can be defined by

$${\left[\mathit{a}\right]}_{\times }=\left[\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right].$$

Error state kinematics equations in discrete time are as follows

$$\begin{array}{c}\delta \mathit{p}=\delta {\mathit{p}}_{k-1}+\delta {\mathit{v}}_{k-1}·\Delta t\\ \delta \mathit{v}=\delta {\mathit{v}}_{k-1}+(-\mathit{R}\left\{{\mathit{q}}_{k-1}\right\}·{\left[{\mathit{a}}_{k-1}\right]}_{\times }·\delta {\mathbf{\theta }}_{k-1})·\Delta t\\ \delta \dot{\mathbf{\theta }}=\mathit{R}{\{{\mathbf{\omega }}_{k-1}·\Delta t\}}^T·\delta {\mathbf{\theta }}_{k-1}\end{array}$$

(5)

The ES EKF, like the classic Kalman filter, includes two recursive steps. The first one considers the propagation of the state vector and covariance (prediction step), and the second carries out correction or update. It is important to point out that EKF generates an output state only after measurement update step [43], while ES EKF output state vector can be obtained with prediction data. It increases the reliability of the system in case of additional sensor failure.

2.5. ES EKF Prediction Step

Propagation deals with the state vector implementing mechanization equations mentioned above (2) which should be rewritten as follows:

$$\begin{array}{c}{\widehat{\mathit{p}}}_k^{-}={\widehat{\mathit{p}}}_{k-1}^{+}+{\widehat{\mathit{v}}}_{k-1}^{+}·\Delta t+(\mathit{R}\left\{{\widehat{\mathit{q}}}_{k-1}^{+}\right\}·{\mathit{a}}_k+\mathit{g})·\frac{\Delta t^2}{2},\\ {\widehat{\mathit{v}}}_k^{-}={\widehat{\mathit{v}}}_{k-1}^{+}+(\mathit{R}\left\{{\widehat{\mathit{q}}}_{k-1}^{+}\right\}·{\mathit{a}}_k+\mathit{g})·\Delta t,\\ {\widehat{\mathit{q}}}_k^{-}={\widehat{\mathit{q}}}_{k-1}^{+}\otimes \mathit{q}({\mathbf{\omega }}_k·\Delta t),\end{array}$$

(6)

where ${\widehat{\mathit{x}}}^{-}$—a priori and ${\widehat{\mathit{x}}}^{+}$— posteriori state estimation for each vactor accordingly.

Likewise, the error covariance matrix of the state estimate ${\mathit{P}}^{-}$ is computed at that stage. Unlike the standard variations of KF, provided that additional sensor data is available, the ES EKF algorithm performs a correction step, otherwise—it continues propagation with time.

The error covariance matrix of the state estimate can be obtained with the following equations:

$${\mathit{P}}_k^{-}={\mathit{F}}_x·{\mathit{P}}_{k-1}^{+}·{\mathit{F}}_x^T+{\mathit{F}}_i·{\mathit{Q}}_i·{\mathit{F}}_i^T,$$

(7)

where ${\mathit{F}}_x$—the transition matrix which can be obtained as Jacobian of error state kinematic Equation (5); ${\mathit{F}}_i$— the perturbation vector Jacobian matrix; ${\mathit{Q}}_i$—measurement covariance matrix for the prediction step; ${\mathit{P}}_{k-1}^{+}$—posteriori error covariance matrix of the state estimate at the previous step.

In the first step matrix ${\mathit{P}}_k^{-}$ is defined as a unit matrix multiplied by the initial covariance value, for example:

$${\mathit{P}}_k^{-}={\mathit{P}}_0={\mathit{I}}_{9\times 9}·0.01$$

The corresponding error state transition matrix ${\mathit{F}}_x$ is derived via the Taylor series block-wise truncation as follows:

$${\mathit{F}}_x=\left[\begin{array}{ccc}{\mathbf{I}}_{3\times 3}& {\mathbf{I}}_{3\times 3}·\Delta t& -0.5·\mathit{R}\left\{{\widehat{\mathit{q}}}_{k-1}^{+}\right\}·{\left[{\mathit{a}}_k\right]}_{\times }·\Delta t^2\\ 0& {\mathbf{I}}_{3\times 3}& -{[\mathit{R}\left\{{\widehat{\mathit{q}}}_k^{+}\right\}·{\mathit{a}}_{k-1}]}_{\times }·\Delta t\\ 0& 0& \mathit{R}\left\{{\widehat{\mathit{q}}}_{k-1}^{+}\right\}·{(\omega ·\Delta t)}^T\end{array}\right]$$

(8)

Perturbation ${\mathit{F}}_i$ matrix is defined by

$${\mathit{F}}_i=\left[\begin{array}{cc}{\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}\end{array}\right]$$

(9)

The error covariance matrix can be obtained as a multiplication of the unit matrix ${\mathit{I}}_{6\times 6}$ multiplied by integrated covariances of linear acceleration ${\sigma }_{a_x}^2,{\sigma }_{a_y}^2,{\sigma }_{a_z}^2$ and angular velocities ${\sigma }_{{\omega }_x}^2,{\sigma }_{{\omega }_y}^2,{\sigma }_{{\omega }_z}^2$ accordingly:

$${\mathit{Q}}_i=Diag\left(\begin{array}{cccccc}{\sigma }_{a_x}^2·\Delta t^2,& {\sigma }_{a_y}^2·\Delta t^2,& {\sigma }_{a_z}^2·\Delta t^2,& {\sigma }_{{\omega }_x}^2·\Delta t^2,& {\sigma }_{{\omega }_y}^2·\Delta t^2,& {\sigma }_{{\omega }_z}^2·\Delta t^2\end{array}\right)$$

(10)

2.6. ES EKF Measurement Update Step

Correction implies the calculation of the error state vector via Kalman gain and innovation computation along with process covariance updating. Instead of using IMU observations as measurements for innovation calculation, additional sensor data is applied. Thereby, ES EKF innovation is the discrepancy between the prediction based on IMU equations and measurements from the additional sensor.

This paper represents a measurement update by fused wheel encoders and magnetometer data, described in Section 3.1. Therefore, the measurement output is defined as

$${\mathit{y}}_k={\left[\begin{array}{ccccccccc}0& 0& 0& v_x& v_y& 0& {\theta }_x& {\theta }_y& {\theta }_z\end{array}\right]}^T$$

(11)

Depending on the availability of measurements from additional sensors, the algorithm performs a correction step or continues updating the nominal state vector based only on the IMU data. If measurements are available, the gain matrix is calculated according to the equation:

$${\mathit{K}}_k={\mathit{P}}_k^{-}·{\mathit{H}}_k^T·{({\mathit{H}}_k·{\mathit{P}}_k^{-}·{\mathit{H}}_k^T+\mathit{R})}^{-1},$$

(12)

where ${\mathit{P}}_k^{-}$—state vector covariance matrix, defined by (7); $\mathit{R}$—measurement noise covariance matrices; $\mathit{H}$—observation matrix. Measurement noise covariance matrices can be defined as

$$\mathit{R}=Diag\left(\begin{array}{ccc}{\mathbf{\sigma }}_{\mathit{p}}^2,& {\mathbf{\sigma }}_{\mathit{v}}^2,& {\mathbf{\sigma }}_{\mathbf{\theta }}^2\end{array}\right),$$

(13)

where ${\mathbf{\sigma }}_{\mathit{p}}^2,{\mathbf{\sigma }}_{\mathit{v}}^2,{\mathbf{\sigma }}_{\mathbf{\theta }}^2$ —measurement dispersion matrices for the position, velocity, and orientation in coordinates $x,y,z$.

The observation matrix demonstrates what types of measurements are taken into account when calculating innovation. If the state of a system is directly measurable then the H matrix for the according element is 1. As the measurement matrix (11) consists of $u_x,u_y,{\theta }_x,{\theta }_y,{\theta }_z$ elements, $\mathit{H}$ can be defined as follows:

$$\mathit{H}=Diag\left(\begin{array}{ccccccccc}0,& 0,& 0,& 1,& 1,& 0,& 1,& 1,& 1\end{array}\right)$$

(14)

The updated error state vector is a $1\times 9$ matrix computed using the difference between additional sensor data and predicted nominal state (that is innovation) and Kalman gain

$$\delta {{\widehat{\mathit{x}}}_k}^{+}={\mathit{K}}_k({\mathit{y}}_k-{\widehat{\mathit{x}}}_k^{-})={\left[\begin{array}{ccc}\delta {\widehat{\mathit{p}}}_k^{+}& \delta {\widehat{\mathit{v}}}_k^{+}& \delta {\widehat{\mathbf{\theta }}}_k^{+}\end{array}\right]}^T$$

(15)

where ${\widehat{\mathit{x}}}_k^{-}$—a priori vector of the estimated state which can be found as

$${\widehat{\mathit{x}}}_k^{-}={\left[\begin{array}{ccc}{\widehat{\mathit{p}}}_k^{-}& {\widehat{\mathit{v}}}_k^{-}& \mathbf{\theta }\left\{{\widehat{\mathit{q}}}_{\mathit{k}}^{-}\right\}\end{array}\right]}^T$$

Then on the grounds of error state vector total state is corrected

$$\begin{array}{c}{\widehat{\mathit{p}}}_k^{+}={\widehat{\mathit{p}}}_k^{-}+\delta {\widehat{\mathit{p}}}_k^{+}\\ {\widehat{\mathit{v}}}_k^{+}={\widehat{\mathit{v}}}_k^{-}+\delta {\widehat{\mathit{v}}}_k^{+}\\ {\widehat{\mathit{q}}}_k^{+}={\widehat{\mathit{q}}}_k^{-}\otimes \mathit{q}\left\{\delta {\widehat{\mathbf{\theta }}}_{\mathit{k}}^{+}\right\}\end{array}$$

(16)

The Joseph formula is used for the covariance update to provide a symmetric and positive matrix.

$${{\mathit{P}}_k}^{+}=(\mathit{I}-{\mathit{K}}_k·{\mathit{H}}_k)·{\mathit{P}}_k^{-}·{(\mathit{I}-{\mathit{K}}_k·{\mathit{H}}_k)}^T+{\mathit{K}}_k·\mathit{R}·{\mathit{K}}_k^T$$

(17)

Thus, the ES EKF itself estimates only errors by taking into account supplementary sensor data, e.g., LiDARs, cameras, or wheel encoders. In this way, by means of an estimated error state, the total state is improved.

2.7. ES EKF Error Reset

After vector state updating performed by (16) as well as the error covariance matrix of the state estimation (17), the error should be set to zero:

$$\left[\begin{array}{ccc}\delta {\widehat{\mathit{p}}}_k^{+}& \delta {\widehat{\mathit{v}}}_k^{+}& \delta {\widehat{\mathit{q}}}_k^{+}\end{array}\right]=\mathbf{0}$$

(18)

Error covariance matrix ${{\mathit{P}}_k}^{+}$ also should be reset according to the following equation:

$${{\mathit{P}}_k}^{+}=\mathit{G}·{{\mathit{P}}_k}^{+}·{\mathit{G}}^T,$$

(19)

where $\mathit{G}$ is the Jacobian matrix which can be found by

$$\mathit{G}=\left[\begin{array}{ccc}\mathit{I}& 0& 0\\ 0& \mathit{I}& 0\\ 0& 0& \mathit{I}-{\left[\frac{1}{2}\delta {\widehat{\mathbf{\theta }}}_{\mathit{k}}^{+}\right]}_{\times }\end{array}\right],$$

(20)

To structurize the error state extended Kalman filter algorithm, a combined block diagram is shown in Figure 2.

According to Figure 2, the ES EKF algorithm first got measurements from the accelerometer and magnetometer. Then, if it is a first measurement, the initial values of ${\mathit{P}}_0,{\mathit{p}}_0,{\mathit{v}}_0$, and ${\mathit{q}}_0$ should be set, as well as matrices ${\mathit{F}}_x,{\mathit{F}}_i,{\mathit{Q}}_i$ initialized. After parameter initialization, the prediction step should be performed by computing ${\widehat{\mathit{p}}}_k^{-},{\widehat{\mathit{v}}}_k^{-},{\widehat{\mathit{q}}}_k^{-},$ and ${\mathit{P}}^{-}$. Then, if the measurements of wheel encoders $\mathit{v}$ and magnetometer $\mathit{q}$ are available, the error calculation block computes $\delta {\widehat{\mathit{p}}}_k^{+},\delta {\widehat{\mathit{v}}}_k^{+},\delta {\widehat{\mathit{q}}}_k^{+}$, and ${\mathit{P}}_k^{+},{\mathit{G}}_k^{+}$. At the final stage, the error values of position, velocity, and orientation are injected into the nominal state and go to <<Output block>> which provides an error reset for ${\mathit{P}}_k^{+}$ and sends updated nominal values back to the prediction block.

The main advantage of that approach is the robustness as the filter’s malfunction does not influence the system algorithm. In the case of filter failure, position and orientation estimation will be obtained by motion model propagation.

3. Simulation Results

All simulations are performed by a Robot operating system (ROS) package [44], with Gazebo simulation software [45]. A four-wheeled differential drive robot <<Jackal>> was chosen as the main platform with an onboard mounted IMU sensor and wheel encoders. The simulated robotic platform has the following specifications, presented in

Table 1. <<Jackal>> robot platform specifications.

Context	Item	Value	Unit
Platform wheelbase	L	$0.262$	m
Robot speed limit	$v_{max}$	$0.62$	m/s
Linear acceleration RMSE	${\mathbf{\sigma }}_{\mathit{a}}=\left[\begin{array}{ccc}{\sigma }_{a_x}& {\sigma }_{a_y}& {\sigma }_{a_z}\end{array}\right]$	$\left[\begin{array}{ccc}0.005& 0.005& 0.005\end{array}\right]$	m/s${}^2$
Angular velocity RMSE	${\mathbf{\sigma }}_{\mathbf{\omega }}=\left[\begin{array}{ccc}{\sigma }_{{\omega }_x}& {\sigma }_{{\omega }_y}& {\sigma }_{{\omega }_z}\end{array}\right]$	$\left[\begin{array}{ccc}0.005& 0.005& 0.005\end{array}\right]$	rad/s
Magnetometer RMSE	${\mathbf{\sigma }}_{\omega }=\left[\begin{array}{ccc}{\sigma }_{{\omega }_x}& {\sigma }_{{\omega }_y}& {\sigma }_{{\omega }_z}\end{array}\right]$	$\left[\begin{array}{ccc}0.005& 0.005& 0.005\end{array}\right]$	$\mathsf{\mu }$T${}^2$
Platform speed RMSE	${\mathbf{\sigma }}_{\mathit{v}}=\left[\begin{array}{ccc}{\sigma }_{v_x}& {\sigma }_{v_y}& {\sigma }_{v_z}\end{array}\right]$	$\left[\begin{array}{ccc}0.001& 0.001& 10000\end{array}\right]$	m/s

.

The platform speed RMSE in z reference frame was set high because of a lack of measurements.

3.1. Magnetometer and Odometer Sensor Fusion

Vector ${\mathit{y}}_k$ from Equation (11) is composed of the wheel encoders and magnetometer measurements. Since the ROS framework has a specific sensor data transmission format, odometry data from the wheel encoders are sent as linear v and angular $\omega$ speed. Therefore, the $v_x,v_y$ values can be obtained by the following equations:

$$v_x=\frac{v_L+v_R}{2·cos\left({\theta }_m\right)},v_y=\frac{v_L+v_R}{2·sin\left({\theta }_m\right)},$$

(21)

where $v_L,v_R$—left and right wheel speed which can be calculated in accordance with the formulas (22); ${\theta }_m$—orientation angle on z reference frame provided from magnetometer data after conversion source orientation vector from quaternion to Euler’s angle according to (3).

$$v_L=v-\frac{\omega ·L}{2},v_R=v+\frac{\omega ·L}{2}$$

(22)

where L—the distance between the left and right wheels.

Therefore, the measurement vector state (11) consists of the velocities $v_x$ and $v_y$ obtained by encoder and magnetometer data fusion (21) as well as magnetometer orientation data ${\theta }_x$, ${\theta }_y$, ${\theta }_z$.

4. Discussion

The algorithm test is performed in a simulated underground mine environment. On the account of the simulation, two trajectories are derived: a reference trajectory and a trajectory based on sensor fusion data (Figure 3). Using the evo package [46], differences between corresponding points of trajectories are calculated. Achieved results were compared with EKF and UKF by using a “robot localization” ROS package [47]. The simulation results are presented in

Table 2. Statistics of calculated differences.

Error	ES EKF	EKF	UKF
RMSE	0.517	0.620	0.516
Mean	0.469	0.543	0.475
Median	0.570	0.679	0.512
Min	0.104	0.095	0.163
Max	0.752	0.929	0.827

. According to Table 2, the mean error values for the ES EKF and UKF are almost the same (0.517 cm and 0.516 cm, accordingly), while the EKF is 0.13 cm worse. The maximum values are also close to each other with values 0.752, 0.929, and 0.827 for the ES EKF, EKF and UKF accordingly. The trajectory built using the proposed algorithm has similar accuracy to the trajectories obtained by the Extended Kalman Filter and the Unscented Kalman Filter. However, all three algorithms need to increase the accuracy of localization.

5. Conclusions

In this paper, the ES EKF algorithm is proposed for real-time localization in GNSS-denied environments. To correct the vector state calculated through motion model equations based on the IMU observations, the heading angle measured by magnetometer and the speed determined by means of encoder data were added. The proposed system is validated through a Gazebo simulation. The algorithm minimizes IMU drift by means of additional sensor data and increases the accuracy of positioning.

The algorithm achieves accuracy comparable to the results of the classic Extended Kalman Filter and Unscented Kalman Filter. At the same time, the proposed system is more resistant to failures of the auxiliary sensors. In the future, 3D LiDAR data will be integrated into the system to increase localization accuracy.