Robust Adaptive Multiple Backtracking VBKF for In-Motion Alignment of Low-Cost SINS/GNSS

Abstract

The low-cost Strapdown Inertial Navigation System (SINS)/Global Navigation Satellite System (GNSS) is widely used in autonomous vehicles for positioning and navigation. Initial alignment is a critical stage for SINS operations, and the alignment time and accuracy directly affect the SINS navigation performance. To address the issue that low-cost SINS/GNSS cannot effectively achieve rapid and high-accuracy alignment in complex environments that contain noise and external interference, an adaptive multiple backtracking robust alignment method is proposed. The sliding window that constructs observation and reference vectors is established, which effectively avoids the accumulation of sensor errors during the full integration process. A new observation vector based on the magnitude matching is then constructed to effectively reduce the effect of outliers on the alignment process. An adaptive multiple backtracking method is designed in which the window size can be dynamically adjusted based on the innovation gradient; thus, the alignment time can be significantly shortened. Furthermore, the modified variational Bayesian Kalman filter (VBKF) that accurately adjusts the measurement noise covariance matrix is proposed, and the Expectation–Maximization (EM) algorithm is employed to refine the prior parameter of the predicted error covariance matrix. Simulation and experimental results demonstrate that the proposed method significantly reduces alignment time and improves alignment accuracy. Taking heading error as the critical evaluation indicator, the proposed method achieves rapid alignment within 120 s and maintains a stable error below 1.2° after 80 s, yielding an improvement of over 63% compared to the backtracking-based Kalman filter (BKF) method and over 57% compared to the fuzzy adaptive KF (FAKF) method.

1. Introduction

The Strapdown Inertial Navigation System (SINS), with its all-weather autonomy and ability to provide attitude, velocity, and position information, offers key advantages for independent navigation [1,2,3,4]. Owing to these features, it has been widely applied in autonomous driving, autonomous underwater vehicles (AUVs), unmanned aerial vehicles, Internet of Things (IoT), and smart cities [5]. SINS calculates the vehicle’s attitude, velocity, and position by processing raw inertial sensor measurements [6]. Initial alignment is critical for subsequent navigation performance as it determines the carrier’s initial attitude accuracy [7,8,9,10,11]. Initial alignment can be divided into two basic types based on the carrier’s motion state: static alignment and in-motion alignment [12]. However, because static alignment becomes ineffective in dynamic environments, in-motion alignment methods have been proposed to enable real-time alignment during continuous maneuvers [13,14,15,16].

To address the requirements of in-motion alignment, Wu et al. [17] proposed an Optimization-Based Alignment (OBA) method that transforms the attitude determination problem into a Wahba problem to make full use of observation vectors. Based on this work, Wu et al. [18] proposed a GPS-aided coarse alignment method based on velocity vector integration, which suppresses random noise in the alignment process to a certain extent. However, when the GPS-aided information contains a large number of outliers, such as in complex urban environments where tall buildings are densely distributed, forests are obscured, and there are tunnels or elevated bridges, the performance of the above-mentioned methods is severely limited. In [5], a Doppler velocity log (DVL)-aided in-motion alignment method based on Huber’s robust theory was designed to reduce the effect of outliers. Xu et al. [19] proposed a robust attitude determination method to accomplish the in-motion alignment process when GPS output contains outliers. However, these methods are only applicable to high-accuracy SINS, and the outlier isolation method for low-cost SINS remains to be studied.

To shorten the time of in-motion alignment, researchers have proposed backtracking alignment methods that artificially extend navigation data, achieving rapid convergence of alignment results [20,21,22,23]. Chang et al. [24] proposed a fast initial alignment method based on a backtracking integration procedure, which can reuse the inertial measurement unit (IMU) and GPS information. Fu et al. [25] proposed a backtracking-based alignment method that reuses stored sensor data for navigation updates and Kalman filtering in the geographic frame, which simplifies the algorithm and improves alignment accuracy. However, the standard backtracking method may not be able to fully utilize the information in the navigation data in complex environments. To improve the utilization of navigation data, Lin et al. [26] proposed a high-accuracy backtracking alignment method that estimates and compensates for transient acceleration errors. Wei et al. [27] proposed an improved alignment method based on backtracking navigation, which fully considered the low-cost inertial sensor biases and corrected the correlated errors to make full use of the limited observation information. However, the aforementioned methods do not consider that the outdated historical data may no longer reflect current noise characteristics, thereby degrading the subsequent estimation performance [28,29,30].

Furthermore, to accomplish in-motion alignment, it is essential to develop novel filtering methods to accurately estimate the sensor biases and correlated errors. Existing alignment methods usually estimate unknown parameters by using the Kalman filter (KF), which assumes that the sensor noise follows a Gaussian distribution [31,32,33,34,35,36]. However, affected by outlier interference, the noise distribution of measured values provided by the sensors is often non-Gaussian noise. In order to improve the performance of the filter, Meng et al. [37] proposed a dual-input interval type-2 fuzzy inference system that allows for real-time adjustments of the measurement noise covariance matrix using fuzzy adjustment strategies. Shi et al. [38] proposed an adaptive Kalman filter to suppress the adverse effects of outliers and approximate the unknown measurement noise covariance matrix using the variational Bayesian (VB) method. Huang et al. [39] proposed a novel VB Kalman filter to address deviations from Gaussian noise distributions, which estimates the varying measurement noise covariance matrix by manually selecting prior parameters. However, the manual selection of prior parameters may be inaccurate in some cases, and the severe maneuvers of vehicles can cause the sensor noise to exhibit heavy-tailed characteristics in complex environments, which leads to a significant decline in the performance of the filters.

To address the above problems, this paper proposes a novel adaptive multiple backtracking robust in-motion alignment method. The calculation flow of the proposed method is illustrated in Figure 1. Specifically, a linear state-space model is established, incorporating GNSS-aided information to estimate gyroscope constant biases and the equivalent rotation vector between the true and calculated body frames. By reconstructing the observation vector using residual information, outlier detection and isolation within the GNSS-aided data are performed. To shorten the alignment time, a multiple backtracking strategy combined with an adaptive window adjustment approach is developed to make full use of the navigation data and mitigate the effect of noise with varying statistical characteristics. An Expectation–Maximization-based robust Kalman filter is employed to address situations where system noise deviates from the Gaussian distribution, thereby improving the estimation accuracy of the measurement noise covariance matrix. The main contributions of this paper are as follows:

• Observation and reference vectors are constructed using the sliding window method to mitigate the accumulation of sensor errors. A weighting function is designed using a magnitude matching method to reconstruct the observation vector, enabling the detection and isolation of outliers.
• An adaptive window adjustment multiple backtracking approach is proposed to effectively utilize navigation data and significantly shorten alignment time in complex environments. This approach intelligently determines the size of the backtracking window based on an innovation gradient, which effectively improves alignment accuracy.
• An EM-based robust variational Bayesian Kalman filter is designed to accurately estimate the alignment errors in complex interference environments. Within the proposed technique, the EM and VB methods are employed to estimate the inaccurate predicted error and measurement noise covariance matrices, respectively.

The remainder of this paper is organized as follows. Section 2 describes an in-motion alignment model for the SINS that suppresses the interference of outliers. Section 3 illustrates the adaptive multiple backtracking EM-based robust variational Bayesian Kalman filter scheme in detail. Section 4 compares the alignment performance of the proposed method with those of the existing typical methods through simulations and field tests. Finally, conclusions are drawn in Section 5. The important symbols used throughout the study are summarized in

Table 1. Nomenclature.

Notations	Definitions
$i$	Inertial frame
$n$	Navigation frame
$b$	True body frame
$\tilde{b}$	Calculated body frame
${\mathit{g}}^n$	Gravity vector
${\mathit{f}}^b$	Specific force
${\mathit{\epsilon }}^b$	Gyroscope constant biases
${\mathit{C}}_b^a$	Attitude matrix from $b$ frame to $a$ frame
${\mathit{\omega }}_{bc}^a$	Angular rate of $b$ frame related to $c$ frame projected in $a$
${\mathit{V}}^n$	Ground velocity of the SINS in $n$ frame
${\mathit{I}}_3$	$3\times 3$ identity matrix
$(\mathit{a})\times$	Skew symmetric matrix of vector $\mathit{a}$
${(\cdot )}^T$	Transpose operation
$tr(\cdot )$	Trace operation

.

2. In-Motion Alignment Model

2.1. Traditional In-Motion Alignment Method

According to the chain rule for attitude matrices, the time-varying attitude matrix ${\mathit{C}}_b^n(t)$ can be decomposed into the product of three attitude matrices, and it can be written as follows:

$${\mathit{C}}_b^n(t)={\left({\mathit{C}}_{n(t)}^{n(0)}\right)}^T{\mathit{C}}_{b(0)}^{n(0)}{\mathit{C}}_{b(t)}^{b(0)}$$

(1)

where ${\mathit{C}}_{n(t)}^{n(0)}$ represents the attitude matrix of the navigation frame from the current time to the initial time. ${\mathit{C}}_{b(t)}^{b(0)}$ represents the attitude matrix of the body frame from the current time to the initial time. ${\mathit{C}}_{b(0)}^{n(0)}$ is the constant attitude matrix that transforms from the initial body frame to the initial navigation frame.

The two time-varying attitude matrices ${\mathit{C}}_{n(t)}^{n(0)}$ and ${\mathit{C}}_{b(t)}^{b(0)}$ in (1) can be obtained by integrating the following differential equations:

$${\dot{\mathit{C}}}_{n(t)}^{n(0)}={\mathit{C}}_{n(t)}^{n(0)}\left({\mathit{\omega }}_{in}^n(t)\times \right)$$

(2)

$${\dot{\mathit{C}}}_{b(t)}^{b(0)}={\mathit{C}}_{b(t)}^{b(0)}\left({\mathit{\omega }}_{ib}^b(t)\times \right)$$

(3)

where ${\mathit{\omega }}_{in}^n(t)$ is determined based on the carrier’s current geographic location, ${\mathit{\omega }}_{ib}^b(t)$ is obtained from the gyroscope output, and $(\cdot )\times$ represents the corresponding vector’s skew-symmetric matrix form.

The constant attitude matrix ${\mathit{C}}_{b(0)}^{n(0)}$ is obtained by appropriately manipulating the specific-force equation, thereby transforming the attitude determination problem into a Wahba problem. Solving the Wahba problem subsequently yields the optimal estimation of this matrix. The specific-force equation is expressed as

$${\dot{\mathit{V}}}^n={\mathit{C}}_{n(0)}^{n(t)}{\mathit{C}}_{b(0)}^{n(0)}{\mathit{C}}_{b(t)}^{b(0)}{\mathit{f}}^b-\left(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n\right)\times {\mathit{V}}^n+{\mathit{g}}^n$$

(4)

By integrating both sides of (4) and performing the necessary rearrangements, the relationship can be expressed in terms of the reference vector ${\mathit{\alpha }}_{\nu }$ and the observation vector ${\mathit{\beta }}_{\nu }$ as follows:

$${\mathit{\beta }}_{\nu }={\mathit{C}}_{b(0)}^{n(0)}{\mathit{\alpha }}_{\nu }$$

(5)

where

$$\begin{array}{l}{\mathit{\alpha }}_{\nu }={{\displaystyle \int }}_0^t{\mathit{C}}_{b(t)}^{b(0)}{\mathit{f}}^{b}dt\\ {\mathit{\beta }}_{\nu }={{\displaystyle \int }}_0^t{\mathit{C}}_{n(t)}^{n(0)}{\dot{\mathit{V}}}^{n}dt+{{\displaystyle \int }}_0^t{\mathit{C}}_{n(t)}^{n(0)}(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n)\times {\mathit{V}}^{n}dt-{{\displaystyle \int }}_0^t{\mathit{C}}_{n(t)}^{n(0)}{\mathit{g}}^{n}dt\end{array}$$

(6)

At this stage, the determination of ${\mathit{C}}_{b(0)}^{n(0)}$ is reformulated as a Wahba problem. The corresponding performance metric $W*$ is defined as follows:

$$W*={\displaystyle \frac{1}{2}}{\displaystyle \sum }_{i=1}^k{\left|\mathit{\beta }-{\mathit{C}}_{b(0)}^{n(0)}\mathit{\alpha }\right|}^2$$

(7)

In (7), the optimal constant attitude matrix ${\mathit{C}}_{b(0)}^{n(0)}$ is obtained by solving this problem via the Singular Value Decomposition (SVD) method.

However, the traditional alignment methods described above employ full-integration formulations, causing sensor errors to accumulate continuously from the initial epoch and thereby reducing alignment accuracy. Outliers in the GNSS measurements further reduce the accuracy of ${\mathit{\beta }}_{\nu }$ and may even cause the failure of the alignment process. Given the limitations of traditional methods, exploring new outlier isolation approaches is essential.

2.2. Outliers’ Isolation Based on Vector Reconstruction

Sliding-window-based vectors are designed to reduce the sensor error accumulation present in traditional full integration. The corresponding formulations are given as follows:

$$\begin{array}{ll}{\mathit{\alpha }}_{sw}& ={\mathit{\alpha }}_{\mathit{v},t}-{\mathit{\alpha }}_{\mathit{v},s}\\ & ={{\displaystyle \int }}_0^t{\mathit{C}}_{b(\tau )}^{b(0)}{\mathit{f}}^{b(\tau )}d\tau -{{\displaystyle \int }}_0^s{\mathit{C}}_{b(\tau )}^{b(0)}{\mathit{f}}^{b(\tau )}d\tau \\ & ={{\displaystyle \int }}_s^t{\mathit{C}}_{b(\tau )}^{b(0)}{\mathit{f}}^{b(\tau )}d\tau \end{array}$$

(8)

$$\begin{array}{ll}{\mathit{\beta }}_{sw}& ={\mathit{\beta }}_{\nu ,t}-{\mathit{\beta }}_{\nu ,s}\\ & ={{\displaystyle \int }}_s^t{\mathit{C}}_{n(\tau )}^{n(0)}{\dot{\mathit{V}}}^{n(\tau )}d\tau -{{\displaystyle \int }}_s^t{\mathit{C}}_{n(\tau )}^{n(0)}{\mathit{g}}^{n(\tau )}d\tau \\ & +{{\displaystyle \int }}_s^t{\mathit{C}}_{n(\tau )}^{n(0)}(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n)\times {\mathit{V}}^{n(\tau )}d\tau \end{array}$$

(9)

where new vectors are formed by subtracting the vectors at epoch $s$ from that at epoch $t$, where $t>s$.

By substituting (8) and (9) into (5), it can be seen that the newly formed vector still satisfies ${\mathit{\beta }}_{sw}={\mathit{C}}_{b(0)}^{n(0)}{\mathit{\alpha }}_{sw}$. Consequently, the integration interval for ${\mathit{\alpha }}_{sw}$ and ${\mathit{\beta }}_{sw}$ is reduced from $[0,t]$ to $[s,t]$, successfully isolating sensor error accumulation over the interval $[0,s]$ and improving alignment performance.

For practical implementation in SINS, ${\mathit{\alpha }}_{sw}$ and ${\mathit{\beta }}_{sw}$ are discretized, and the resulting discretized vectors are expressed as follows:

$${\mathit{\alpha }}_{sw}={\displaystyle \sum }_{\tau =s}^t{\mathit{C}}_{b(\tau )}^{b(0)}(\Delta \mathit{v}+{\displaystyle \frac{1}{2}}\Delta \mathit{\theta }\times \Delta \mathit{v})$$

(10)

$$\begin{array}{ll}{\mathit{\beta }}_{sw}=& {\mathit{C}}_{n(t)}^{n(0)}{\mathit{V}}^n(t)-{\mathit{C}}_{n(s)}^{n(0)}{\mathit{V}}^n(s)\\ & +{\displaystyle {\displaystyle \sum }_{\tau =s}^{t-1}}{\mathit{C}}_{n(\tau )}^{n(0)}\left[\left({\displaystyle \frac{1}{2}}T{\mathit{I}}_3+{\displaystyle \frac{1}{6}}{T}^2\left[{\mathit{\omega }}_{in}^n\left(\tau \right)\times \right]\right){\mathit{\omega }}_{ie}^n\left(\tau \right)\times {\mathit{V}}^n\left(\tau \right)\right]\\ & +{\displaystyle {\displaystyle \sum }_{\tau =s}^{t-1}}{\mathit{C}}_{n(\tau )}^{n(0)}\left[\left({\displaystyle \frac{1}{2}}T{\mathit{I}}_3+{\displaystyle \frac{1}{3}}{T}^2\left[{\mathit{\omega }}_{in}^n\left(\tau \right)\times \right]\right){\mathit{\omega }}_{ie}^n\left(\tau \right)\times {\mathit{V}}^n\left(\tau +1\right)\right]\\ & -{\displaystyle {\displaystyle \sum }_{\tau =s}^{t-1}}{\mathit{C}}_{n(\tau )}^{n(0)}\left[\left(T{\mathit{I}}_3+{\displaystyle \frac{1}{2}} T^2\left[{\mathit{\omega }}_{in}^n\left(\tau \right)\times \right]\right){\mathit{g}}^n\right]\end{array}$$

(11)

where $\Delta \mathit{v}$ and $\Delta \mathit{\theta }$ are gyro output and $T$ denotes the IMU sampling interval. Due to the outliers and noise in GNSS output, the velocity information is reformulated as follows:

$${\tilde{\mathit{V}}}^n={\mathit{V}}^n+\delta {\mathit{v}}^n$$

(12)

where ${\tilde{\mathit{V}}}^n$ is the velocity obtained by the GNSS outputs, $\delta {\mathit{v}}^n$ denotes the error in the GNSS velocity output, $\delta {\mathit{v}}^n=\mathit{\Gamma }+{\mathit{\upsilon }}_G$, ${\mathit{\upsilon }}_G$ denotes the measurement noise, and $\mathit{\Gamma }$ represents the outlier component.

By substituting (12) into (11), ${\mathit{\beta }}_{sw}$ is reconstructed as follows:

$$\begin{array}{ll}{\tilde{\mathit{\beta }}}_{sw}=& {\mathit{C}}_{n(t)}^{n(0)}{\tilde{\mathit{V}}}^n(t)-{\mathit{C}}_{n(s)}^{n(0)}{\tilde{\mathit{V}}}^n(s)\\ & +{\displaystyle {\displaystyle \sum }_{\tau =s}^{t-1}}{\mathit{C}}_{n(\tau )}^{n(0)}\left[\left({\displaystyle \frac{1}{2}}T{\mathit{I}}_3+{\displaystyle \frac{1}{6}}{T}^2\left[{\mathit{\omega }}_{in}^n\left(\tau \right)\times \right]\right){\mathit{\omega }}_{ie}^n\left(\tau \right)\times {\tilde{\mathit{V}}}^n\left(\tau \right)\right]\\ & +{\displaystyle {\displaystyle \sum }_{\tau =s}^{t-1}}{\mathit{C}}_{n(\tau )}^{n(0)}\left[\left({\displaystyle \frac{1}{2}}T{\mathit{I}}_3+{\displaystyle \frac{1}{3}}{T}^2\left[{\mathit{\omega }}_{in}^n\left(\tau \right)\times \right]\right){\mathit{\omega }}_{ie}^n\left(\tau \right)\times {\tilde{\mathit{V}}}^n\left(\tau +1\right)\right]\\ & -{\displaystyle {\displaystyle \sum }_{\tau =s}^{t-1}}{\mathit{C}}_{n(\tau )}^{n(0)}\left[\left(T{\mathit{I}}_3+{\displaystyle \frac{1}{2}} T^2\left[{\mathit{\omega }}_{in}^n\left(\tau \right)\times \right]\right){\mathit{g}}^n\right]\\ \hspace{1em} =& {\mathit{\beta }}_{sw}+{\mathit{e}}_{\beta }\end{array}$$

(13)

where ${\mathit{e}}_{\beta }$ denotes the actual observation vector error. Because outliers are typically several times larger than the current velocity, the resulting ${\mathit{e}}_{\beta }$ cannot be neglected, which significantly degrades the accuracy of the constructed observation vector. If this contaminated vector is directly utilized to solve ${\mathit{C}}_{b(0)}^{n(0)}$, the solution’s accuracy is significantly compromised, and convergence may even fail.

To effectively suppress the outliers, a residual-based outlier suppression strategy is proposed. According to (5), the two sides of the equation should be strictly equal. Outliers in GNSS-aided measurements degrade the accuracy of ${\mathit{\beta }}_{sw}$. However, ${\mathit{\alpha }}_{sw}$ relies solely on accelerometer-derived specific force and remains unaffected by such outliers. Consequently, the equality is violated. The Euclidean norm of both sides yields $‖{\mathit{\beta }}_{sw}‖=‖{\mathit{C}}_{b(0)}^{n(0)}{\mathit{\alpha }}_{sw}‖$. The orthogonality of the attitude matrix ${\mathit{C}}_{b(0)}^{n(0)}$ yields $‖{\mathit{C}}_{b(0)}^{n(0)}‖=1$.

Rewriting the above equation yields

$$‖{\mathit{\beta }}_{sw}‖=‖{\mathit{\alpha }}_{sw}‖$$

(14)

By substituting (13) into (14), the residual is defined as follows:

$${\varsigma }_w=\parallel {\tilde{\mathit{\beta }}}_{sw}{\parallel }^2-\parallel {\mathit{\alpha }}_{sw}{\parallel }^2$$

(15)

When the velocity output by GNSS contains only measurement noise and no outliers, i.e., $\mathit{\Gamma }=0$, the residual remains small, ${\mathit{\beta }}_{sw}\approx {\tilde{\mathit{\beta }}}_{sw}$. In contrast, when the velocity data are contaminated by outliers, the residual increases significantly. On this basis, a weighting function $\mathit{\Lambda }({\varsigma }_w)$ is constructed from the residual to determine whether ${\tilde{\mathit{\beta }}}_{sw}$ has been affected by outliers, and it can be written as follows:

$$\Lambda ({\varsigma }_w)=\left\{\begin{array}{ll}{\displaystyle \frac{\Delta }{{\varsigma }_w}}\hfill & {\varsigma }_w\ge \Delta \hfill \\ 1\hfill & {\varsigma }_w<\Delta \hfill \end{array}\right.$$

(16)

where $\Delta$ is a threshold and its value is set to 25 in this study based on experimental analysis. Using the weighting function defined above, the observation vector is reconstructed as follows:

$${\overline{\mathit{\beta }}}_{sw}=\mathit{\Lambda }({\varsigma }_w){\tilde{\mathit{\beta }}}_{sw}+\left(1-\mathit{\Lambda }({\varsigma }_w)\right){\mathit{C}}_{b(0)}^{n(0)}{\mathit{\alpha }}_{sw}$$

(17)

When the residual ${\varsigma }_w$ is below the preset threshold $\Delta$, the data are considered free from outlier interference, and the observation vector does not require modification. If ${\varsigma }_w$ exceeds $\Delta$, the weighting function reduces the reliability of ${\tilde{\mathit{\beta }}}_{sw}$, and ${\mathit{\alpha }}_{sw}$ is used to compensate for ${\tilde{\mathit{\beta }}}_{sw}$, yielding a corrected observation vector ${\overline{\mathit{\beta }}}_{sw}$. This procedure mitigates the effect of outliers on the observation vector, thereby enhancing the accuracy of its reconstruction.

2.3. Construction of State-Space Model

To accomplish the in-motion alignment, it is essential to develop an accurate model for the subsequent filtering process. Due to the inevitable presence of gyroscope errors during actual alignment, discrepancies emerge between the $\tilde{b}$ frame and the $b$ frame. A relationship between the two frames can be established through an equivalent rotation vector, which can be represented as follows:

$${\mathit{C}}_{\tilde{b}(t)}^{b(t)}={\mathit{I}}_3+{\mathit{\phi }}_b\times$$

(18)

where ${\mathit{\phi }}_b$ represents the equivalent rotation vector.

Taking the gyroscope errors into account, (3) is reformulated to derive the rotation matrix from the initial $b(0)$ frame to the current $\tilde{b}(t)$ frame, which can be written as follows:

$$\begin{array}{ll}{\dot{\mathit{C}}}_{\tilde{b}(t)}^{b(0)}& ={\mathit{C}}_{\tilde{b}(t)}^{b(0)}({\tilde{\mathit{\omega }}}_{ib}^b\times )={\mathit{C}}_{b(t)}^{b(0)}{\mathit{C}}_{\tilde{b}(t)}^{b(t)}\left[({\mathit{\omega }}_{ib}^b\times )+(\delta {\mathit{\omega }}_{ib}^b\times )\right]\\ & ={\mathit{C}}_{b(t)}^{b(0)}\left[{\mathit{I}}_3+({\mathit{\phi }}_b\times )\right]\left[({\mathit{\omega }}_{ib}^b\times )+(\delta {\mathit{\omega }}_{ib}^b\times )\right]\\ & ={\mathit{C}}_{b(t)}^{b(0)}\left[({\mathit{\omega }}_{ib}^b\times )+(\delta {\mathit{\omega }}_{ib}^b\times )+({\mathit{\phi }}_b\times )({\mathit{\omega }}_{ib}^b\times )+({\mathit{\phi }}_b\times )(\delta {\mathit{\omega }}_{ib}^b\times )\right]\end{array}$$

(19)

where $\delta {\mathit{\omega }}_{ib}^b$ denotes gyroscope errors. By applying the chain rule for attitude matrices, ${\mathit{C}}_{b(t)}^{b(0)}$ in (1) can be rewritten as follows:

$${\mathit{C}}_{b(t)}^{b(0)}={\mathit{C}}_{\tilde{b}(t)}^{b(0)}{\mathit{C}}_{b(t)}^{\tilde{b}(t)}$$

(20)

By substituting (18) into (20), the expression is obtained as follows:

$${\mathit{C}}_{\tilde{b}(t)}^{b(0)}={\mathit{C}}_{b(t)}^{b(0)}{\mathit{C}}_{\tilde{b}(t)}^{b(t)}={\mathit{C}}_{b(t)}^{b(0)}\left({\mathit{I}}_3+({\mathit{\phi }}_b\times )\right)$$

(21)

Differentiating both sides of (21) with respect to time yields

$$\begin{array}{ll}{\dot{\mathit{C}}}_{\tilde{b}(t)}^{b(0)}& ={\dot{\mathit{C}}}_{b(t)}^{b(0)}\left({\mathit{I}}_3+({\mathit{\phi }}_b\times )\right) +{\mathit{C}}_{b(t)}^{b(0)}({\dot{\mathit{\phi }}}_b\times )\\ & ={\mathit{C}}_{b(t)}^{b(0)}({\mathit{\omega }}_{ib}^b\times )+{\mathit{C}}_{b(t)}^{b(0)}({\mathit{\omega }}_{ib}^b\times )({\mathit{\phi }}_b\times )+{\mathit{C}}_{b(t)}^{b(0)}({\dot{\mathit{\phi }}}_b\times )\\ & ={\mathit{C}}_{b(t)}^{b(0)}\left[({\mathit{\omega }}_{ib}^b\times )+({\mathit{\omega }}_{ib}^b\times )({\mathit{\phi }}_b\times )+({\dot{\mathit{\phi }}}_b\times )\right]\end{array}$$

(22)

By comparing (19) and (22), it can be observed that their right-hand sides are equivalent. Extracting these expressions yields

$$\begin{array}{l}\left[({\mathit{\omega }}_{ib}^b\times )+(\delta {\mathit{\omega }}_{ib}^b\times )+\left({\mathit{\phi }}_b\times \right)({\mathit{\omega }}_{ib}^b\times )+\left({\mathit{\phi }}_b\times \right)(\delta {\mathit{\omega }}_{ib}^b\times )\right]\\ =\left[({\mathit{\omega }}_{ib}^b\times )+({\mathit{\omega }}_{ib}^b\times )({\mathit{\phi }}_b\times )+({\dot{\mathit{\phi }}}_b\times )\right]\end{array}$$

(23)

By further simplifying (23), the formula for calculating the equivalent rotation vector is obtained as follows:

$${\dot{\mathit{\phi }}}_b=-{\tilde{\mathit{\omega }}}_{ib}^b\times {\mathit{\phi }}_b+\delta {\mathit{\omega }}_{ib}^b$$

(24)

In low-cost SINS, the gyroscope’s constant bias is typically a non-negligible constant, which is defined as follows:

$${\dot{\mathit{\epsilon }}}^b={\mathbf{0}}_{3\times 1}$$

(25)

By defining the system state vector $\mathit{X}={[\begin{array}{cc}{\mathit{\phi }}_b& {\mathit{\epsilon }}^b\end{array}]}^T$, the state-space equations are established by (24) and (25).

Utilizing the reconstructed observation vector ${\overline{\mathit{\beta }}}_{sw}$ and the reference vector ${\mathit{\alpha }}_{sw}$, the measurement equation is formulated as follows:

$$\begin{array}{ll}{\overline{\mathit{\beta }}}_{sw}& ={\mathit{C}}_{b(0)}^{n(0)}{\mathit{\alpha }}_{sw}={\mathit{C}}_{b(0)}^{n(0)}{{\displaystyle \int }}_s^t{\mathit{C}}_{b(\tau )}^{b(0)}{\mathit{f}}^{b(\tau )}d\tau \\ & ={\mathit{C}}_{b(0)}^{n(0)}{\mathit{C}}_{\tilde{b}(s)}^{b(0)}{\mathit{C}}_{b(s)}^{\tilde{b}(s)}{{\displaystyle \int }}_s^t{\mathit{C}}_{b(\tau )}^{b(s)}{\mathit{f}}^{b(\tau )}d\tau \end{array}$$

(26)

Substituting (18) into (26) yields

$$\begin{array}{ll}{\overline{\mathit{\beta }}}_{sw}& ={\mathit{C}}_{b(0)}^{n(0)}{\mathit{C}}_{\tilde{b}(s)}^{b(0)}\left[{\mathit{I}}_3-({\mathit{\phi }}_b\times )\right]{{\displaystyle \int }}_s^t{\mathit{C}}_{b(\tau )}^{b(s)}{\mathit{f}}^{b(\tau )}d\tau \\ & ={\mathit{C}}_{b(0)}^{n(0)}{\mathit{C}}_{\tilde{b}(s)}^{b(0)}{{\displaystyle \int }}_s^t{\mathit{C}}_{b(\tau )}^{b(s)}{\mathit{f}}^{b(\tau )}d\tau -{\mathit{C}}_{b(0)}^{n(0)}{\mathit{C}}_{\tilde{b}(s)}^{b(0)}{{\displaystyle \int }}_s^t{\mathit{C}}_{b(\tau )}^{b(s)}{\mathit{f}}^{b(\tau )}d\tau ({\mathit{\phi }}_b\times )\\ & ={\mathit{C}}_{b(0)}^{n(0)}{\mathit{C}}_{\tilde{b}(s)}^{b(0)}{\tilde{\mathit{\alpha }}}_{sw}-{\mathit{C}}_{b(0)}^{n(0)}{\mathit{C}}_{\tilde{b}(s)}^{b(0)}{\tilde{\mathit{\alpha }}}_{sw}({\mathit{\phi }}_b\times )={\mathit{C}}_{b(0)}^{n(0)}{\mathit{C}}_{\tilde{b}(s)}^{b(0)}{\tilde{\mathit{\alpha }}}_{sw}+{\mathit{C}}_{b(0)}^{n(0)}{\mathit{C}}_{\tilde{b}(s)}^{b(0)}({\tilde{\mathit{\alpha }}}_{sw}\times )({\mathit{\phi }}_b)\end{array}$$

(27)

where ${\tilde{\mathit{\alpha }}}_{sw}={{\displaystyle \int }}_s^t{\mathit{C}}_{b(\tau )}^{b(s)}{\mathit{f}}^{b(\tau )}d\tau$. Rearranging (27), the measurement equation can be expressed as follows:

$${\mathit{Z}}_k={\mathit{H}}_k{\mathit{X}}_k+{\mathit{v}}_k$$

(28)

where ${\mathit{v}}_k$ denotes the measurement noise. The expressions for the measurement vector ${\mathit{Z}}_k$ and the measurement matrix ${\mathit{H}}_k$ are given by

$${\mathit{Z}}_k={\overline{\mathit{\beta }}}_{sw}-{\mathit{C}}_{b(0)}^{n(0)}{\mathit{C}}_{\tilde{b}(s)}^{b(0)}{\tilde{\mathit{\alpha }}}_{sw}$$

(29)

$${\mathit{H}}_k=\left[\begin{array}{c}{\mathit{C}}_{b(0)}^{n(0)}{\mathit{C}}_{\tilde{b}(s)}^{b(0)}({\tilde{\mathit{\alpha }}}_{sw}\times ) {\mathbf{0}}_{3\times 3}\end{array}\right]$$

(30)

The established system forms the basis for estimating the gyroscope constant biases and the equivalent rotation vector between the $b$ frame and the $\tilde{b}$ frame. Moreover, in complex environments, the short available data record is insufficient for conventional methods to achieve high accuracy and may result in non-convergence. In view of these limitations, it is essential to develop new fast alignment methods.

3. The Proposed Robust Filtering Method

3.1. Adaptive Window Adjustment Multiple Backtracking Approach

The presence of uncertain model parameters, noise with unknown statistical characteristics, and outliers limits the effectiveness of conventional multiple backtracking approaches for in-motion alignment. As a result, these methods fail to fully exploit navigation data and cannot satisfy the carrier’s rapid response requirements in complex environments. Sensor noise in complex environments exhibits significant time-varying characteristics, while outdated data cannot reflect the noise characteristics at the current time. Continuing to use the outdated data will lead to the degradation of the estimation performance of the filter. Existing approaches generally ignore the effect of the relationship between the size of the backtracking window and the filtering performance.

To address this problem, this paper proposes an adaptive window adjustment multiple backtracking approach based on innovation gradient detection. The innovation in filtering reflects the mismatch between observations and predictions. In complex environments, this leads to a notable discrepancy between the theoretical and actual error covariance matrices, which reflects the variability in noise characteristics. In the initial stage of the alignment process, the backtracking window size is set manually. Subsequently, the trend in noise characteristics can be assessed by comparing the innovation gradients calculated over the earlier and later segments within the window. If fluctuations in noise characteristics are detected, the subsequent window size is reduced to enhance tracking capability. Conversely, if noise characteristics are stable, the window size is appropriately increased to improve the unbiasedness of estimation.

The innovation and actual value of the error covariance matrix is given as follows:

$${\mathit{\gamma }}_k={\mathit{Z}}_k-{\mathit{H}}_k{\mathit{X}}_{k|k-1}$$

(31)

$${\mathit{C}}_k=〈{\mathit{\gamma }}_k,{\mathit{\gamma }}_k〉={\displaystyle \frac{1}{L_j}}{\displaystyle \sum }_{i=1}^k{\mathit{\gamma }}_i{\mathit{\gamma }}_i^T$$

(32)

where ${\mathit{\gamma }}_k$ is the innovation, ${\mathit{C}}_k$ is the estimated error covariance matrix, and $L_j$ represents the size of the $j^{th}$ backtracking window. The theoretical error covariance matrix is given by

$${\mathit{{\rm M}}}_k={\mathit{H}}_k{\mathit{P}}_{k|k-1}{\mathit{H}}_k^{\mathrm{T}}+{\mathit{R}}_k$$

(33)

To reflect the difference between ${\mathit{C}}_k$ and ${\mathit{{\rm M}}}_k$, the matching degree is defined as the ratio of their traces and expressed as follows:

$${\zeta }_k={\displaystyle \frac{tr({\mathit{C}}_k)}{tr({\mathit{{\rm M}}}_k)}}$$

(34)

To effectively determine whether the statistical characteristics of noise have changed within the previous window, this study proposes a metric based on the innovation gradient value. The innovation gradient value is defined as follows:

$${\nabla }_j={\displaystyle \frac{\left({\zeta }_{j,k+1}-{\zeta }_{j,k}\right)}{{\zeta }_{j,k}}}$$

(35)

where ${\zeta }_{j,k+1}$ and ${\zeta }_{j,k}$ denote the matching degree for the first and second halves of the $j^{th}$ backtracking window, respectively. By detecting the fluctuation of noise characteristics within the $j^{th}$ backtracking window based on (35), the size of the ${(j+1)}^{th}$ window can be adaptively adjusted as follows:

$$L_{j+1}=L_j(1+{\nabla }_j)$$

(36)

According to (36), when the noise characteristics dramatically fluctuate within the previous backtracking window, resulting in a negative innovation gradient value, the current window size should be appropriately reduced to achieve higher tracking sensitivity. Conversely, when the noise characteristics remain stable in the previous window, resulting in a positive innovation gradient value, the current window size should be increased to enhance estimation unbiasedness. The diagram of the proposed adaptive multiple backtracking approach is illustrated in Figure 2.

3.2. EM-Based Robust Variational Bayesian Kalman Filter

In this section, an EM-based robust variational Bayesian Kalman filter is proposed. Modeling the likelihood function with Student’s t distribution significantly improves the robustness of the approach. By employing the variational Bayesian method, the joint posterior distributions of system states and unknown parameters are estimated, which facilitates effective estimation of the uncertain measurement noise covariance matrix. Moreover, the EM method is designed to estimate the prior scale matrix of the prediction error covariance matrix. Therefore, the proposed approach demonstrates robust filtering performance in complex interference environments and notably improves the accuracy of estimation.

The discrete-time state-space equations for the SINS/GNSS integrated navigation system are defined as follows:

$$\begin{array}{l}{\mathit{X}}_k={\mathit{F}}_{k-1}{\mathit{X}}_{k-1}+{\mathit{w}}_{k-1}\\ {\mathit{Z}}_k={\mathit{H}}_k{\mathit{X}}_k+{\mathit{v}}_k\end{array}$$

(37)

where ${\mathit{X}}_k$ is the state vector, ${\mathit{F}}_{k-1}$ is the state-transition matrix, ${\mathit{Z}}_k$ is the measurement vector, and ${\mathit{H}}_k$ is the measurement matrix. ${\mathit{w}}_{k-1}$ and ${\mathit{v}}_k$ represent the process and measurement noise, respectively. The process noise is assumed to be a zero-mean Gaussian distribution, i.e., ${\mathit{w}}_k \sim N(\mathbf{0},{\mathit{Q}}_k)$, where ${\mathit{Q}}_k$ is the process noise covariance matrix. Due to the presence of outliers in complex environments, the measurement noise is better modeled by a heavy-tailed distribution. Hence, the measurement noise is modeled as a zero-mean Student’s t distribution, i.e., ${\mathit{v}}_k \sim St({\mathit{v}}_k;0,{\mathit{R}}_k,{\xi }_k)$, where ${\mathit{R}}_k$ is the measurement noise covariance matrix and ${\xi }_k$ is the degree of freedom. A smaller value of ${\xi }_k$ corresponds to a heavier-tailed distribution.

The state’s prior probability density function (PDF) and the likelihood function are modeled by a Gaussian distribution and a Student’s t distribution, respectively, and can be expressed as follows:

$$p({\mathit{X}}_k|{\mathit{Z}}_{1:k-1})=N({\mathit{X}}_k;{\mathit{X}}_{k|k-1},{\mathit{P}}_{k|k-1})$$

(38)

$$p({\mathit{Z}}_k|{\mathit{X}}_k)=St({\mathit{Z}}_k;{\mathit{H}}_k{\mathit{X}}_k,{\mathit{R}}_k,{\xi }_k)$$

(39)

where ${\mathit{P}}_{k|k-1}$ is the predicted error covariance matrix (PECM) and ${\mathit{X}}_{k|k-1}$ is the predicted state vector.

To simplify the subsequent computations, the likelihood function is formulated as an infinite Gaussian mixture, which can be represented as follows:

$$\begin{array}{ll}p({\mathit{Z}}_k|{\mathit{X}}_k)& =St({\mathit{Z}}_k;{\mathit{H}}_k{\mathit{X}}_k,{\mathit{R}}_k,{\xi }_k)\\ & ={\displaystyle {\int }_0^{+\infty }N({\mathit{Z}}_k;{\mathit{H}}_k{\mathit{X}}_k,\frac{{\mathit{R}}_k}{{\vartheta }_k})G({\vartheta }_k; 0.5{\xi }_k,0.5{\xi }_k )d{\vartheta }_k}\end{array}$$

(40)

where $G(·)$ denotes a Gamma distribution and ${\vartheta }_k$ is an auxiliary random variable. By factorizing (40), the likelihood function can be further rewritten as follows:

$$\left\{\begin{array}{l}p({\mathit{Z}}_k|{\mathit{X}}_k,{\xi }_k)=N({\mathit{Z}}_k;{\mathit{H}}_k{\mathit{X}}_k,{\displaystyle \frac{{\mathit{R}}_k}{{\vartheta }_k}})\hfill \\ p({\vartheta }_k)=G({\vartheta }_k; 0.5{\xi }_k,0.5{\xi }_k)\hfill \end{array}\right.$$

(41)

To ensure that the posterior distributions of ${\mathit{P}}_{k|k-1}$ and ${\mathit{R}}_k$ have the same functional form as the prior distributions, the inverse Wishart distribution is adopted for the covariance matrices. Consequently, the prior distributions of ${\mathit{P}}_{k|k-1}$ and ${\mathit{R}}_k$ are given by

$$p({\mathit{P}}_{k|k-1}|{\mathit{Z}}_{1:k-1})=IW({\mathit{P}}_{k|k-1}; {\lambda }_k, {\mathit{\gamma }}_k)$$

(42)

$$p({\mathit{R}}_k|{\mathit{Z}}_{1:k-1})=IW({\mathit{R}}_k;{\mu }_{k|k-1},{\mathit{\psi }}_{k|k-1})$$

(43)

where $IW(·)$ denotes the inverse Wishart distribution and ${\lambda }_k$ and ${\mathit{\gamma }}_k$ are the prior degrees of freedom and the prior scale matrix for ${\mathit{P}}_{k|k-1}$, respectively. Similarly, ${\mu }_{k|k-1}$ and ${\mathit{\psi }}_{k|k-1}$ are the prior degrees of freedom and the prior scale matrix for ${\mathit{R}}_k$. Updates to these prior parameters are as follows:

$${\mathit{\gamma }}_k={\lambda }_k{\mathit{P}}_{k|k-1}$$

(44)

$${\mu }_{k|k-1}=\varphi {\mu }_{k-1|k-1}$$

(45)

$${\mathit{\psi }}_{k|k-1}=\varphi {\mathit{\psi }}_{k-1|k-1}$$

(46)

where $\varphi$ is the forgetting factor satisfying $\varphi \in (0 1]$. At time step $k-1$, the symbols ${\mu }_{k-1|k-1}$ and ${\mathit{\psi }}_{k-1|k-1}$ denote the posterior degrees of freedom and the posterior scale matrix of ${\mathit{R}}_k$, respectively. In this study, $\varphi =0.968$, ${\lambda }_0=10$, and ${\mu }_{0|0}=5$.

Based on the previously derived prior PDFs, the joint posterior PDF $p({\mathit{X}}_k,{\mathit{P}}_{k|k-1},{\mathit{R}}_k,{\vartheta }_k|{\mathit{Z}}_{1:k})$ needs to be evaluated. Because the joint posterior does not admit a closed-form solution, a VB method is employed to iteratively approximate it. The update for the ${(i+1)}^{th}$ VB iteration is given by

$$p(\mathrm{\Xi }|{\mathit{Z}}_{1:k})=q^{(i+1)}({\mathit{X}}_k)q^{(i+1)}({\mathit{P}}_{k|k-1})q^{(i+1)}({\mathit{R}}_k)q^{(i+1)}({\vartheta }_k)$$

(47)

where $\mathrm{\Xi } = \{{\mathit{X}}_k,{\mathit{P}}_{k|k-1},{\mathit{R}}_k, {\vartheta }_k\}$ and $q^{(i+1)}(·)$ denotes the approximate posterior PDF obtained in the ${(i+1)}^{th}$ iteration. The VB iteration is terminated when further iterations result in negligible improvement in estimation accuracy, and the number of iterations is set to 10. The posterior PDF of ${\mathit{P}}_{k|k-1}$, along with updates to its posterior parameters, are given by

$$q^{(i+1)}({\mathit{P}}_{k|k-1})=IW({\mathit{P}}_{k|k-1};{\lambda }_{k|k}^{(i+1)},{\mathit{\gamma }}_{k|k}^{(i+1)})$$

(48)

$${\lambda }_{k|k}^{(i+1)}={\lambda }_k+1, {\mathit{\gamma }}_{k|k}^{(i+1)}={\mathit{\gamma }}_k^{(i)}+{\mathit{A}}_k^{(i)}$$

(49)

$${\mathit{A}}_k^{(i)}={\mathit{P}}_{k|k}^{(i)}+\left({\mathit{X}}_{k|k}^{(i)}-{\mathit{X}}_{k|k-1}\right){\left({\mathit{X}}_{k|k}^{(i)}-{\mathit{X}}_{k|k-1}\right)}^T$$

(50)

According to (48), the PECM ${\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}$ is obtained as follows:

$${\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}={\mathit{\gamma }}_{k|k}^{(i+1)}{({\lambda }_{k|k}^{(i+1)})}^{-1}$$

(51)

The posterior PDF of ${\mathit{R}}_k$, together with its updated posterior parameters, are given by

$$q^{(i+1)}({\mathit{R}}_k)=IW({\mathit{R}}_k;{\mu }_{k|k}^{(i+1)},{\mathit{\psi }}_{k|k}^{(i+1)})$$

(52)

$${\mu }_{k|k}^{(i+1)}={\mu }_{k|k-1}+1, {\mathit{\psi }}_{k|k}^{(i+1)}={\mathit{\psi }}_{k|k-1}+{\mathit{E}}^{(i+1)}({\vartheta }_k){\mathit{B}}_k^{(i)}$$

(53)

$${\mathit{B}}_k^{(i)}=\left({\mathit{Z}}_k-{\mathit{H}}_k{\mathit{X}}_{k|k}^{(i)}\right){\left({\mathit{Z}}_k-{\mathit{H}}_k{\mathit{X}}_{k|k}^{(i)}\right)}^T+{\mathit{H}}_k{\mathit{P}}_{k|k}^{(i)}{\mathit{H}}_k^T$$

(54)

The posterior PDF of ${\vartheta }_k$, along with the updates of its posterior parameters, are given as follows:

$$q^{(i+1)}({\vartheta }_k)=G({\vartheta }_k;{\delta }_{k|k}^{(i+1)},{\chi }_{k|k}^{(i+1)})$$

(55)

$${\delta }_{k|k}^{(i+1)}=0.5({\xi }_k+d_z), {\chi }_{k|k}^{(i+1)}=0.5({\xi }_k+tr({\mathit{B}}_k^{(i)}{\mathit{E}}^{(i)}[{\mathit{R}}_k^{-1}]))$$

(56)

where ${\mathit{E}}^{(i)}[{\mathit{R}}_k^{-1}]={\mu }_{k|k}^{(i)}{\left({\mathit{\psi }}_{k|k}^{(i)}\right)}^{-1}$ and $d_z$ is the dimension of ${\mathit{Z}}_k$. By jointly applying (41) and (52), the refined measurement noise covariance matrix ${\widehat{\mathit{R}}}_k^{(i+1)}$ is derived as follows:

$${\widehat{\mathit{R}}}_k^{(i+1)}={\mathit{\psi }}_{k|k}^{(i+1)}{({\mu }_{k|k}^{(i+1)})}^{-1}{\mathit{E}}^{(i+1)}{({\vartheta }_k)}^{-1}$$

(57)

where ${\mathit{E}}^{(i+1)}({\vartheta }_k)={\delta }_{k|k}^{(i+1)}{({\chi }_{k|k}^{(i+1)})}^{-1}$. Meanwhile, the posterior PDF of ${\mathit{X}}_k$ is given by

$$q^{(i+1)}({\mathit{X}}_k)=N({\mathit{X}}_k,{\mathit{X}}_{k|k}^{(i+1)},{\widehat{\mathit{P}}}_{k|k}^{(i+1)})$$

(58)

On the basis of (51) and (57), the PECM ${\widehat{\mathit{P}}}_{k|k-1}$ and the refined ${\widehat{\mathit{R}}}_k^{(i+1)}$ are obtained. The optimal state estimation ${\mathit{X}}_{k|k}^{(i+1)}$ and its covariance matrix ${\mathit{P}}_{k|k}^{(i+1)}$ are calculated as follows:

$${\mathit{K}}_k^{(i+1)}={\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}{\mathit{H}}_k^T{\left({\mathit{H}}_k{\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}{\mathit{H}}_k^T+{\widehat{\mathit{R}}}_k^{(i+1)}\right)}^{-1}$$

(59)

$${\mathit{X}}_{k|k}^{(i+1)}={\mathit{X}}_{k|k-1}+{\mathit{K}}_k^{(i+1)}({\mathit{Z}}_k-{\mathit{H}}_k{\mathit{X}}_{k|k-1})$$

(60)

$${\mathit{P}}_{k|k}^{(i+1)}={\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}-{\mathit{K}}_k^{(i+1)}{\mathit{H}}_k{\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}$$

(61)

As shown in (59), the accuracy of ${\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}$ directly affects the filter gain and thus the accuracy of state estimation. Therefore, improving the accuracy of ${\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}$ is critical for enhancing the performance of state estimation. Moreover, an EM method is employed to refine ${\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}$.

The EM method consists of two alternating phases: an expectation step (E-step) and a maximization step (M-step). For the current parameter estimation ${\theta }^{(i)}$, the E-step computes the conditional expectation $\mathit{Q}(\theta ,{\theta }^{(i)})$ of the latent variables, where $i$ denotes the iteration index. The M-step then maximizes this conditional expectation to update the parameter estimation ${\theta }^{(i+1)}$. These two steps are repeated until convergence or until a predefined maximum number of iterations is reached.

By selecting the latent variables as $\Omega = \{{\mathit{X}}_k,{\mathit{P}}_{k|k-1},{\mathit{R}}_k, {\vartheta }_k\}$, the maximum likelihood estimation of the prior scale matrix ${\mathit{\gamma }}_k$ is given as follows:

$${\mathit{\gamma }}_k\approx \arg \underset{{\mathit{\gamma }}_k}{\max }[\log p_{{\mathit{\gamma }}_k}(\Omega ,{\mathit{Z}}_{1:k})]\approx \arg \underset{{\mathit{\gamma }}_k}{\max }\mathit{Q}({\mathit{\gamma }}_k,{\mathit{\gamma }}_k^{(i)})$$

(62)

where $p_{{\mathit{\gamma }}_k}(\Omega ,{\mathit{Z}}_{1:k})$ denotes the likelihood function of the joint PDF, which is determined by ${\mathit{\gamma }}_k$.

First, the E-step is performed. Given the current estimation of the prior scale matrix, the conditional expectation of the latent variables is calculated as follows:

$$\begin{array}{ll}\mathit{Q}({\mathit{\gamma }}_k,{\mathit{\gamma }}_k^{(i)})& \stackrel{\Delta }{=}{\mathit{E}}_{\mathrm{\Xi }}\left[\log p_{{\mathit{\gamma }}_k}(\Omega ,{\mathit{Z}}_{1:k})|{\mathit{\gamma }}_k^{(i)},{\mathit{Z}}_{1:k}\right]\\ & ={\displaystyle \int \log p_{{\mathit{\gamma }}_k}(\Omega ,{\mathit{Z}}_{1:k})p_{{\mathit{\gamma }}_k^{(i)}}(\Omega |{\mathit{Z}}_{1:k})d\Omega }\end{array}$$

(63)

By applying the probability chain rule and Bayes’ theorem, the joint PDF $p_{{\mathit{\gamma }}_k}(\Omega ,{\mathit{Z}}_{1:k})$ is rewritten as follows:

$$\begin{array}{ll}{p}_{{\mathit{\gamma }}_k}(\Omega ,{\mathit{Z}}_{1:k})& =p_{{\mathit{\gamma }}_k}({\mathit{X}}_k,{\mathit{P}}_{k|k-1},{\mathit{R}}_k,{\vartheta }_k,{\mathit{Z}}_{1:k})\\ & =p({\mathit{Z}}_k|{\mathit{X}}_k, {\displaystyle \frac{{\mathit{R}}_k}{{\vartheta }_k}})p({\vartheta }_k)p({\mathit{X}}_k|{\mathit{Z}}_{1:k-1},{\mathit{P}}_{k|k-1})\\ & \times p({\mathit{R}}_k|{\mathit{Z}}_{1:k-1})p_{{\mathit{\gamma }}_k}({\mathit{P}}_{k|k-1}|{\mathit{Z}}_{1:k-1})p({\mathit{Z}}_{1:k-1})\end{array}$$

(64)

Because only ${\mathit{P}}_{k|k-1}$ depends on ${\mathit{\gamma }}_k$, the expression can be simplified to obtain the conditional expectation of the latent variables, which is given as follows:

$$\mathit{Q}({\mathit{\gamma }}_k,{\mathit{\gamma }}_k^{(i)})={\displaystyle \int \log p_{{\mathit{\gamma }}_k}({\mathit{P}}_{k|k-1}|{\mathit{Z}}_{1:k-1})\times p_{{\mathit{\gamma }}_k^{(i)}}({\mathit{P}}_{k|k-1}|{\mathit{Z}}_{1:k})d{\mathit{P}}_{k|k-1}}+{\varpi }_{\mathit{\gamma }}$$

(65)

where ${\varpi }_{\mathit{\gamma }}$ is a constant independent of ${\mathit{\gamma }}_k$.

Second, the M-step is performed. Based on the conditional expectation obtained in the E-step, ${\mathit{\gamma }}_k$ is estimated using the maximum likelihood method, and (65) can be rewritten as follows:

$$\begin{array}{ll}{\mathit{Q}}^{(i+1)}({\mathit{\gamma }}_k,{\mathit{\gamma }}_k^{(i)})& ={\displaystyle \int IW({\mathit{P}}_{k|k-1};{\lambda }_{k|k}^{(i+1)}, {\mathit{\gamma }}_{k|k}^{(i+1)})\times \log p_{{\mathit{\gamma }}_k}({\mathit{P}}_{k|k-1}|{\mathit{Z}}_{1:k-1})d{\mathit{P}}_{k|k-1}+{\varpi }_{\mathit{\gamma }}}\\ & ={\displaystyle \int IW({\mathit{P}}_{k|k-1};{\lambda }_{k|k}^{(i+1)}, {\mathit{\gamma }}_{k|k}^{(i+1)})\times \left(-0.5{\lambda }_k\log \left|{\mathit{P}}_{k|k-1}\right|-0.5tr\left[{\mathit{\gamma }}_k{\mathit{P}}_{k|k-1}^{-1}\right]\right)d{\mathit{P}}_{k|k-1}+{\varpi }_{\mathit{\gamma }}}\\ & =-0.5tr\left({\mathit{\gamma }}_k{E}^{(i+1)}[{\mathit{P}}_{k|k-1}^{-1}]\right)+0.5{\lambda }_k\log \left|{\mathit{\gamma }}_k\right|+{\varpi }_{\mathit{\gamma }}\end{array}$$

(66)

Based on (51), the expectation of the inverse of ${\mathit{P}}_{k|k-1}$ is calculated as follows:

$$\begin{array}{ll}{E}^{(i+1)}[{\mathit{P}}_{k|k-1}^{-1}]& ={\displaystyle \int {\mathit{P}}_{k|k-1}^{-1}IW({\mathit{P}}_{k|k-1};{\lambda }_{k|k}^{(i+1)},{\mathit{\gamma }}_{k|k}^{(i+1)})d{\mathit{P}}_{k|k-1}}\\ & ={\lambda }_{k|k}^{(i+1)}{\left({\mathit{\gamma }}_{k|k}^{(i+1)}\right)}^{-1}\end{array}$$

(67)

Substituting (67) into (66) yields

$${\mathit{Q}}^{(i+1)}({\mathit{\gamma }}_k,{\mathit{\gamma }}_k^{(i)})=-0.5tr\left({\mathit{\gamma }}_k{\lambda }_{k|k}^{(i+1)}{[{\mathit{\gamma }}_{k|k}^{(i+1)}]}^{-1}\right)+0.5{\lambda }_k\log \left|{\mathit{\gamma }}_k\right|+{\varpi }_{\mathit{\gamma }}$$

(68)

By maximizing (68) in the M-step, the refined prior scale matrix ${\overline{\mathit{\gamma }}}_k^{(i+1)}$ is obtained. Differentiating (68) with respect to ${\mathit{\gamma }}_k$ yields

$${\displaystyle \frac{\partial {\mathit{Q}}^{(i+1)}({\mathit{\gamma }}_k,{\mathit{\gamma }}_k^{(i)})}{\partial {\mathit{\gamma }}_k}}=0.5{\lambda }_k{\mathit{\gamma }}_k^{-1}-0.5{\lambda }_{k|k}^{(i+1)}{\left({\mathit{\gamma }}_{k|k}^{(i+1)}\right)}^{-1}$$

(69)

Setting (69) to zero yields the refined prior scale matrix as follows:

$${\overline{\mathit{\gamma }}}_k^{(i+1)}={\lambda }_k{\displaystyle \frac{{\mathit{\gamma }}_{k|k}^{(i+1)}}{{\lambda }_{k|k}^{(i+1)}}}$$

(70)

To solve the joint posterior PDF $p({\mathit{X}}_k,{\mathit{P}}_{k|k-1},{\mathit{R}}_k,{\vartheta }_k|{\mathit{Z}}_{1:k})$, the VB method is employed to sequentially approximate the posterior PDFs of the parameters, yielding the updated ${\widehat{\mathit{P}}}_{k|k-1}$ and refined ${\widehat{\mathit{R}}}_k$. In the VB method, the prior scale matrix ${\mathit{\gamma }}_k$ remains fixed. Using the EM method, the conditional expectation of the latent variable is computed. This expectation is then used to perform maximum likelihood estimation, yielding the refined prior scale matrix ${\overline{\mathit{\gamma }}}_k^{(i+1)}$, which is applied in the next iteration to improve the estimation of ${\widehat{\mathit{P}}}_{k|k-1}$.

3.3. Algorithm Summarization

The procedure of the adaptive multiple backtracking robust in-motion alignment method is summarized in Algorithm 1.

Algorithm 1: Adaptive multiple backtracking robust in-motion alignment method.

Initialization: $L_j=15$, ${\mathit{\phi }}_b={\mathbf{0}}_{3\times 1}$$,{\mathit{\epsilon }}^b={\mathbf{0}}_{3\times 1}$$,\varphi =0.968$,${\lambda }_0=10$,${\mu }_{0|0}=5$. Inputs: ${\mathit{\omega }}_{ib}^b$$,{\mathit{f}}^b$$,{\mathit{V}}^n$

While($\mathrm{backtracking} \mathrm{window} \mathrm{size} L_j$< IMU data length) Forward alignment   for $k=L_{j-1}:1:L_j$

(1) $\mathrm{Update}{\mathit{C}}_{n(t)}^{n(0)}$$\mathrm{and} {\mathit{C}}_{\tilde{b}(t)}^{b(0)}$ using (2) and (21)   (2) $\mathrm{Construct} \mathrm{reference} \mathrm{vector} {\mathit{\alpha }}_{sw}$$\mathrm{and} \mathrm{observation} \mathrm{vector} {\mathit{\beta }}_{sw}$   (3) $\mathrm{Calculate} \mathrm{weighting} \mathrm{function} \Lambda ({\varsigma }_w)$   (4) $\mathrm{Reconstruct} \mathrm{observation} \mathrm{vector} {\overline{\mathit{\beta }}}_{sw}$   (5) $\mathrm{Calculate} {\mathit{C}}_{b(0)}^{n(0)}$ by (7)   Forward time update   (6) ${\mathit{x}}_{k|k-1}={\mathit{F}}_{k-1}{\mathit{x}}_{k-1}+{\mathit{w}}_{k-1}$$, {\mathit{P}}_{k|k-1}={\mathit{F}}_{k-1}{\mathit{P}}_{k-1}{\mathit{F}}_{k-1}^T+{\mathit{Q}}_k$   Forward measurement update   (7) Initialize prior parameters by (44)–(46)   (8) VB iteration      for 0:9         (a) $\mathrm{Update} {\widehat{\mathit{P}}}_{k|k-1}$ by (48)–(51)         (b) $\mathrm{Calculate} \mathrm{refined} \mathrm{prior} \mathrm{scale} \mathrm{matrix} {\overline{\mathit{\gamma }}}_k^{(i+1)}$         (c) $\mathrm{Update} {\widehat{\mathit{R}}}_k$ by (52)–(57)         (d) $\mathrm{Calculate} {\mathit{X}}_{k|k}^{(i+1)}$$\mathrm{and} {\mathit{P}}_{k|k}^{(i+1)}$ by (59)–(61)         end for   (9) Feedback the estimated parameters  end for

Backward alignment

(10) Reverse navigation data and filter parameters  for$\mathrm{k} = L_{j-1}$$:1: L_j$   (11) Calculate reverse SINS parameters   (12) Reverse time update by step 6)   (13) Reverse measurement update by step 7)–9)  end for   (14) $\mathrm{Calculate} \mathrm{next} \mathrm{backtracking} \mathrm{window} \mathrm{size} L_{j+1}$ by (36)   (15) $\mathrm{Update} \mathrm{backtracking} \mathrm{stage}: j=j+1$ end while return real-time attitude matrix ${\mathit{C}}_b^n(t)$

4. Results and Discussion

To verify the effectiveness of the proposed method, this section presents both simulation and field tests. In this paper, the Optimization-Based Alignment method (OBA) [18], the filter alignment method based on variational Bayesian Kalman filter (VBKF), the backtracking-based Kalman filter alignment method (BKF), the fuzzy adaptive Kalman filter-based method (FAKF) [37], and the proposed method are tested and compared.

4.1. Simulation Test

In this section, simulation analyses are carried out to evaluate the five methods described previously. The complete alignment process takes place over 100 s, starting with the vehicle positioned at 32.11°N and 119.37°E. Figure 3 and Figure 4 illustrate the vehicle’s trajectory and reference attitude, respectively. Additionally, Figure 5 illustrates the GNSS velocity output affected by outliers.

The sampling frequency of the three-axis gyroscopes and three-axis accelerometers in the SINS was set at 100 Hz. The constant biases of the gyroscopes were $0.1°/s$, and the gyroscopes’ random biases were $0.05°/\sqrt{s}$. The constant biases of the accelerometers were $500{\mu }_{\mathit{g}}$, and the acceleration random biases were $100{\mu }_{\mathit{g}}$. The GNSS sampling frequency was set at 1 Hz. Due to signal reflections from tunnel/high-rise scenes and occlusions by obstacles in complex urban environments, the noise in real-world GNSS measurements exhibits significant heavy-tailed behavior. Therefore, a Gaussian mixture noise model was adopted to simulate the noise in GNSS velocity output, which is represented as follows:

$$\delta {\mathit{v}}^n=\left\{\begin{array}{ll}N(0,{(0.1)}^2\mathit{I})\hfill & \hspace{1em}w.p\mathrm{.0.97}\hfill \\ N(0,{(50)}^2\mathit{I})\hfill & \hspace{1em}w.p\mathrm{.0.03}\hfill \end{array}\right.$$

(71)

Figure 6 and Figure 7 show the pitch and roll errors for the five methods under comparison. As shown in Figure 6 and Figure 7, the conventional OBA and VBKF methods exhibit divergence in pitch and roll, while the other three methods achieve convergence. This result mainly occurs because the OBA method neglects errors in the inertial sensors and does not consider the outliers in GNSS. Significant fluctuations are observed in the initial estimation results of both the VBKF and FAKF methods, mainly because sensor parameters have not been accurately estimated at this stage. In contrast, the proposed method achieves improved accuracy and stability from the early stage of alignment, owing to the adaptive multiple backtracking approach. The presence of outliers leads to varying degrees of performance degradation in the VBKF, BKF, and FAKF methods. Taking the pitch error as an example, at 75 s, the VBKF method exhibits a 2.5° fluctuation lasting for 3 s. The BKF method shows a 1.8° fluctuation over the same duration, and the FAKF method also experiences disturbances. While the convergence accuracy of the above methods is degraded by outliers, the proposed method exhibits strong robustness and maintains stable convergence throughout the alignment process.

As illustrated by the heading errors in Figure 8, the OBA and VBKF methods fail to achieve convergence during the alignment process. During the initial stage, the FAKF method exhibits significant estimation fluctuations lasting approximately 30 s. In contrast, the BKF method and the proposed method exhibit stable convergence characteristics in the initial stage. Experimental data show that the heading error of the BKF method is approximately 5°, and the proposed method is approximately 1°. During the later stage, the heading error of the proposed method stabilizes within 0.8°, whereas the FAKF and BKF methods stabilize within 5° and 4°, respectively. Although the FAKF method employs magnitude matching to mitigate the effect of outliers, the limited amount of IMU and GNSS data acquired during short-term in-motion alignment restricts its ability to achieve higher accuracy. The BKF method extends the navigation data to obtain a smoother alignment result by utilizing a standard backtracking approach. However, its performance remains susceptible to outliers and noise with varying statistical characteristics. In contrast, the proposed method maintains both stability and high accuracy throughout the alignment process. Even when the GNSS-aided velocity measurements contain outliers, the heading errors remain stable, without any abrupt jumps. This conclusion is further corroborated by the roll and pitch error curves shown in Figure 6 and Figure 7.

To demonstrate the performance of the aforementioned alignment methods more clearly,

Table 2. The 60~100 s attitude error statistical characteristics.

Evaluating Indicator	Attitude Error (°)	OBA	VBKF	BKF	FAKF	Proposed
Mean	Pitch	5.534	−2.956	0.530	0.636	−0.122
	Roll	4.653	1.561	0.405	0.669	0.257
	Heading	−27.446	−21.265	3.951	−1.890	0.337
STD	Pitch	0.274	1.081	0.566	0.765	0.049
	Roll	1.414	0.917	0.735	0.638	0.026
	Heading	18.719	3.898	0.468	1.074	0.037

summarizes the mean and standard deviation (STD) of the attitude errors over the interval from 60 s to 100 s. The proposed method achieves an STD of less than 0.05° across all three attitude angles, where the FAKF method is around 1° and the BKF method is around 0.7°. Based on the profile toolbox of MATLAB 2022a, the running time of the proposed method in the simulation test is 3.79 s. From these statistical results, it is obvious that the proposed method offers superior stability and higher alignment accuracy compared to the other methods. Enhanced stability benefits from the residual-based reconstruction of the observation vector, which enables effective outlier isolation and detection.

Additionally, the designed adaptive multiple backtracking approach not only extends the length of available IMU and GNSS data but also maintains the filter’s tracking sensitivity and unbiasedness, thereby fully exploiting the available information and shortening the alignment time. Furthermore, the EM-based robust Kalman filter enables real-time adjustment of the measurement noise covariance matrix and the predicted error covariance matrix, effectively addressing the issue of inaccurate noise modeling and enhancing the estimation accuracy.

4.2. Field Test

To further demonstrate the effectiveness and advantages of the proposed method, this section provides experimental validation using field test data. The test vehicle starts from an initial position at 39.7°N latitude and 116.5°E longitude, and the alignment process lasts for 120 s. The trajectory of the vehicle is shown in Figure 9. The experimental setup is illustrated in Figure 10, which comprises a Micro-Electromechanical Systems (MEMS) IMU, a fiber-optic SINS, and a GNSS receiver. The MEMS IMU employed is the MTI-G-710, with sensor specifications provided in

Table 3. MTI-G-710 sensor specifications.

Index	Gyroscope	Accelerometer
Update rate	100 Hz	100 Hz
Standard full range	±450°/s	±20 g
Initial bias error	0.2°/s	5 mg
In-run bias stability	10°/h	15 μg
Noise density	0.01°/s/√Hz	60 μg/√Hz

. Figure 11 presents the GNSS velocity output affected by outliers. Figure 12 shows the attitude output by the high-accuracy fiber-optic SINS/GNSS navigation system, which serves as the reference.

Figure 13, Figure 14 and Figure 15 present the attitude error curves for the five methods. The OBA method exhibits divergence in all three attitude angles. The pitch error of the VBKF method shows high variability, and the estimation accuracy is significantly lower compared to other methods. Furthermore, the roll and heading errors show divergent trends during the field test. In contrast, the remaining three methods achieve convergence in all three attitude angles. However, both the BKF and FAKF methods exhibit varying degrees of abrupt jumps as a result of outlier interference. The proposed method demonstrates a clear advantage in stability and accuracy. Taking the heading error as the critical evaluation indicator, it can be observed that after 80 s, the maximum heading error of the BKF method reached 7.3°, and that of the FAKF method reached 8°. In contrast, the proposed method stabilized the heading error within 1.2°.

Table 4. The 80~120 s attitude error statistical characteristics.

Evaluating Indicator	Attitude Error (°)	OBA	VBKF	BKF	FAKF	Proposed
Mean	Pitch	−1.068	0.668	−0.119	0.227	0.120
	Roll	24.393	1.902	0.058	−0.284	−0.088
	Heading	102.960	−39.164	−3.869	4.423	0.870
STD	Pitch	2.448	0.255	0.078	0.181	0.079
	Roll	5.158	0.391	0.214	0.187	0.079
	Heading	11.172	14.398	2.671	5.034	0.314

summarizes the statistical characteristics of the alignment results for the five methods over the interval from 80 s to 120 s. The proposed method achieves STD of less than 0.8° for roll and pitch errors, representing improvements of 63% and 57% over the BKF and FAKF methods, respectively. The heading error STD of the proposed method is 0.31°, which represents an improvement of over 2° compared to the other methods. In the later stages of the alignment process, around 95 and 115 s, the BKF and FAKF methods exhibited abrupt jumps due to outliers in the measurement data. In contrast, the proposed method maintains a more stable heading error around 0.8°. Based on the profile toolbox of MATLAB 2022a, the running time of the proposed method in the field test is 4.11 s. Overall, the proposed in-motion alignment algorithm demonstrates superior accuracy and enhanced robustness against disturbances.

The statistical results indicate that the proposed method significantly reduces the alignment time while achieving high alignment accuracy. This result can be attributed to two aspects. On the one hand, an adaptive window adjustment approach is applied to process the navigation data, in which an innovation gradient is used to distinguish windows with different noise statistical characteristics. By incorporating the multiple backtracking mechanism, this approach improves the utilization of navigation data, thereby effectively shortening the alignment time. On the other hand, the sliding window method mitigates the accumulation of sensor errors. By reconstructing the observation vector using the residual, the proposed method effectively isolates outliers from the GNSS velocity output. Meanwhile, the incorporation of the VB and EM methods mitigates the inaccuracies in the predicted error covariance matrix and the measurement noise covariance matrix, thereby enhancing the robustness of the filtering process in complex environments. The combination of these two aspects enables a reduction in in-motion alignment time and enhances alignment accuracy.

5. Conclusions

This paper proposes an adaptive multiple backtracking robust in-motion alignment method of low-cost SINS/GNSS for autonomous vehicles. An outlier suppression strategy based on the magnitude matching method is designed to mitigate the effect of outliers. An adaptive window adjustment multiple backtracking approach is proposed to effectively utilize navigation data and significantly shorten the alignment time in complex environments. By intelligently determining the size of the backtracking window, the proposed method effectively improves alignment accuracy. Finally, an EM-based robust variational Bayesian Kalman filter is proposed to improve attitude estimation accuracy. Simulation and experimental results demonstrate that the proposed method offers significant advantages in stability and alignment time. The proposed method completes alignment within 120 s and achieves higher alignment accuracy compared to other methods within the same alignment time. After 80 s, the heading error of the BKF method remains within 4°, while that of the FAKF method remains within 7°. In contrast, the proposed method converges with a heading error within 2.3°. Compared with existing methods, the proposed method can rapidly achieve the alignment accuracy required for navigation within a short duration. Hence, the proposed method is applicable to autonomous vehicles equipped with low-cost SINS. Reliable alignment under transient GNSS signal interruptions in complex environments will be addressed in future research.