Sensor Fusion of GNSS and IMU Data for Robust Localization via Smoothed Error State Kalman Filter

Abstract

High−precision and robust localization is critical for intelligent vehicle and transportation systems, while the sensor signal loss or variance could dramatically affect the localization performance. The vehicle localization problem in an environment with Global Navigation Satellite System (GNSS) signal errors is investigated in this study. The error state Kalman filtering (ESKF) and Rauch–Tung–Striebel (RTS) smoother are integrated using the data from Inertial Measurement Unit (IMU) and GNSS sensors. A segmented RTS smoothing algorithm is proposed in order to estimate the error state, which is typically close to zero and mostly linear, which allows more accurate linearization and improved state estimation accuracy. The proposed algorithm is evaluated using simulated GNSS signals with and without signal errors. The simulation results demonstrate its superior accuracy and stability for state estimation. The designed ESKF algorithm yielded an approximate 3% improvement in long straight line and turning scenarios compared to classical EKF algorithm. Additionally, the ESKF−RTS algorithm exhibited a 10% increase in the localization accuracy compared to the ESKF algorithm. In the double turning scenarios, the ESKF algorithm resulted in an improvement of about 50% in comparison to the EKF algorithm, while the ESKF−RTS algorithm improved by about 50% compared to the ESKF algorithm. These results indicated that the proposed ESKF−RTS algorithm is more robust and provides more accurate localization.

1. Introduction

The Global Navigation Satellite System (GNSS) has found widespread usage in vehicle measurement, intelligent driving, and robot navigation due to its stable long−term localization performance [1,2,3]. However, the susceptibility of satellite signals to external interference would lead to the GNSS having unreliable localization accuracy. Therefore, the combination of GNSS and Inertial Navigation System (INS) has been widely employed, in which both systems could compensate for each other’s drawbacks and leverage their respective strengths to achieve continuous localization [4,5,6]. The reported studies have investigated the combined filtering algorithm of GNSS and INS, GNSS/IMU can provide position, velocity, and attitude information for vehicle control. Liu [7,8] presents a novel pitch and roll feedback mechanism that utilizes intrinsic information of the vehicle such as steering angle and wheel speed. To compensate for the cumulative velocity errors that occur during low sampling intervals of GNSS, the integration of reverse smoothing and grey prediction is employed. Shin and Naser [9] reported that the algorithm yielded better navigation and localization results, even in cases of short−term GNSS loss. Han [10] compared the combined satellite/inertial guidance parameter estimation results with those of a single satellite system, with the former yielding superior results. Li and Zhang [11] studied the combined GNSS/INS navigation algorithm and highlighted that the filtering algorithm still struggled to provide accurate parameter estimation in situations where few satellite data were available. Erfianti [12] researched a combined GNSS/INS navigation algorithm and discovered that the INS could only provide high−accuracy parameter estimation results for a short duration. During the satellite signal occlusion, the forward filtering algorithm alone thus cannot guarantee a robust localization.

Applying the extended Kalman filter (EKF) to estimate the motion of vehicle systems is well desirable due to the system nonlinearity [13,14,15,16]. The EKF linearizes the nonlinear model by approximating it with a first−order Taylor series around the state estimate and then estimates the state using the Kalman filter. M. M. Atia [17] proposed utilizing the extended Kalman filter to merge data from inertial sensors with GNSS data in a loosely coupled mode to improve the accuracy of road network maps in determining lanes. The resulting lane determination success rate was an impressive 97.14%. However, the local linearization operation of this approach can introduce significant estimation errors. Julier [18] introduced an Unscented Kalman filter algorithm that improves the estimation results, while its high complexity makes it unsuitable for most inertial guidance devices. Arasaratnam [19] et al. pointed out through theoretical analysis and simulation verification that the UKF has poor filtering performance or even divergence in solving the high−dimensional (≥20) nonlinear state estimation problem. For this reason, they used the spherical radial rule to approximate the state posterior distribution in the optimal framework, and then proposed the Cubature Kalman Filter (CKF), but the CKF has higher computational complexity and requires more sampling and operations, resulting in poor real−time performance, which is not suitable for some applications with high real−time performance requirements. The extended Kalman filter thus remains the mainstream state estimation algorithm, and developing a low−complexity filter with high accuracy is still challenging [20,21].

To address this challenge, S. S Kourabbaslou [22] presents a flexible design framework utilizing symbolic engines to represent and linearize system and measurement models. A robust fixed−lag smoothing approach is proposed in case there is a mismatch between the nominal model and the actual model [23,24]. To improve the accuracy of vehicle stand−alone localization in highly dynamic driving conditions during GNSS outages, Gao [25] proposed a vehicle localization system based on vehicle chassis sensors considering vehicle lateral velocity. The Kalman filter combines vehicle states obtained from vehicle kinematics and dynamics to improve the reliability and accuracy of autonomous driving. A consensus−based and vehicle kinematics/dynamics integrated autonomous vehicle sideslip angle estimation algorithm based on GNSS/INS was proposed [26,27]. Madyastha [28] proposed a Kalman filtering method based on attitude error states. The error state Kalman filter (ESKF) is designed for covariance estimation, which serves as a weight for the inertial odometry optimization process [29,30,31]. These results validate the ESKF’s effectiveness for error−state estimation, although a full−state estimation based on ESKF is not available yet. Further exploration of state estimation based on ESKF is thus favorable.

The major challenge in designing a sensor fusion algorithm for state estimation is to address GNSS signal errors and low−cost hardware limitations with a smoother approach. The errors of inertial navigation systems accumulate over time, while GNSS signals are heavily influenced by satellite signal quality and urban obstructions [32,33,34]. These would lead to significant localization errors. Using INS alone cannot provide accurate position estimation for a long term in signal−obscured environments [35,36,37]. In such cases, real−time high−accuracy localization is necessary, and data fusion from multiple sensors is often required [38,39]. In order to provide reliable localization information, simultaneous localization and map construction (SLAM) systems can be used, as well as multi−sensor integrated systems that combine LiDAR [40] and navigation systems to solve problems caused by GNSS antenna failures. To reduce GNSS and Inertial Measurement Unit (IMU) fusion costs and achieve desired accuracy, different navigation satellite [41,42,43] constellations need to be combined, especially in urban working environments.

In this paper, the low−cost sensor combination of only GNSS and IMU is considered, and effects of GNSS signal error and different fusion algorithm design are investigated. The extended Kalman filter and error state Kalman filter are designed in Section 2. The application of the error−extended Kalman filter and the RTS optimal smoothing algorithm is investigated in Section 3 for combined inertial guidance and GNSS localization in urban working environments. The insights and conclusions on the performance of the designed sensor fusion algorithms are further analyzed.

2. GNSS and IMU Integrated Filter Design

The overall sensor fusion framework integrating the GNSS and IMU sensor data with significant GNSS signal errors is illustrated in Figure 1. It mainly consists of four procedures, including data analysis, prediction process, update process and reverse smoothing, contributing to the developed ESKF−RTS smoothing localization algorithm. In particular, the algorithm defines a nominal state without considering the measurement noise of IMU and system disturbance, and an error state containing the noise and disturbance information is used for state estimation. The nominal state condition and error state prediction are updated simultaneously, and the error state is corrected using GNSS signal measurement and injected into the nominal state. The error state and its covariance matrix are subsequently reset. The acquisition frequency for GNSS data is 1 Hz, while the IMU data are acquired at a frequency of 100 Hz. The reverse time update and reverse segmentation smoothing are parallelly performed, which will be detailed below.

2.1. Extended Kalman Filter

The extended Kalman filter (EKF) is also designed for comparison, using the sensor measurements directly. The EKF procedure is described briefly.

The developed discrete nonlinear vehicle system equations are:

$$\{\begin{array}{l}{\mathit{X}}_k=f({\mathit{X}}_{k-1})+{\mathit{W}}_{k-1}\\ {\mathit{Z}}_K=h({\mathit{X}}_k)+{\mathit{V}}_k\end{array},$$

(1)

where $f$(·) and $h$(·) represent the state function and measurement function of nonlinear vehicle systems, $\mathit{W}$ and $\mathit{V}$ are Gaussian white noise of state $\mathit{X}$ and measurement $\mathit{Z}$, respectively, and $k$ represents the discrete time step.

The state function of the nonlinear vehicle system in Equation (1) is approximated through local linearization. The nonlinear equation is expanded using a Taylor series, and only the first−order term is retained while ignoring the second and higher−order terms. This yields the following expression:

$${\mathit{X}}_k=f({\stackrel{⌣}{\mathit{X}}}_{k-1})+{\mathit{F}}_{k-1}({\mathit{X}}_{k-1}-{\stackrel{⌣}{\mathit{X}}}_{k-1})+{\mathit{W}}_{k-1},$$

(2)

where ${\mathit{F}}_{k-1}$ is the Jacobian matrix of the function of the function $f({\mathit{X}}_{k-1})$.

The statistical characteristics of the predicted state can be obtained based on the results of the linearization in the form of

$${\stackrel{⌢}{\mathit{X}}}_{k,k-1}=E[f({\stackrel{⌣}{\mathit{X}}}_{k-1})+{\mathit{F}}_{k-1}({\mathit{X}}_{k-1}-{\stackrel{⌣}{\mathit{X}}}_{k-1})+{\mathit{W}}_{k-1}]=f({\stackrel{⌣}{\mathit{X}}}_{k-1}).$$

(3)

It is possible to further predict the variance matrix as

$$\begin{array}{ll}{\mathit{P}}_{k,k-1}& =E[({\mathit{X}}_k-{\stackrel{⌣}{\mathit{X}}}_{k,k-1}){({\mathit{X}}_k-{\stackrel{⌣}{\mathit{X}}}_{k,k-1})}^T]\\ & =E\left\{[{\mathit{F}}_{k-1}({\mathit{X}}_{k-1}-{\stackrel{⌣}{\mathit{X}}}_{k-1})+{\mathit{W}}_{k-1}]{[{\mathit{F}}_{k-1}({\mathit{X}}_{k-1}-{\stackrel{⌣}{\mathit{X}}}_{k-1})+{\mathit{W}}_{k-1}]}^T\right\}.\\ & ={\mathit{F}}_{k-1}{\mathit{P}}_{k-1}{\mathit{F}}_{{}_{k-1}}^T+{\mathit{Q}}_{k-1}\end{array}$$

(4)

The same method is used for the measurement function $h\left({\mathit{x}}_k\right)$ at the point ${\widehat{\mathit{x}}}_k$, which can be obtained as

$${\mathit{Z}}_K=h({\stackrel{⌣}{\mathit{X}}}_k)+{\mathit{H}}_k({\mathit{X}}_k-{\stackrel{⌣}{\mathit{X}}}_k)+{\mathit{V}}_k,$$

(5)

where ${\mathit{H}}_k$ is the Jacobian matrix of the function of the function $h({\mathit{X}}_{k-1})$. The prediction of measurement is calculated as

$${\stackrel{⌢}{\mathit{Z}}}_{k,k-1}=E[h({\stackrel{⌣}{\mathit{X}}}_{k-1})+H_{k-1}({\mathit{X}}_{k-1}-{\stackrel{⌣}{\mathit{X}}}_{k-1})+{\mathit{V}}_{k-1}]=h({\stackrel{⌣}{\mathit{X}}}_{k-1}).$$

(6)

Then, the collaborative variance can be further predicted as

$$\begin{array}{ll}{\mathit{P}}_{k,k-1}& =E[({\mathit{Z}}_k-{\stackrel{⌣}{\mathit{Z}}}_{k,k-1}){({\mathit{Z}}_k-{\stackrel{⌣}{\mathit{Z}}}_{k,k-1})}^T]\\ & =E\left\{[H_{k-1}({\mathit{X}}_{k-1}-{\stackrel{⌣}{\mathit{X}}}_{k-1})+{\mathit{V}}_{k-1}]{[H_{k-1}({\mathit{X}}_{k-1}-{\stackrel{⌣}{\mathit{X}}}_{k-1})+{\mathit{V}}_{k-1}]}^T\right\}.\\ & ={\mathit{H}}_{k-1}{\mathit{P}}_{k-1}{\mathit{H}}_{k-1}^T+{\mathit{R}}_{k-1}\end{array}$$

(7)

Using the information above, the iterative process of the EKF algorithm for the sensor fused localization can be summarized as

$$\begin{array}{l}{\stackrel{⌣}{\mathit{X}}}_k=f({\stackrel{⌣}{\mathit{X}}}_{k-1})\\ {\stackrel{⌣}{\mathit{P}}}_k={\mathit{F}}_{k-1}{\mathit{P}}_{k-1}{\mathit{F}}_{{}_{k-1}}^T+{\mathit{Q}}_{k-1}\\ {\mathit{K}}_{k+1}={\stackrel{⌣}{\mathit{P}}}_{k+1}^{-}{\mathit{H}}_{k+1}^T{({\mathit{H}}_{k+1}{\stackrel{⌣}{\mathit{P}}}_{k+1}^{-}{\mathit{H}}_{k+1}^T+{\mathit{V}}_{k+1})}^{-1}.\\ {\mathit{X}}_{k+1}={\mathit{K}}_{k+1}({\mathit{Z}}_{k+1}-\mathit{h}({\stackrel{⌣}{\mathit{X}}}_{k+1}))\\ {\mathit{P}}_{k+1}=(\mathit{I}-{\mathit{K}}_{k+1}{\mathit{H}}_{k+1}){\stackrel{⌣}{\mathit{P}}}_{k+1}^{-}\end{array}$$

(8)

2.2. Error State Kalman filter

For the formulation of the error state filter, the system states are defined as the true, nominal, and error state values. The true state is expressed as a combination of the nominal and error states. The approach is to treat the nominal state as a dominant signal, which is highly nonlinear, and the error state as a small signal, which is linearly effective and suitable for linear Gaussian filtering. The nominal state vector of ${\left[\mathit{p},\mathit{v},\mathit{q},{\mathit{a}}_{\mathit{b},}{\mathit{\omega }}_{\mathit{b}}\right]}^T$ is used in this study, where the additional dimension appears due to the quaternion representation used for rotation, the quaternion method is widely used in attitude update because it only requires the calculation of matrix differential equations, which is relatively small in computation and can avoid the singular value problem of Euler angles. In this paper, the quaternion method is used to solve the attitude of the carrier. The operators $\mathit{q}$ and $\mathit{R}$ represent, respectively, the quaternion corresponding to the axial angular vector $\mathit{\theta }$ and its rotation matrix. In addition, the error state vector is ${\left[\mathit{\delta }\mathit{p},\mathit{\delta }\mathit{v},\mathit{\delta }\mathit{q},\mathit{\delta }{\mathit{a}}_{\mathit{b},}\mathit{\delta }{\mathit{\omega }}_{\mathit{b}}\right]}^T$. The relevant symbol definitions of true, nominal, and error state are listed and compared in

Table 1. The definition of variable symbols.

Name	True State	Nominal State	Error States
All states	${\mathit{x}}_{\mathit{t}}$	$\mathit{x}$	$\mathit{\delta }\mathit{x}$
Location	${\mathit{p}}_{\mathit{t}}$	$\mathit{p}$	$\mathit{\delta }\mathit{p}$
Speed	${\mathit{v}}_{\mathit{t}}$	$\mathit{v}$	$\mathit{\delta }\mathit{v}$
Quaternion	${\mathit{q}}_{\mathit{t}}$	$\mathit{q}$	$\mathit{\delta }\mathit{q}$
Rotation matrix	${\mathit{R}}_{\mathit{t}}$	$\mathit{R}$	$\mathit{\delta }\mathit{R}$
Angular vectors	${\mathit{\theta }}_{\mathit{t}}$	$\mathit{\theta }$	$\mathit{\delta }\mathit{\theta }$
Acceleration bias	${\mathit{a}}_{\mathit{b}\mathit{t}}$	${\mathit{a}}_{\mathit{b}}$	$\mathit{\delta }{\mathit{a}}_{\mathit{b}}$
Angular velocity bias	${\mathit{\omega }}_{\mathit{b}\mathit{t}}$	${\mathit{\omega }}_{\mathit{b}}$	$\mathit{\delta }{\mathit{\omega }}_{\mathit{b}}$

.

2.2.1. Continuous Time Kinetic Model

The nominal state kinematics refers to the system modelled with system noise or external perturbations in the form of

$$\begin{array}{l}{\dot{\mathit{p}}}_t={\mathit{v}}_t\\ {\dot{\mathit{v}}}_t={\mathit{R}}_t({\mathit{a}}_m-{\mathit{a}}_{bt}-{\mathit{a}}_n)+{\mathit{g}}_t\\ {\dot{\mathit{q}}}_t=\frac{1}{2}{\mathit{q}}_t\otimes ({\mathit{\omega }}_m-{\mathit{\omega }}_{bt}-{\mathit{\omega }}_n).\\ {\dot{\mathit{a}}}_{bt}={\mathit{a}}_{\omega }\\ {\dot{\mathit{\omega }}}_{bt}={\mathit{\omega }}_{\omega }\end{array}$$

(9)

The acceleration and angular velocity measurements are represented by ${\mathit{a}}_m$ and ${\mathit{\omega }}_m$, respectively, while ${\mathit{a}}_n$ and ${\mathit{\omega }}_n$ represent the corresponding noise vectors, and ${\mathit{a}}_{\omega }$ and ${\mathit{\omega }}_{\omega }$ represent the bias vectors for acceleration and angular velocity, respectively. The equations driving the system dynamics in the nominal state are as follows, where the nominal state refers to the modeled system without any noise or perturbations:

$$\begin{array}{l}\dot{\mathit{p}}=\mathit{v}\\ \dot{\mathit{v}}=\mathit{R}({\mathit{a}}_m-{\mathit{a}}_{\mathit{b}})+\mathit{g}\\ \dot{\mathit{q}}=\frac{1}{2}\mathit{q}\otimes ({\mathit{\omega }}_m-{\mathit{\omega }}_b).\\ {\dot{\mathit{a}}}_b=0\\ {\dot{\mathit{\omega }}}_b=0\end{array}$$

(10)

Solving for the error state and simplifying all second−order infinitesimals. The kinetic equation for the error state can be obtained from Equations (9) and (10).

$$\begin{array}{l}\mathit{\delta }\dot{\mathit{p}}=\mathit{\delta }\mathit{v}\\ \mathit{\delta }\dot{\mathit{v}}=\mathit{R}({\mathit{a}}_m-{\mathit{a}}_b)\mathit{\delta }\mathit{\theta }-\mathit{R}\mathit{\delta }{\mathit{a}}_b-\mathit{R}{\mathit{a}}_n\\ \mathit{\delta }\dot{\mathit{\theta }}=-({\mathit{\omega }}_m-{\mathit{\omega }}_b)\mathit{\delta }\mathit{\theta }-\mathit{\delta }{\mathit{\omega }}_b-{\mathit{\omega }}_n.\\ \mathit{\delta }{\dot{\mathit{a}}}_b={\mathit{a}}_{\mathit{\omega }}\\ \mathit{\delta }{\dot{\mathit{\omega }}}_b={\mathit{\omega }}_{\mathit{\omega }}\end{array}$$

(11)

2.2.2. Discrete Time Kinetic Model

In order to apply the derived state and measurement equations to the sensor fusion filter, they must be discretized based on the sampling time. The original continuous time equations are transformed into their discrete time form.

The recursive expression for the motion model of the nominal state is

$$\begin{array}{l}{\mathit{p}}_{(k+1)}={\mathit{p}}_k+{\mathit{v}}_k\mathrm{\Delta }t+\frac{1}{2}[{\mathit{R}}_k({\mathit{a}}_{mk}-{\mathit{a}}_{bk})+\mathit{g}]\mathrm{\Delta }{t}^2\\ {\mathit{v}}_{(k+1)}={\mathit{v}}_k+[{\mathit{R}}_k({\mathit{a}}_{mk}-{\mathit{a}}_{bk})+{\mathit{g}}_k]\mathrm{\Delta }t\\ {\mathit{q}}_{(k+1)}={\mathit{q}}_k\otimes {\mathit{q}}_k(({\mathit{\omega }}_{mk}-{\mathit{\omega }}_{bk})\mathrm{\Delta }t)\\ {\mathit{a}}_b(k+1)={\mathit{a}}_{bk}\\ {\mathit{\omega }}_b(k+1)={\mathit{\omega }}_{bk}\end{array}$$

(12)

The kinematic model of the error state is expressed recursively as a function of the non−negative integer k representing the kth time step:

$$\begin{array}{l}\mathit{\delta }{\mathit{p}}_{k+1}=\mathit{\delta }{\mathit{p}}_k+\mathit{\delta }{\mathit{v}}_k\mathrm{\Delta }t\\ \mathit{\delta }{\mathit{v}}_{k+1}=\left[-{\mathit{R}}_k(a_{mk}-a_{bk})\mathit{\delta }{\mathit{\theta }}_k-{\mathit{R}}_k\mathit{\delta }{\mathit{a}}_{bk}\right]\mathrm{\Delta }t+\mathit{\delta }{\mathit{v}}_k-{\mathit{w}}_{vk}\\ \mathit{\delta }{\mathit{\theta }}_{k+1}={\mathit{R}}_k^T\left(({\mathit{\omega }}_{mk}-{\mathit{\omega }}_{bk})\mathrm{\Delta }t\right)\mathit{\delta }{\mathit{\theta }}_k-\mathit{\delta }{\mathit{\omega }}_{bk}\mathrm{\Delta }t+{\mathit{w}}_{\mathit{\theta }k}\\ \mathit{\delta }{\mathit{a}}_{b(k+1)}=\mathit{\delta }{\mathit{a}}_{bk}+{\mathit{w}}_{ak}\\ \mathit{\delta }{\mathit{\omega }}_{b(k+1)}=\mathit{\delta }{\mathit{\omega }}_{bk}+{\mathit{w}}_{wk}\end{array}$$

(13)

The Gauss random noise for velocity, attitude, acceleration bias, and angular velocity bias are denoted as ${\mathit{w}}_{vk},{\mathit{w}}_{\mathit{\theta }k},{\mathit{w}}_{ak},,{\mathit{w}}_{\mathit{\omega }k}$, their mean is zero, and their covariance matrices are obtained by integrating the covariances of ${\sigma }_{{\mathit{a}}_n}$, ${\sigma }_{{\mathit{\omega }}_n}$, ${\sigma }_{{\mathit{a}}_{\omega }}$ and ${\sigma }_{{\mathit{\omega }}_{\mathit{\omega }}}$ over the step time $\mathrm{\Delta }t$.

$$\begin{array}{l}{\mathit{W}}_v={\sigma }_{{\mathit{a}}_n}^2\mathrm{\Delta }{t}^2\mathit{I}\\ {\mathit{W}}_{\mathit{\theta }}={\sigma }_{{\mathit{\omega }}_n}^2\mathrm{\Delta }{t}^2\mathit{I}\\ {\mathit{W}}_A={\sigma }_{{\mathit{a}}_{\mathit{\omega }}}^2\mathrm{\Delta }t\mathit{I}\\ {\mathit{W}}_{\mathrm{\Omega }}={\sigma }_{{\mathit{\omega }}_{\mathit{\omega }}\mathrm{\Delta }t}^2\mathit{I}\end{array}$$

(14)

The standard deviation of Gaussian white noise for acceleration and angular velocity are denoted by ${\sigma }_{{\mathit{a}}_{\mathit{n}}}$ and ${\sigma }_{{\mathit{\omega }}_n}$, respectively. Similarly, ${\sigma }_{{\mathit{a}}_{\mathit{\omega }}}$ and ${\sigma }_{{\mathit{\omega }}_{\mathit{\omega }}}$ are used to represent the standard deviation of Gaussian white noise for acceleration and angular velocity bias, respectively.

2.2.3. Development of the Error State Model

The discrete vector forms for all states, error states, IMU measurements, and noise are defined as follows:

$$\begin{array}{l}{\mathit{x}}_k={[{\mathit{x}}_k,{\mathit{v}}_k,{\mathit{q}}_k,{\mathit{a}}_{bk},{\mathit{\omega }}_{bk}]}^T,\mathit{\delta }{\mathit{x}}_k={\left[\mathit{\delta }{\mathit{p}}_k,\mathit{\delta }{\mathit{v}}_k,\mathit{\delta }{\mathit{q}}_k,\mathit{\delta }{\mathit{a}}_{bk},\mathit{\delta }{\mathit{\omega }}_{bk}\right]}^T,\\ {\mathit{u}}_{mk}={[{\mathit{a}}_{mk},{\mathit{\omega }}_{mk}]}^T,{\mathit{w}}_k={[{\mathit{w}}_{vk},{\mathit{w}}_{\mathit{\theta }k},{\mathit{w}}_{ak},{\mathit{w}}_{\mathit{\omega }k}]}^T\end{array}$$

(15)

The recursive equation for all the error state is derived by combining Equations (5) and (7) in the form of

$$\mathit{\delta }{\mathit{x}}_{k+1}={\mathit{f}}_{\mathit{\delta }}({\mathit{x}}_k,\mathit{\delta }{\mathit{x}}_k,{\mathit{u}}_{mk},{\mathit{w}}_k)={\mathit{F}}_{xk}({\mathit{x}}_k,{\mathit{u}}_{mk})+{\mathit{F}}_{wk}{\mathit{w}}_k,$$

(16)

where $f_{\mathit{\delta }}\left(·\right)$ represents the recursive function of the error state, the $F_{xk}$ and $F_{wk}$ are the Jacobian matrices corresponding to the error and noise states, respectively, which can be derived as

$${\mathit{F}}_{xk}=\left[\begin{array}{ccccc}\mathit{I}& \mathit{I}\mathrm{\Delta }t& 0& 0& 0\\ 0& \mathit{I}& -{\mathit{R}}_k^T({\mathit{\omega }}_{mk}-{\mathit{\omega }}_{bk})\mathrm{\Delta }t& -{\mathit{R}}_k\mathrm{\Delta }t& 0\\ 0& 0& {\mathit{R}}_k^T({\mathit{\omega }}_{mk}-{\mathit{\omega }}_{bk})\mathrm{\Delta }t& 0& -\mathit{I}\mathrm{\Delta }t\\ 0& 0& 0& \mathit{I}& 0\\ 0& 0& 0& 0& \mathit{I}\end{array}\right],{\mathit{F}}_{wk}=\left[\begin{array}{cccc}0& 0& 0& 0\\ \mathit{I}& 0& 0& 0\\ 0& \mathit{I}& 0& 0\\ 0& 0& \mathit{I}& 0\\ 0& 0& 0& \mathit{I}\end{array}\right].$$

(17)

2.2.4. The ESKF Prediction Process

The error states and the covariance prediction process can be obtained as

$$\begin{array}{l}\mathit{\delta }{\stackrel{⌣}{\mathit{x}}}_{k+1}^{-}={\mathit{F}}_{xk}({\stackrel{⌢}{\mathit{x}}}_k,{\mathit{u}}_{mk})\mathit{\delta }{\mathit{x}}_k\\ {\stackrel{⌣}{\mathit{P}}}_{(k+1)}^{-}={\mathit{F}}_{xk}{\stackrel{⌣}{\mathit{P}}}_k{\mathit{F}}_{xk}^T+{\mathit{F}}_{wk}{\mathit{Q}}_w{\mathit{F}}_{wk}^T\end{array}$$

(18)

where ${\mathit{Q}}_w$ represents the noise covariance matrix of the form:

$${\mathit{Q}}_w=\left[\begin{array}{cccc}{\mathit{W}}_v& 0& 0& 0\\ 0& {\mathit{W}}_{\theta }& 0& 0\\ 0& 0& {\mathit{W}}_A& 0\\ 0& 0& 0& {\mathit{W}}_{\mathrm{\Omega }}\end{array}\right].$$

(19)

2.2.5. The ESKF Observation Process

Once the GNSS information becomes available, the observation is incorporated to continuously update the filter. This process also involves calibrating for the accelerometer and gyroscope biases.

The observation equation is typically expressed in a more uniform form within the filter in the form of

$${\mathit{z}}_k=\mathit{h}({\mathit{x}}_{tk})+{\mathit{w}}_{mk},$$

(20)

where ${\mathit{z}}_k$ is the measurement signal vector, ${\mathit{w}}_{mk}$ represents the Gaussian white noise of the measurement signal, and its covariance is $\mathit{V}$. The error calibration update equation is derived as

$$\begin{array}{l}{\mathit{K}}_{k+1}={\stackrel{⌣}{\mathit{P}}}_{k+1}^{-}{\mathit{H}}_{k+1}^T{({\mathit{H}}_{k+1}{\stackrel{⌣}{\mathit{P}}}_{k+1}^{-}{\mathit{H}}_{k+1}^T+{\mathit{V}}_{k+1})}^{-1}\\ \mathit{\delta }{\stackrel{⌢}{\mathit{X}}}_{k+1}={\mathit{K}}_{k+1}({\mathit{Z}}_{k+1}-\mathit{H}({\stackrel{⌣}{\mathit{X}}}_{k+1}))\\ {\stackrel{⌢}{\mathit{P}}}_{k+1}=(\mathit{I}-{\mathit{K}}_{k+1}{\mathit{H}}_{k+1}){\stackrel{⌣}{\mathit{P}}}_{k+1}^{-}\end{array}$$

(21)

The Jacobian matrix H is required to be defined with respect to the error state $\mathit{\delta }\mathit{x}$, and evaluated at the best true state estimate ${\widehat{\mathit{x}}}_t=\mathit{x} \oplus \mathit{\delta }\widehat{\mathit{x}}$. As the error state mean is zero at this stage (we have not observed it yet), we have ${\mathit{x}}_t=\mathit{x},$ and we can use the nominal error x as the evaluation point, leading to

$$\mathit{H}={\frac{\partial \mathit{h}}{\partial {\mathit{x}}_t}|}_x{\frac{\partial {\mathit{x}}_t}{\partial \mathit{\delta }\mathit{x}}|}_x={\mathit{H}}_x{\mathit{X}}_{\mathit{\delta }x},$$

(22)

in which

$${\mathit{X}}_{\mathit{\delta }\mathit{x}}\triangleq {\frac{\partial {\mathit{x}}_t}{\partial \mathit{\delta }\mathit{x}}|}_{\mathit{x}}=\left[\begin{array}{ccc}{\mathit{I}}_6& 0& 0\\ 0& {\mathit{Q}}_{\mathit{\delta }\mathit{\theta }}& 0\\ 0& 0& {\mathit{I}}_6\end{array}\right],$$

(23)

$${\mathit{Q}}_{\mathit{\delta }\mathit{\theta }}=\frac{1}{2}\left[\begin{array}{ccc}-q_x& -{\mathit{q}}_y& -{\mathit{q}}_z\\ {\mathit{q}}_w& -{\mathit{q}}_z& {\mathit{q}}_y\\ {\mathit{q}}_z& {\mathit{q}}_w& -{\mathit{q}}_{\mathit{x}}\\ -{\mathit{q}}_y& {\mathit{q}}_{\mathit{x}}& {\mathit{q}}_w\end{array}\right].$$

(24)

2.2.6. Combination of ESKF Error State and Nominal State Process

Based on the recurrence of the a priori system state and the calibration of the error state, the updated form is obtained as follows:

$${\stackrel{⌢}{\mathit{x}}}_k={\stackrel{⌢}{\mathit{x}}}_{{}_k}^{-}\oplus \mathit{\delta }{\stackrel{⌢}{\mathit{x}}}_k.$$

(25)

Each of these system states corresponds to the following:

$$\begin{array}{l}{\stackrel{⌢}{\mathit{p}}}_k={\stackrel{⌢}{\mathit{p}}}_k^{-}+\mathit{\delta }{\stackrel{⌢}{\mathit{p}}}_k\\ {\stackrel{⌢}{\mathit{v}}}_k={\stackrel{⌢}{\mathit{v}}}_k^{-}+\mathit{\delta }{\stackrel{⌢}{\mathit{v}}}_k\\ {\stackrel{⌢}{\mathit{q}}}_k={\stackrel{⌢}{\mathit{q}}}_k^{-}\otimes \stackrel{⌢}{\mathit{q}}\mathit{\delta }{\stackrel{⌢}{\mathit{\theta }}}_k.\\ {\stackrel{⌢}{\mathit{a}}}_{bk}={\stackrel{⌢}{\mathit{a}}}_{bk}^{-}+\mathit{\delta }{\stackrel{⌢}{\mathit{a}}}_{bk}\\ {\stackrel{⌢}{\mathit{\omega }}}_{bk}={\stackrel{⌢}{\mathit{\omega }}}_{bk}^{-}+\mathit{\delta }{\stackrel{⌢}{\mathit{\omega }}}_{bk}\end{array}$$

(26)

2.2.7. ESKF Error State Reset

After injecting the error state into the nominal state, the a priori error state and the corresponding covariance need to be reset, as

$$\begin{array}{l}\mathit{\delta }\stackrel{⌢}{\mathit{x}}=0\\ \mathit{P}={\mathit{G}}_k{\mathit{P}}_k{\mathit{G}}_k^T\end{array}$$

(27)

where $\mathit{G}$ is the reset function $\mathit{g}\left(\mathit{\delta }\mathit{x}\right)=\mathit{\delta }\mathit{x}\ominus \mathit{\delta }\widehat{\mathit{x}}$ of the Jacobi matrix, which is defined in the form of

$$\mathit{G}\triangleq {\frac{\partial \mathit{g}}{\partial \mathit{\delta }\mathit{x}}|}_{\mathit{\delta }\stackrel{⌢}{\mathit{x}}}=\left(\begin{array}{ccc}{\mathit{I}}_6& 0& 0\\ 0& \mathit{I}-(\frac{1}{2}\mathit{\delta }\stackrel{⌢}{\mathit{\theta }})& 0\\ 0& 0& {\mathit{I}}_9\end{array}\right).$$

(28)

3. Robust Localization via RTS Smoothing

3.1. Reliability of Measurement Information

In the case of a GNSS signal loss, when there are no available GNSS measurements to update, the covariance matrix in Equation (21) tends to be infinite. The Kalman gain $K$ tends to zero, so that when the measurement information is zero or differs too much from the predicted information, the update equation changes to the following form:

$$\begin{array}{l}\mathit{\delta }{\stackrel{⌢}{\mathit{x}}}_{k+1}=\mathit{\delta }{\stackrel{⌢}{\mathit{x}}}_k\\ {\stackrel{⌢}{\mathit{P}}}_{k+1}={\stackrel{⌣}{\mathit{P}}}_k^{-}\end{array}$$

(29)

3.2. RTS Fundamental Design

The RTS smoothing can be regarded as a technique for obtaining an optimal state estimate when observations are available from moment 1 to moment N; it involves using previous estimates obtained through Kalman filtering in order to perform backward smoothing from moment k + 1 to moment k resulting in a more precise estimate. This method falls under the category of fixed interval smoothing and is considered as a fixed interval smoother, which is convenient for implementation.

The RTS smoother involves a two−step process: a forward filter followed by a backward smoothing. The forward filter is a standard Kalman filter described by Equation (18), which maintains all the predicted and updated estimates as well as their corresponding covariances for each epoch during the entire mission. The backward smoothing procedure begins at the end of the forward filter at time $t_N$, with an initial condition $\mathit{\delta }{\mathit{x}}_{N,N}$ and $t_{N,N}$. Therefore, the backward smoothing can be seen as an update to the forward filter for obtaining an enhanced estimate. The equation for the RTS algorithm can be represented in the form of

$$\begin{array}{l}\mathit{\delta }{\stackrel{⌢}{\mathit{x}}}_{k,N}=\mathit{\delta }{\stackrel{⌢}{\mathit{x}}}_{k,k}+{\mathit{U}}_k({\stackrel{⌢}{\mathit{x}}}_{k+1,N}-{\stackrel{⌢}{\mathit{x}}}_{k+1,k})\\ {\stackrel{⌢}{\mathit{P}}}_{k,N}={\stackrel{⌢}{\mathit{P}}}_{k,k}+{\mathit{U}}_k({\stackrel{⌢}{\mathit{P}}}_{k+1,N}-{\stackrel{⌢}{\mathit{P}}}_{k+1,k}){\mathit{U}}_k^T\end{array}$$

(30)

where delta $\mathit{\delta }{\mathit{x}}_{k,N}$ is the smoothed estimate of the state vector, and $U_k$ is the smoothing gain matrix, which is calculated by the following equation:

$${\mathit{U}}_k={\stackrel{⌢}{\mathit{P}}}_{k,k}{\mathit{F}}_{k+1,k}^T{\stackrel{⌢}{\mathit{P}}}_{k+1,k}^{-1},$$

(31)

where $k=N-1,N-2,\dots \dots ,0$. The RTS smoothing algorithm uses the results of the forward filter and backward smoothing to obtain an improved estimate. The inverse smoothing starts from the last filter result and proceeds forward one by one, so it must obtain N filter results to smooth the observations within the period [0, N]. However, if N is too large, the inverse smoothing process can be obviously lagged, which would limit the algorithm’s application in real time. Therefore, a segmented RTS smoothing method is designed in this study.

3.3. Segmented RTS Smoothing Algorithm

Assuming the duration of a dynamic system is k = 1, 2……, N, the segmented RTS smoothing algorithm involves forward filtering and inverse smoothing of N observations in segments. The segment length is L, where 1 < L < N. In other words, the RTS inverse smoothing process is performed immediately after obtaining the filtering results for the segmented period, without waiting for subsequent filtering results. This approach significantly reduces the lag time of the inverse smoothing process. Additionally, the segmentation length can be varied, avoiding the issue of poor real−time performance associated with RTS smoothing. In this study, the segmented RTS smoothing algorithm is applied to the potential probability assumption density filtering algorithm, effectively addressing the problem of poor real−time performance resulting from the use of RTS smoothing.

Step 1 Output variables are cleared: $\mathit{\delta }{\widehat{\mathit{x}}}_k=[ ]$,${\widehat{\mathit{p}}}_k=[ ]$.

Step 2 Local filtering result variable clearing $\mathit{\delta }{\breve{\mathit{x}}}_k=[ ]$,${\breve{\mathit{p}}}_k=[ ]$. The filtered result variable after local association is cleared ${\mathit{x}}_k=[ ]$,${\mathit{Q}}_k=[ ]$, the segmented smoothing counter $count$ is cleared to zero.

Step 3 If the tracking of the target is not finished, action continues down the track, otherwise the smoothing result $\mathit{\delta }{\widehat{\mathit{x}}}_k$,${\widehat{\mathit{p}}}_k$ is output; then, the entire segmented RTS smoothing algorithm ends.

Step 4 Upon receiving the current filtering result, it is stored in the variables $\mathit{\delta }{\breve{\mathit{x}}}_k$ and ${\breve{\mathit{p}}}_k$. the Hungarian algorithm is used to correlate the trajectories and estimates, obtaining the filter value for each target. The correlated filter result is saved in variables ${\mathit{x}}_k$ and ${\mathit{Q}}_k$, and they are counted using a counter called count.

Step 5 If $count==L$, after performing local correlation using ${\mathit{x}}_k$ and ${\mathit{Q}}_k$ for each target’s filtering results, the inverse smoothing process uses the RTS smoothing algorithm. The results of the smoothing process are stored in variables delta $\mathit{\delta }{\breve{\mathit{x}}}_k$ and ${\breve{\mathit{p}}}_k$ before being transferred to Step 2. In order to significantly reduce the lag time of the inverse smoothing process, it is carried out immediately following the acquisition of the filtered data without waiting for subsequent data. Real−time performance of the fusion algorithm can be improved by decreasing the segment length $L$.

To correct the position, velocity, and attitude values that have been calculated in the forward filtering, it is recommended to use the state error smoothing values. These smoothing values can be utilized to derive the final optimal smoothed values for these states using the following equations.

4. Results and Discussions

The proposed sensor fusion algorithm is demonstrated in a relatively open environment, which allows for uninterrupted satellite signal and individualized GNSS localization. The acquisition frequency for GNSS data is 1 Hz, while the IMU data are acquired at a frequency of 100 Hz; the smooth dimension $L$ is selected as 10. These parameters provide acceptable conditions for analyzing the accuracy of combined GNSS/IMU localization, even in the event of GNSS signal loss, as demonstrated by the trajectory shown in Figure 2. In order to evaluate the localization accuracy of the GNSS/IMU combined localization process, the simulation involves artificially inducing the loss of lock by introducing exaggerated errors to the GNSS satellite observation information based on raw GNSS data.

4.1. Oval Track Simulation Analysis

Figure 2 illustrates the simulated oval shape trajectory, which comprises a long straight line with small angle turns in two dimensions.

The localization results for the various filtering algorithms are shown in Figure 3, Figure 4 and Figure 5 and

Table 2. RMS values of localization errors for different algorithms (Oval).

Position Error RMS (m)	Lateral	Longitudinal	Vertical
GNSS	2.049	2.598	2.619
EKF	1.680	1.820	3.075
ESKF	1.633	1.782	1.476
ESKF–RTS	1.463	1.588	1.393

. It can be seen that the ESKF algorithm exhibits better target tracking accuracy compared to the EKF algorithm, as evidenced by its RMS values of 1.633 m, 1.782 m, and 1.476 m for Lateral, Longitudinal, and Vertical directions, respectively. By reducing the error state parameter, the accuracy of the three poses is improved by 2.8%, 2.1%, and 52.0%, respectively. After performing the backward smoothing filtering process, the RMS values of the three attitudes are further optimized to 1.463 m, 1.588 m, and 1.393 m, respectively. Moreover, the target tracking accuracy of ESKF–RTS is superior to ESKF, with the attitude accuracy in the three directions improving by 10.4%, 10.9%, and 5.6%, respectively. The trajectory also reveals that the curve of ESKF–RTS is smoother, further highlighting the advantages of ESKF–RTS over the other two algorithms in addressing the problem of smooth estimation of nonlinear states.

4.2. Serpentine Track Simulation Analysis

Further verification of the designed algorithms is performed in a serpentine trajectory, as shown in Figure 6, and the localization results are compared as shown in Figure 7, Figure 8 and Figure 9 and

Table 3. RMS values of localization errors for different algorithms (Serpentine 1).

Position Error RMS (m)	Lateral	Longitudinal	Vertical
GNSS	1.912	1.793	5.680
EKF	0.955	1.585	5.823
ESKF	0.464	0.641	1.700
ESKF−RTS	0.206	0.243	0.912

.

Table 3 illustrates the improved performance of the ESKF and ESKF−RTS algorithms compared to the EKF algorithm in the serpentine condition. The RMS of Lateral, Longitudinal, and Vertical in EKF are 0.955 m, 1.585 m, and 5.823 m, respectively. After reducing the state value, the RMS of the three positions for the ESKF algorithm are 0.464 m, 0.641 m, and 1.700 m, which are 51.4%, 59.6%, and 70.8% better than the EKF algorithm. The ESKF−RTS algorithm further improves the position accuracy, with RMS values of 0.206 m, 0.243 m, and 0.912 m for the three directions, respectively, which are 55.6%, 62.1%, and 46.4% higher than those of the ESKF algorithm. Figure 7, Figure 8 and Figure 9 show the comparison of the root mean square errors, which further demonstrates the superior performance and stability of the ESKF−RTS algorithm. However, since the simulation was conducted with a relatively good GNSS signal, the robustness of the algorithm could not be well evaluated. Subsequently, the GNSS data for the serpentine condition are partitioned into eight segments, and the error magnitude is amplified by a factor of two at intervals of 600 s in order to evaluate the algorithm’s robustness.

The serpentine working condition with further exaggerated GNSS signal error is analyzed, as shown in Figure 10, Figure 11 and Figure 12 and

Table 4. RMS values of localization errors for different algorithms (Serpentine 2).

Position Error RMS (m)	Lateral	Longitudinal	Vertical
GNSS	3.370	3.308	8.988
EKF	1.231	1.735	1.453
ESKF	0.635	0.890	0.957
ESKF−RTS	0.368	0.422	1.456

. The RMS values for the Lateral, Longitudinal, and Vertical directions are 1.231 m, 1.735 m, and 1.453 m, respectively, for the EKF algorithm. Meanwhile, the ESKF algorithm improves these values by 48.4%, 48.7%, and 34.1%, respectively, resulting in the RMS values of 0.635 m, 0.890 m, and 0.957 m. Moreover, the ESKF−RTS algorithm improves the position accuracy in three directions by 42.1%, 52.6%, and 52.1%, respectively, compared to the ESKF algorithm, with RMS values of 0.368 m, 0.422 m, and 1.456 m. Even though the accuracy of ESKF−RTS decreases on the Vertical axis, the higher degree of smoothing in Figure 12 suggests that the ESKF−RTS smoothing algorithm can significantly enhance robustness and achieve precise localization, even with lower accuracy of GNSS sensors.

Next, we continued the simulation of circular operating conditions and divided the entire simulation process into four sections to validate the feasibility of the algorithm by doubling the GNSS signal error, as shown in Figure 13, Figure 14, Figure 15 and Figure 16. The experimental results shown in

Table 5. RMS values of localization errors for different algorithms (Polygonal).

Position Error RMS (m)	Lateral	Longitudinal	Vertical
GNSS	2.601	2.677	8.516
EKF	1.243	1.289	4.328
ESKF	1.253	1.320	1.237
ESKF−RTS	0.900	0.896	1.999

once again demonstrated the feasibility and robustness of the error state Kalman filtering algorithm, indicating that the algorithm can achieve stable and accurate integrated navigation under various operating conditions.

The above data show the results of testing the navigation system using EKF, ESKF, and ESKF−RTS algorithms. By analyzing these results, we can draw the following conclusions. The above data demonstrate the performance of three different filtering algorithms in GNSS navigation. First, we can see that the performance of the EKF and ESKF algorithms is relatively similar, with position errors around 1.3 m. This is because both algorithms use Kalman filtering, but the ESKF algorithm introduces error states while considering errors, which can more accurately estimate errors and improve accuracy. At the same time, the ESKF algorithm can also reduce the impact of errors in future time by predicting error states, thus improving the stability of the algorithm.

On the other hand, the ESKF−RTS algorithm performs even better, with position errors even lower than 1 m. This is because the ESKF−RTS algorithm uses segmental smoothing to optimize the filtering results, which can more accurately estimate and correct errors. In the ESKF−RTS algorithm, RTS stands for “Recursive Least Squares Smoothing” which can combine the prediction results of the ESKF algorithm with the observation results to obtain more accurate state estimation results.

Overall, the above data indicate that the ESKF−RTS algorithm performs well in GNSS navigation. The advantage of this algorithm is that it not only considers error states, but also further optimizes the filtering results through smoothing algorithms. Therefore, in practical applications, selecting the ESKF−RTS algorithm for navigation filtering can achieve more accurate and stable results, improving the reliability and accuracy of the navigation system.

5. Conclusions and Future Work

This paper introduces an error state extended Kalman filter algorithm and segmented Rauch–Tung–Striebel (RTS) smoothing algorithm to enhance the localization accuracy and robustness of GNSS and IMU sensors. The cumulative error of INS over time are overcome when GNSS signal is disturbed. The simulation results show that the proposed method is more linear and has higher localization accuracy than the traditional EKF algorithm. It also demonstrates good robustness in achieving better accuracy with low−quality GNSS signals. The algorithm can serve as a foundation for low−cost sensor fusion processing and is a valuable reference for further research.

Over the next few years, combined navigation systems will see increased usage and development across a range of applications. As sensor and communication technologies progress, and the demands for navigation safety and reliability continue to rise, there will be a greater need for more accurate and dependable navigation systems.

Furthermore, the RTS smoothing algorithm−assisted combined navigation algorithm presented in this paper was simulated for offline computation. For practical applications in the future, real−world testing will be necessary to verify the hardware feasibility of the algorithm, and to consider real−time data transmission and computation to achieve unmanned driving. Overall, positioning is a crucial component of intelligent driving, and its potential impact on society and the economy is vast. Combined navigation will be more and more widely used in production and practical life.