Improved-Performance Vehicle’s State Estimator Under Uncertain Model Dynamics

This article proposes an enhanced fusion technique to improve the accuracy of the state estimation of a navigational system. The smooth variable structure filter (SVSF) is examined to estimate the system’s state under model uncertainty. Its combination with the unscented Kalman filter (UKF) to acquire better navigational accuracy while being robust to the system’s modeling uncertainty is investigated. The proposed hybrid method is compared with the extended Kalman filter (EKF), the UKF, and the SVSF. The proposed algorithms fuse an inertial measurement unit (IMU) with the Global Positioning Systems (GPS) measurements to obtain the vehicle’s state. Experimental results are compared to a commercial off-the-shelf (COTS) solution. It is shown that all filtering strategies have similar performance in the absence of large-magnitude noise and model uncertainties. When injecting modeling uncertainties, the performance of the UKF degrades, and that of the EKF goes out of bounds. On the other hand, increasing the covariances of the measurement and dynamics noise sequences causes the path of the SVSF to become nonsmooth and roughly oscillates around the true path. The proposed integrated UK-SVSF algorithm achieves the following objectives: first, using the Kaman-based filter enhances the optimality of the filter to GPS/IMU dynamics and measurements noise. Second, using the UKF reduces the estimation error by eliminating the first-order linearization step. Finally, using the SVSF enhances the estimate’s robustness to model uncertainty. Results reveal that, in the presence of both large-magnitude noise and model uncertainties, the UK-SVSF gives an enhanced estimation performance.

SECTION I. Introduction

Navigation is the process of determining a moving vehicle’s position, velocity, and attitude with respect to a known [1]. The state of the moving vehicle (position, velocity, and attitude) is known as the navigational state. This state can be obtained based on onboard navigational systems (dead reckoning methods) or external navigational sources (position fixing methods) [2]. The dead reckoning methods [such as the inertial navigation systems (INSs)] are based on 3-D integration sequences, relating the initial conditions of the vehicle’s position and velocity to the future states of the vehicle. The availability of a high-frequency solution at all times is the advantage of dead reckoning methods. However, given the possible bias in the measured state dynamics, its solution contains a moderately high error due to the integration process. On the other hand, position-fixing methods (such as GPS-based systems) are not based on integration from past states. Therefore, their associated results are more reliable and contain less error. However, position-fixing solutions are not always available due to their low sampling frequency and possible environmental constraints.

Navigational sensors have varying accuracy [3], [4]. Low-cost COTS sensors are typically of low accuracy, making their data unreliable [2]. Additionally, sensors’ aging causes an increase in estimation error, adversely affecting the overall solution’s accuracy. Therefore, to have an acceptable vehicle’s state estimation with high accuracy, low cost, and continuous availability, multiple aiding and complementary sensors are fused to overcome inherent individual errors.

In the early 1960s, Rudolph Kalman introduced the most common fusion technique, the Kalman filter (KF) [5], [6]. The KF is a predictor–corrector estimator that initially predicts the system’s states utilizing the system’s dynamics and subsequently corrects the predicted estimate through the filter’s gain once the measurements are acquired [7]. The KF is the optimal filter for linear systems based on the minimum-mean-squared error (MMSE) criterion. However, if the system is nonlinear, the KF is not applicable. Therefore, the extended KF (EKF) was introduced as an extension to KF for nonlinear systems [8]. EKF’s process is almost similar to the KF, having the Jacobian linearization in its formulation to overcome the nonlinearity in the system. If the system’s nonlinearity is moderately high, the EKF’s performance is greatly affected as it uses the first-order Taylor series expansion (linearization). Hence, its solution is inaccurate, unreliable, and, in some cases, will not converge to the true trajectory.

To overcome the EKF’s limitations, Uhlmann and Julier introduced a new filter known as the unscented KF (UKF) [9], [10]. The UKF is formulated based on an unscented transformation that utilizes the sigma points to evaluate the measurement’s posteriori probability and accurately obtain the high-probability estimate of the state. However, the UKF is computationally heavier compared to the EKF.

The KF and its variants are only robust to the dynamic and measurement noises. The presence of modeling uncertainty causes their solution to be inaccurate and unreliable [11], [12]. These uncertainties could result from utilizing low-cost sensors that do not fully capture the system’s dynamics. To handle these model uncertainties, Habibi [13] introduced a new filter, the smooth variable structure filter (SVSF), based on the sliding-mode theory. This filter forces the estimated trajectory to vary within the boundary layer, which makes the filter robust to modeling uncertainty. However, this filter cannot handle the dynamics and measurement noise, making it a suboptimal filter compared to the KF-based solutions [14].

The INS, an electromechanical system consisting of inertial sensors in a unit called Inertial the inertial measurement unit (IMU), is a core component of any navigational system to formulate the vehicle’s dynamics. Due to its complexity, sustained high-accuracy IMUs that precisely characterize the vehicle dynamics are unaffordable. Many fusion techniques combine low-cost IMUs with aiding systems (such as GPS measurements, encoders, vision, etc.) [4], [15], [16], [17], [18], [19], [20], [21], [22], [23], [24]. These methods were implemented to minimize the error in the estimation by enhancing the performance while reducing the effect of the dynamics and measurement noise. Nonetheless, to employ low-cost IMUs, the utilized sensor fusion technique must consider possible model uncertainty. The model uncertainty results from possible unmodeled vehicle dynamics or bias in the vehicle’s specific force and angular velocity measurements. In addition, they occur due to simplification assumptions in obtaining the system’s error model. Accounting for the model uncertainty will enable a long-term, high-accuracy navigational solution.

This article aims to improve state estimation accuracy by compensating for the uncertainties in the system’s model. A hybrid fusion technique that combines the UKF with the SVSF is proposed to achieve this objective. This technique, known as the UK-SVSF, compensates for additive dynamics, measurement noise sequences, and model uncertainty in the system. To the best of the authors’ knowledge, no one has addressed the issue of improving the IMU/GPS localization due to model uncertainty.

The proposed UK-SVSF algorithm achieves three objectives. First, using the Kalman-based filter enhances the optimality of the filter to dynamics and measurement noise. Second, using the UKF reduces the estimation error by eliminating the first-order linearization step. Finally, using the SVSF enhances the estimate’s robustness to model uncertainty. The proposed algorithm is compared with the SVSF, EKF, and UKF. Table 1 summarizes the compared filters with their distinctive characteristics.

TABLE 1 Filters Used for Vehicle State Estimation

This article is organized as follows. Section II describes the GPS/IMU’s kinematic model of the vehicle. Section III describes the sensor fusion methodologies that are utilized. Section IV describes the experimental setup and the results obtained to verify the proposed algorithms’ accuracy. Finally, the work is concluded in Section V.

SECTION II. Kinematic Modeling and Mechanization

Utilizing the IMU, gyroscope sensors measure the vehicle’s angular velocity in three perpendicular directions. Similarly, the accelerometer sensors measure the specific forces of the vehicle along three perpendicular directions. In a strap-down IMU, the angular velocity and specific force measurements are acquired with respect to the vehicle’s body frame [25]. The INS provides position, velocity, and the host vehicle’s attitude by applying a 3-D integration on the IMU data given initial conditions of the position, velocity, and attitude. This process is called the IMU mechanization [25].

In this work, the MIDG IIC unit [26], a commercial off-the-shelf (COTS) unit composed of a strap-down IMU and a GPS receiver, provides the ground-truth solution. On the other hand, the proposed algorithm utilizes the raw IMU and GPS measurements. The IMU raw data of the vehicle’s specific forces, $F^{b}$ , measured by the accelerometer along the three-body axes of the vehicle, and the angular rate $\omega _{ib}^{b}$ , measured by the gyroscope along the three-body axes of the vehicle, are used to characterize the dynamics of the state of the vehicle.

In the sensor fusion algorithm, three coordinate systems are used: the body frame (b-frame), the navigation frame (n-frame), and the Earth-centered Earth-fixed frame (e-frame). The body frame’s x-axis (or roll axis), y-axis (or pitch axis), and z-axis (or yaw axis) are fixed with respect to the vehicle, where the x-axis points forward, the y-axis points to the right perpendicular to the x-axis, and the z-axis points downward perpendicular to the ${xy}$ -plane. The transformation matrix from the b-frame to the e-frame can be described as

$$\begin{equation*} C_{b}^{e} = {C_{e}^{n}}^{T}C_{b}^{n} = C_{n}^{e}C_{b}^{n} \tag{1}\end{equation*}$$ where $C_{n}^{e}$ is the transformation matrix from the n-frame to the e-frame and can be calculated based on the geodetic latitude $(\Phi)$ and longitude $(\Lambda)$ of the vehicle. Additionally, $C_{b}^{n}$ is the transformation matrix from the b-frame to the n-frame, which is calculated utilizing the attitude of the vehicle [roll$(\phi)$ , pitch$(\theta)$ , and yaw$(\psi)]$ .

The IMU measurements are modeled in the following two equations [25]:

$$\begin{align*} {\overline {\omega }}_{ib}^{b}=&\omega _{ib}^{b} + b_{g}^{b} + w_{g} \tag{2}\\ {\overline {F}}^{b}=&F^{b} + b_{a}^{b} + w_{a} \tag{3}\end{align*}$$ where ${\overline {\omega }}_{ib}^{b}$ is the gyroscope measurement vector (rad/s), $\omega _{ib}^{b}$ is the true gyroscope measurement vector (rad/s), $b_{g}^{b}$ is the gyroscope bias (rad/s) which is assumed as a constant but slightly varying according to a white noise sequence, $w_{g}$ represents the gyroscope sensor noise in vector form (rad/s), ${\overline {F}}^{b}$ is the specific force measurement vector $({\mathrm{ m/s}}^{2})$ , $F^{b}$ is the vehicle’s true specific force vector $({\mathrm{ m/s}}^{2})$ , $b_{a}^{b}$ is the accelerometer bias $({\mathrm{ m/s}}^{2})$ which assumed as a constant but slightly varying according to a white noise sequence, and $w_{a}$ is the accelerometer sensor noise in vector form $({\mathrm{ m/s}}^{2})$ .

A. INS Mechanization in E-Frame

1) Position and Velocity

Accelerometer and gyroscope sensors represent the vehicle’s kinematics model. The time rate of change of the velocity in the e-frame can be described as [27]

$$\begin{equation*} {\dot {V}}^{e} = C_{b}^{e}F^{b} - 2\Omega _{ie}^{e}V^{e} - {\Omega _{ie}^{e}}^{2}P^{e} + G^{e} \tag{4}\end{equation*}$$ where $2\Omega _{ie}^{e}V^{e}$ denotes the Coriolis acceleration, whereas the centrifugal acceleration is represented by ${\Omega _{ie}^{e}}^{2}P^{e}$ . $\Omega _{ie}^{e}$ can be described as $$\begin{equation*} \Omega _{ie}^{e} = \left [{ \omega _{ie}^{e} }\right]^{\times } \tag{5}\end{equation*}$$ where $\omega _{ie}^{e}$ is the angular velocity of Earth with respect to the inertial frame and represented in the e-frame is given as $$\begin{align*} \omega _{ie}^{e} = \begin{bmatrix} 0 \\ 0 \\ \omega _{e} \\ \end{bmatrix}. \tag{6}\end{align*}$$ $[\omega _{ie}^{e}]^{\times }$ is the skew-symmetric matrix of $\omega _{ie}^{e}$ , and $\omega _{e}$ represents the Earth sidereal rotation rate. $G^{e}$ in (4) is the gravitational vector in the e-frame. The velocity can be integrated to find the time rate of change of the position by the following: $$\begin{equation*} {\dot {P}}^{e} = V^{e}. \tag{7}\end{equation*}$$

2) Attitude

The angular rate of the vehicle’s body with respect to the inertial frame, $\omega _{ib}^{b}$ , is measured by the body-fixed and body-aligned gyroscope sensor. The angular velocity of the vehicle with respect to the ECEF frame is calculated as

$$\begin{equation*} \omega _{eb}^{b} = \omega _{ib}^{b} - \omega _{ie}^{b} \tag{8}\end{equation*}$$ where $\omega _{ie}^{b} = C_{e}^{b}\omega _{ie}^{e}$ . Since the Euler approach may lead to singularity, the quaternion approach is used to obtain the attitude of the vehicle [28]. The quaternion represents the attitude between the b-frame and e-frame in the form: $\begin{aligned} q^{e} = \begin{bmatrix} q_{0} & q_{1} & q_{2} & q_{3} \\ \end{bmatrix}^{T} \end{aligned}$ . The dynamics of the quaternion is represented as $$\begin{equation*} {\dot {q}}^{e} = \frac {1}{2}{\overline {\Omega }}_{eb}^{b}q^{e} \tag{9}\end{equation*}$$ where $\begin{aligned} {\overline {\Omega }}_{eb}^{b} = \begin{bmatrix} 0 & - {\omega _{eb}^{b}}^{T} \\ \omega _{eb}^{b} & - [\omega _{eb}^{b}]^{\times } \\ \end{bmatrix} \end{aligned}$ .

SECTION III. Methodology

Using the INS alone, especially with the bias-prone low-cost sensors, leads to large-magnitude errors in the long-term state calculation. Therefore, it is necessary to integrate it with other aiding systems, such as LIDARs, vision systems, global positioning sensors, and encoders. Many fusion techniques can be considered to combine INS with these aiding systems. The most widely used techniques are the KF, the SVSF, and the particle filter (PF), among other extensions of these filters. This work investigates the use of hybrid techniques based on the EKF, the UKF, and the SVSF to obtain a high-accuracy estimation of the state of the vehicle.

A. Error Equation

To reduce the accumulated error in the integrated INS solution, the error between the true state and the propagated state has to be formulated. Estimating this error with time and correcting the propagated state leads to the sought high-accuracy vehicle’s state estimate. In this section, we describe this error state and the techniques used to estimate it.

The dynamics of the error state for e-frame INS can be represented in a matrix format as [27]

$$\begin{align*} \dot {x}=&Ax + Bw \tag{10}\\ \begin{bmatrix} \delta {\dot {P}}^{e} \\ \delta {\dot {V}}^{e} \\ \delta {\dot {q}}^{e} \\ \delta {\dot {b}}_{a}^{b} \\ \delta {\dot {b}}_{g}^{b} \\ \end{bmatrix}=& \begin{bmatrix} 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ C_{b}^{\overline {e}} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & {\frac {1}{2}C}_{b}^{\overline {e}} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & I_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & I_{3 \times 3} \\ \end{bmatrix}\begin{bmatrix} w_{a} \\ w_{g} \\ w_{ba} \\ w_{bg} \\ \end{bmatrix} \\&{} +\begin{bmatrix} 0_{3 \times 3} & I_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ \nabla {\overline {G}}^{e} - {\Omega _{ie}^{e}}^{2} & - 2\Omega _{ie}^{e} & - 2\left [{ C_{b}^{\overline {e}}F^{b} }\right]^{\times } & C_{b}^{\overline {e}} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & {- \Omega }_{ie}^{e} & 0_{3 \times 3} & \frac {1}{2}C_{b}^{\overline {e}} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ \end{bmatrix}\begin{bmatrix} \delta P^{e} \\ \delta V^{e} \\ \delta q^{e} \\ \delta b_{a}^{b} \\ \delta b_{g}^{b} \\ \end{bmatrix} \\{}\tag{11}\end{align*}$$ where $\delta P^{e}$ represents the error in position $P^{e}$ , $\delta V^{e}$ represents the error in velocity $V^{e}$ , $\nabla {\overline {G}}^{e}$ represents the gravity tensor, and $\delta q^{e}$ represents the attitude difference between the e-frame and computed $\overline {e}$ -frame. $b_{g}^{a}$ and $b_{a}^{b}$ represent the bias in the gyroscopes’ and accelerometers’ measurements. Next, techniques to estimate the above error state and therefore correct the propagated estimate of the state are described.

B. Extended Kalman Filter

Like the KF, the EKF filter is a predictor–corrector estimator which is constructed by linearizing the system about the most recent state estimate. The dynamic model is linearized about the posteriori estimate, ${\widehat {x}}_{k|k}$ , while the measurement model is linearized about the a priori estimate, ${\widehat {x}}_{k + 1|k}$ [8], [29]. To describe this as well as the following filter, the discretized form of the dynamic model is first represented as [27]

$$\begin{equation*} x_{k + 1} = Fx_{k} + \Gamma w_{k} \tag{12}\end{equation*}$$ where F is the transition matrix and can be approximated as $F = I + A{\Delta t}$ . $\Gamma$ is the noise gain matrix and can be approximated as $\Gamma = B\Delta t$ . w is the dynamic noise of the system, and its covariance Q can be defined as $$\begin{align*} Q = \begin{bmatrix} I_{3 \times 3}*\sigma _{a}^{2} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & I_{3 \times 3}*\sigma _{g}^{2} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & I_{3 \times 3}*{\sigma _{b}^{a}}^{2} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & I_{3 \times 3}*{\sigma _{b}^{g}}^{2} \\ \end{bmatrix} \\{}\tag{13}\end{align*}$$ where $\sigma _{a}^{2}$ is the covariance of the noise on the acceleration measurement,$\sigma _{g}^{2}$ is the covariance of the noise on the gyroscope measurement, ${\sigma _{b}^{a}}^{2}$ is the covariance of the noise on the acceleration bias state, and ${\sigma _{b}^{g}}^{2}$ is the covariance of the noise on the gyroscope bias state.

The discretized form of the measurement can be described as

$$\begin{align*} z_{k + 1} = \begin{bmatrix} z_{P,k + 1}^{\mathrm {INS}} - z_{P,k + 1}^{\mathrm {GPS}} \\ z_{V,k + 1}^{\mathrm {INS}} - z_{V,k + 1}^{\mathrm {GPS}} \\ \end{bmatrix} = Hx_{k + 1} + v_{k + 1} \tag{14}\end{align*}$$ where H is the observation matrix, and $v_{k + 1}\sim \mathcal {N}(0,\,\,R_{k + 1})$ is the measurement error vector which is assumed a zero mean Gaussian noise with covariance $R_{k + 1}$ . H can be defined as $$\begin{align*} H = \begin{bmatrix} I_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 10} \\ 0_{3 \times 3} & I_{3 \times 3} & 0_{3 \times 10} \\ \end{bmatrix} \tag{15}\end{align*}$$

In general, the EKF process can be summarized as follows [5].

Initialization Process $\rightarrow {\widehat {x}}_{k|k}$ , $P_{k|k}$

Prediction Step:

$$\begin{align*} {\widehat {x}}_{k + 1|k}=&f({\widehat {x}}_{k|k}) \rightarrow F_{k} = \frac {\partial f}{\partial x}|_{x = {\widehat {x}}_{k|k}} \tag{16}\\ {\widehat {z}}_{k + 1|k}=&h({\widehat {x}}_{k + 1|k}) \rightarrow H_{k + 1} = \frac {\partial h}{\partial x}|_{x = {\widehat {x}}_{k + 1|k}} \tag{17}\\ e_{z_{k + 1|k}}=&z_{k + 1} - {\widehat {z}}_{k + 1|k} \tag{18}\\ P_{k + 1|k}=&F_{k} P_{k|k}F_{k}^{T} + Q_{k} \tag{19}\end{align*}$$

Update Step:

$$\begin{align*} S_{k + 1}=&H_{k + 1}P_{k + 1|k}H_{k + 1}^{T} + R_{k + 1} \tag{20}\\ K_{k + 1}=&P_{k + 1|k}H_{k + 1}^{T}S_{k + 1}^{- 1} \tag{21}\\ P_{k + 1|k + 1}=&P_{k + 1|k} - K_{k + 1}S_{k + 1}K_{k + 1}^{T} \tag{22}\\ {\widehat {x}}_{k + 1|k + 1}=&{\widehat {x}}_{k + 1|k} + K_{k + 1}e_{z_{k + 1|k}} \tag{23}\end{align*}$$ where in the prediction step, $\mathbf {P}_{k + 1|k}$ is the a priori state estimate’s covariance and $\mathbf {H}_{k + 1}$ is the linearized observation matrix around the most recent posteriori estimate. In the update step, $\mathbf {S}_{k + 1}$ is the innovation covariance, $\mathbf {K}_{k + 1}$ is the Kalman gain, and $\mathbf {P}_{k + 1|k + 1}$ is the posteriori state estimate’s covariance matrix. $e_{z_{k + 1|k}}$ and $e_{z_{k|k}}$ are the innovation error for the a priori error at time $k + 1$ and the a posteriori error at time $k + 1$ , respectively. $\mathbf {f}$ and $\mathbf {h}$ are the nonlinear function of the model dynamics and measurement equation, respectively, and their linearized form is $\mathbf {F}$ and $\mathbf {H}$ , respectively.

C. Smooth Variable Structure Filter

The SVSF is another predictor–corrector estimator formulated based on the sliding-mode concept and variable structure theory [13], [30], [31], [32], [33]. This filter has been established to improve the robustness to modeling uncertainty by forcing the estimated trajectory to vary within the smoothing boundary layer (SBL), $\psi$ . The existing substance, $\beta$ , represents the amount of uncertainty in the system. SBL is an unknown parameter, and its value can be assigned based on a design criterion.

If the SBL is smaller than the existence subspace, $\beta$ , then chattering happens (see Fig. 1). This chattering makes the filter robust to model uncertainty. However, it simultaneously increases the filter’s sensitivity to the system noise, causing the accuracy of the estimator to reduce as well. On the other hand, if the SBL is larger than $\beta$ , the filter’s sensitivity to the system noise and the correction gain will be reduced (see Fig. 2). However, the stability of the system will be reduced. Hence, the width of the SBL must be carefully chosen. In Figs. 1 and 2, the true trajectory is marked in blue color, the estimated trajectory is marked in thin black color, the smoothing subspace is marked in dashed–dotted red, and the existence subspace is marked in thick black color. More on the design of SBL is described in the below sections.

FIGURE 1.

SVSF chattering effect ${(}\psi \lt \beta {)}$ .

FIGURE 2.

SVSF smoothed trajectory ${(}\psi > \beta {)}$ .

The formulation of the SVSF is similar to the EKF. The only difference is in the calculation of the gain factor, K. This gain factor can be calculated as

$$\begin{equation*} K_{K + 1} = H_{k + 1}^{+}{\mathrm{ diag}} \left [{ A \circ \mathrm {sat} \left ({\overline {\psi },e_{z_{k + 1|k}} }\right)\mathrm {diag}({e_{z_{k + 1|k}}})^{- 1}}\right] \tag{24}\end{equation*}$$ where $A = \,\,(| e_{z_{k + 1|k}} | + \,\,\gamma \,\,| e_{z_{k|k}} |)$ , $H^{+}$ is the pseudo inverse of the observation matrix, $\gamma$ is the convergence rate with values between 0 and 1, $\circ$ is the Schur-Product (element-by-element multiplication), $\overline {\psi }$ is the diagonalized SBL, and sat is the saturation function calculated as $$\begin{align*} \mathrm {sat} \left ({a,b }\right) = \begin{Bmatrix} \frac {b}{a} & \left |{ b }\right | \leq \left |{ a }\right | \\ \mathrm {sign}(b) & \left |{ b }\right | > \left |{ a }\right | \\ \end{Bmatrix}. \tag{25}\end{align*}$$

D. Smoothing Boundary Layer

The SBL can be constant [13] or variable [34]. The constant SBL is an $n \times n$ diagonal matrix with off-diagonal elements equal to zero, as shown in (26). The diagonal elements can be assigned based on design criteria [35].

$$\begin{align*} \psi = \begin{bmatrix} \psi _{1} & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & \psi _{m} \\ \end{bmatrix} \tag{26}\end{align*}$$ In the case of variable SBL, the SBL is formulated as a function of $e_{z_{k|k}}$ , $e_{z_{k + 1|k}}$ , convergence rate $(\gamma)$ , a priori state covariance matrix $P_{k + 1|k}$ , linearized measurement matrix H, and the innovation covariance matrix $S_{k + 1}$ as $$\begin{equation*} \psi ^{- 1} = {\mathrm{ diag(A)}}^{- 1}H_{k + 1}P_{k + 1|k}H_{k + 1}^{T}{S_{k + 1}}^{- 1}. \tag{27}\end{equation*}$$

This gives a possibly fully populated matrix as

$$\begin{align*} \psi = \begin{bmatrix} \psi _{11} & \cdots & \psi _{1m} \\ \vdots & \ddots & \vdots \\ \psi _{m1} & \cdots & \psi _{mm} \\ \end{bmatrix}. \tag{28}\end{align*}$$

These equations are used to examine the quality of the innovation signal, and then decide which filter will operate in the hybrid filters.

E. Unscented Kalman Filter

The UKF is formulated based on unscented transformation and sigma points [10]. This filter is more reliable and accurate in highly nonlinear systems than the EKF. This is because the UKF eliminates the Jacobian linearization term in the EKF. However, the UKF is computationally more complex than the EKF since its formulation contains $2L + 1$ sigma points, where L is the dimension of the state. The UKF formulation can be described as follows.

Initialization $\rightarrow {\widehat {x}}_{0|0}$ , $P_{0|0}$ when $k = 0$

Prediction Stage:

$$\begin{align*} X_{i,k|k}=& {\widehat {x}}_{k|k} \pm \left ({\sqrt {(L + c)P_{k|k}} }\right)_{i} {\mathrm{ for}} i = 0,\ldots, 2L \tag{29}\\ {\widehat {X}}_{i_{k + 1|k}}=&\widehat {f}\left ({{\widehat {x}}_{k|k} }\right) \tag{30}\\ W_{i}=&\frac {1}{2(L + c)} \tag{31}\\ {\widehat {x}}_{k + 1|k}=&\sum _{i = 0}^{2L}W_{i}{\widehat {X}}_{i_{k + 1|k}} \tag{32}\\ P_{k + 1|k}=&\sum _{i = 0}^{2L}{W_{i}\left ({{\widehat {X}}_{i_{k + 1|k}} - {\widehat {x}}_{k + 1|k} }\right)\left ({{\widehat {X}}_{i_{k + 1|k}} - {\widehat {x}}_{k + 1|k} }\right)^{T}} + Q_{k} \\{}\tag{33}\end{align*}$$ where ${\widehat {X}}_{i}$ is the sigma points, $W_{i}$ is the weight factor corresponding to the ith sigma point, $(\alpha)_{i}$ is the ith row of the matrix $\alpha$ and for I with a value of zero it is equal to zero, and c is a design value (usually $< 1$ ).

The updated stage of the filter is described as

Update Stage:

$$\begin{align*} {\widehat {Z}}_{i_{k + 1|k}}=&\widehat {h}\left ({{\widehat {x}}_{k + 1|k} }\right) \tag{34}\\ {\widehat {z}}_{k + 1|k}=&\sum _{i = 0}^{2L}W_{i}{\widehat {Z}}_{i_{k + 1|k}} \tag{35}\\ { P}_{zz}=&\sum _{i = 0}^{2L}{W_{i}\left ({{\widehat {Z}}_{i_{k + 1|k}} - {\widehat {z}}_{k + 1|k} }\right)\left ({{\widehat {Z}}_{i_{k + 1|k}} - {\widehat {z}}_{k + 1|k} }\right)^{T}} + R_{k} \\{}\tag{36}\\ { P}_{xz}=&\sum _{i = 0}^{2L}{W_{i}\left ({{\widehat {X}}_{i_{k + 1|k}} - {\widehat {x}}_{k + 1|k} }\right)\left ({{\widehat {Z}}_{i_{k + 1|k}} - {\widehat {z}}_{k + 1|k} }\right)^{T} } \tag{37}\\ K_{k}=&{ P}_{xz}{ P_{zz}}^{- 1} \tag{38}\\ {\widehat {x}}_{k + 1|k + 1}=&{\widehat {x}}_{k + 1|k} + { P}_{xz}{ P_{zz}}^{- 1}\left ({z_{k} - {\widehat {z}}_{k + 1|k} }\right) \tag{39}\\ P_{k + 1|k + 1}=&\left ({P_{k + 1|k} - {P}_{xz}{ P_{zz}}^{- 1}{ P}_{xz} }\right) \tag{40}\end{align*}$$ where ${\widehat {Z}}_{i_{k + 1|k}}$ is the measurement predicted corresponding to the ith sample point, ${\widehat {z}}_{k + 1|k}$ is the nonlinear propagated measurement, h is the nonlinear measurement vector, ${ P}_{zz}$ is the measurement covariance matrix, and ${ P}_{xz}$ is the cross-covariance between state estimate and measurement.

F. UK-SVSF

The UK-SVF is a method designed to improve the accuracy of estimations in the presence of dynamic and measurement noise while also being resilient to model uncertainty. The SVSF alone enhances the robustness of the estimator to modeling uncertainty and smooths out the estimated trajectory. However, the presence of dynamic and measurement noise can cause inaccuracies and make the solution unreliable, as noted in [30] and [36]. In [36] and [37], it has been proven that merging UKF with SVSF gave better performance than utilizing other forms like KF/EKF with SVSF. Therefore, we are not using them in this research unlike [38], where they suggested a very complicated framework of the interacting multiple model (IMM) estimator for switching between the KF and SVSF. Fig. 3 illustrates the flowchart of the hybrid filter, which is used to process the dynamics of a vehicle. The input to all filters is the data obtained from the IMU. Moreover, this figure shows how GPS measurements are used in the update stage of the filter.

FIGURE 3.

Methodology for combining the nonlinear strategies [30–31].

As shown in Fig. 1, after the predicted states are obtained, the variable SBL is calculated using (27). Also, a fixed SBL is selected based on design criteria (prior knowledge). If the variable SBL is greater than the fixed SBL, then the predicted states must be updated based on the SVSF formulation. Otherwise, they must be updated based on UKF formulation.

In the UK-SVSF formulation, the initial state and its corresponding covariance can be initialized as ${\widehat {x}}_{k|k}$ and $P_{k|k}$ , respectively, then the priori state estimate, its corresponding covariance, the a priori measurement, the innovation, and its corresponding matrix can be calculated using (29)–​(37). Subsequently, the fixed SBL $(\psi _{\mathrm {fixed}})$ is assigned based on design criteria (prior knowledge). The variable SBL ($\psi _{vbl}$ ) can be obtained using (27). If $\psi _{vbl} > \psi _{\mathrm {fixed}}$ (any term in the diagonal of $\psi _{vbl}$ is larger than its corresponding term in $\psi _{\mathrm {fixed}}$ ) then the SVSF gain factor in (24) will be chosen to update the state and the covariance matrix is initialized again. Otherwise, the UKF gain factor (38) will be chosen to update the state, and finally, the posteriori state estimate, its corresponding covariance, and posteriori estimated measurement can be calculated as in (38)–(40).

SECTION IV. Experimental Verification

A vehicle was driven around the campus of the American University of Sharjah to verify the performance of the proposed filtering techniques. The vehicle is equipped with MIDG IIC and GPS antenna sensors [26]. The experimental setup and testing site are shown in Fig. 4. The length of the testing path ranged between 1.3 and 4 km in periods of 4–5 min. The loosely coupled INS/GPS integration is employed to fuse the IMU and GPS data. The acquired results are compared and validated against the COTs MIDG solution. The IMU is sampled at 50 Hz. However, the GPS is sampled at 5 Hz. The MIDG specifications are shown in Table 2. In the experimental tests, $R_{k + 1}$ in (14) is assumed as a stationary $6 \times 6$ diagonal matrix.

TABLE 2 MIDG IIC Specifications [26]

FIGURE 4.

Experimental testing. (a) MIDG II unit with its body frame axes. (b) Experimental setup. (c) Experimental site.

A. Experimental Results

In this section, the proposed methods are experimentally verified through several scenarios. First, the algorithms are tested in an ideal scenario where no GPS outage or additional measurement or dynamics noise are induced to the signals. Subsequently, GPS outage is intentionally simulated, and the comparative performance of the various algorithms is analyzed. Additionally, the algorithms are further tested when adding measurements noise of various magnitudes to simulate GPS sensor’s aging and/or signal multipath. Finally, a scenario corresponding to increased IMU noise is also simulated. This scenario might result from low-cost IMU’s aging that onset a fault in the measurements under harsh vehicle’s operating conditions.

1) First Scenario

In this scenario, the GPS data is available all time. Additional noise or additional uncertainties are not added to the system, making this the ideal scenario. Fig. 5 demonstrates the INS/GPS sensor fusion results based on the EKF, SVSF, UKF, and UK-SVSF, respectively. As shown in the figure, all the solutions have similar results in converging to the truth path (taken as the MIDG solution). Furthermore, the mean and the standard deviation (STD) of the error (between estimated solutions and MIDG solution) and the maximum absolute error (MAE) in the position and the velocity in all five filters have been calculated as shown in Tables 3–​5.

TABLE 3 Mean and STD Comparison in Position

TABLE 4 Mean and STD Comparison in Velocity

TABLE 5 Position and Velocity MAE Comparison

FIGURE 5.

INS mechanization fused with GPS based on EKF, SVSF, UKF, and UK-SVSF.

Based on these results and as summarized numerically in Tables 3–​5, it is concluded that all filters have almost the same performance in terms of mean and STD. This is the case as the filter is operating in near-ideal conditions, without being affected by high-measurement noise, model-uncertainty, or GPS outages. EKF’s performance is not as good compared to the rest of the filters since the system is nonlinear, and as explained earlier, this nonlinearity adversely affects the performance of the EKF.

2) Second Scenario

In urban canyon environments, GPS data is not always available. If the moving platform is near a tall building, or if less than four satellites are in the sight of the GPS receiver, then the receiver’s measurements are either poor or unavailable.

In this scenario, the GPS data were made unavailable between 6000 and 8000 epochs. In this environment, the performance of the fusion techniques is compared against the true path following, error mean, error STD, and MAE of the position and the velocity estimates. Fig. 6 shows the estimate of the vehicle’s path based on the different fusion techniques. The mean, STD, and MAE results are calculated in Tables 5–​7. Fig. 6 shows that the UK-SVSF is the best filtering strategy since it has the least deviation from the true path in the event where the GPS data is absent for some duration. The UKF has similar performance, however, it has slightly a bigger error mean and STD compared to the UK-SVSF. The SVSF has almost the same performance with more errors compared to the UKF and UK-SVSF. On the other hand, EKF has the worst performance since the GPS missed data will add to the nonlinearity problem of the system. The statistical results shown in Tables 6–​8 confirm the result that the UK-SVSF is the filter with the least error in terms of mean, STD, and MAE.

TABLE 6 Position Error Mean and STD When GPS Data is Not Available Between 6000 and 8000 Epochs

TABLE 7 Velocity Error Mean and STD When GPS Data is Not Available Between 6000 and 8000 Epochs

TABLE 8 MAE Comparison in Position (GPS Data is Not Available Between 6000 and 8000 Epochs)

FIGURE 6.

INS/GPS sensor fusion based on EKF, UKF, SVSF, and UK-SVSF (GPS removed in one period).

3) Third Scenario

In this scenario, the navigation problem was made harder by blocking GPS data during two periods (2500–4000 and 6000–8000 epochs) and increasing the measurement noise STD by 5% of the absolute maximum GPS position and velocity measurements. The comparative performance of the fusion algorithms is shown in this section. Based on Fig. 7, as the noise is added to the system, the performance of the SVSF decreases. As explained in the previous chapters, SVSF is only robust to model uncertainty, and in the presence of noise, its performance degrades. However, UKF and UK-SVSF are the best filters to handle higher amounts of noise without deviating from the true path.

FIGURE 7.

INS/GPS sensor fusion based on EKF, UKF, SVSF, and UK-SVSF (GPS removed in two periods of time with 5% added measurement noise).

Tables 9–​11 summarize the filters’ performance in terms of the estimation error mean, STD, and MAE in the vehicle’s position and velocity. As predicted, the UKF and UK-SVSF outperform the rest of the filters.

TABLE 9 Position Error Mean and STD When GPS is Not Available in Two Periods of Time With 5% Added Measurement Noise

TABLE 10 Velocity Error Mean and STD When GPS Outage in Two Periods of Time With 5% Noise

TABLE 11 MAE Comparison in Position (GPS Outage in Two Periods and 5% Noise is Added)

4) Fourth Scenario

In this scenario, 10% measurement noise is added while having access to GPS data at all times. Fig. 8(a) and (b) as well as Tables 12–​14 summarize the performance of the filters.

TABLE 12 Position Error Mean And STD With 10% Noise in the Measurements

TABLE 13 Velocity Error Mean and STD With 10% Noise in the Measurements

TABLE 14 MAE With 10% Added Noise in the Measurements

FIGURE 8.

INS/GPS sensor fusion based on EKF, UKF, SVSF, and UK-SVSF with 10% added noise. (a) Full path. (b) Zoomed path of the yellow circle.

As shown in Fig. 8(a) and (b), similar to the previous configuration, UK-SVSF outperforms the rest of the filters, followed by the UKF. This is mainly due to the UKF’s ability to reduce the effect of the nonlinearity in the system as well as the ability of the SVSF to handle the possible existence of model uncertainty. It can be also seen that EKF’s performance is better than the SVSF’s performance as it is more robust to noise (see Tables 12–​14). Therefore, the UK-SVSF, which has the advantage of both SVSF and UKF, is recommended for GPS/IMU state estimation as it has a low magnitude of error mean, STD, and MAE.

5) Fifth Scenario

In this scenario, 40% of uncertainty is added to the IMU data that is used to obtain the dynamic model of the system. The uncertainties are injected into the kinematic model where the specific force $(F^{b})$ and the angular velocity $(\omega _{ib}^{b})$ appear in the formulation. The amount of uncertainties injected into the model is 40% of the actual value of the specific force and angular velocity. The GPS data was available during the whole path. The comparison is similar to the previous sections. The filter performance is shown in Fig. 9 and Tables 15–​17.

TABLE 15 Position Error Mean and STD With 40% Uncertainty in IMU

TABLE 16 Velocity Error Mean and STD With 40% IMU Uncertainty

TABLE 17 Position and Velocity MAE With 40% IMU Uncertainty

FIGURE 9.

INS/GPS sensor fusion based on EKF, UKF, SVSF, and UK-SVSF with 40% added IMU uncertainty. (a) Full path. (b) Zoomed path of the yellow circle.

Fig. 9(a) and (b) as well as Tables 15–​17 show that the UK-SVSF has the best performance as it is robust to model uncertainty. Also, the results of Tables 15 and 17 show that the SVSF and UK-SVSF have the least MAE. The reason why the SVSF performs well in this case is that high-magnitude dynamics and measurement noise were not added to the system.

6) Sixth Scenario

In this scenario, with the presence of uncertainties, 10% noise is added to the GPS data making this a depiction of the worst-case scenario. Filter performance comparisons are done in the absence of GPS data over two time periods (2500–4000 and 6000–8000 epochs), while simultaneously adding uncertainties and noise to the system.

Based on the results of Fig. 10, when both additional noise and additional uncertainty are present, the UK-SVSF has a better performance compared to noncombined filters (EKF, SVSF, and UKF). However, the UK-SVSF is the best filter in truth path following. That is, due to UKF having a better performance when compared to the EKF in being robust to uncertainty and system noise. Furthermore, EKF’s performance is lowered as it experiences overshooting in the parts where GPS data is removed.

FIGURE 10.

INS/GPS sensor fusion based on EKF, UKF, SVSF, and UK-SVSF (with 10% added IMU uncertainty, 10% added measurement noise, and GPS removed in two periods of time).

Tables 18–​20 are used to compare the performance of filters in terms of mean, STD, and MAE in the position and velocity. Based on these results, UK-SVSF is the best filter in terms of the mean, STD, and MAE, since it has the benefits of both fast convergence to the truth path due to the SVSF gain factor and being smooth in the presence of noise due to the UKF gain factor.

TABLE 18 Position Error Mean and STD With 10% IMU Uncertainty,10% Added Measurement Noise, and GPS Removed in Two Periods of Time

TABLE 19 Velocity Error Mean and STD With 10% IMU Uncertainty, 10% Added Measurement Noise, and GPS Removed in Two Periods of Time

TABLE 20 MAE of Position and Velocity With 10% IMU Uncertainty, 10% Added Measurement Noise, and GPS Removed in Two Periods of Time

7) Seventh Scenario

A new path is examined with a new average speed of around 50 km/h. (In order not to exceed the maximum speed in the vicinity of the AUS campus.) In this scenario, 40% of uncertainty is added to the IMU data that is used to obtain the dynamic model of the system. Moreover, the measurements were injected with 5% extra measurement noise. The filter performance is shown in Fig. 11 and Tables 21–​23.

TABLE 21 Position Error Mean and STD With 40% IMU Uncertainty and 5% Added Measurement Noise

TABLE 22 Velocity Error Mean and STD With 40% IMU Uncertainty and 5% Added Measurement Noise

TABLE 23 MAE of Position and Velocity With 40% IMU Uncertainty and 5% added Measurement Noise

FIGURE 11.

INS/GPS sensor fusion based on EKF, UKF, SVSF, and UK-SVSF (with 40% added IMU uncertainty and 5% added measurement noise).

The results show that the UK-SVSF solution is robust to both model uncertainty and added dynamics and measurements noise.

The above seven experimental scenarios thoroughly verified the accuracy and integrity of the proposed method. Next, concluding remarks will be given.

SECTION V. Conclusion

This article addressed the problem of state estimation of ground vehicles utilizing GPS/IMU sensors under practical, nonideal environments. A filter is proposed to ensure high-accuracy estimation of the vehicle’s state under possible model uncertainty and under additive dynamics and measurement noise sequences. The filter is a hybrid technique that utilizes a UKF and SVSF in a combined filter, namely, the UK-SVSF. The hybrid filter is implemented and experimentally compared against three filtering strategies: 1) EKF; 2) SVSF; and 3) UKF. The performance of the filters is validated and compared in seven different scenarios. Based on the experimental results, the proposed UK-SVSF method is seen to outperform the other fusion techniques for the following reasons. First, it eliminates the linearization that is utilized in the EKF formulation, causing the error in the estimation to be minimized. Second, the proposed filter provides a robust solution to dynamic and measurement noise in the system through the formulation of the UKF. Finally, by using the SVSF, the hybrid filter minimizes the effect of possible modeling uncertainty in the system.

The optimality and robustness of the proposed algorithm come with a small increase in computational cost. Both the EKF and the UKF are of the same order or computational complexity; order of L³. The increase in the computational cost of the UKF in comparison to the EKF is due to the use of the sigma points. However, this does not create a big burden to the system in view of the capability of current embedded systems. The computational complexity of the SVSF is minimal in comparison to the UKF as it does not include the update computation of covariance matrices nor the usage of sigma points.

Future work will examine the utilization of AI methods such as the gradient descent or genetic algorithms to optimally tune filter parameters and maximize the system’s performance.