A fault-tolerant sins/dual 2D-LDV tightly coupled integration scheme for autonomous vehicle navigation

Abstract

Strapdown inertial navigation systems (SINS) integrated with two-dimensional laser Doppler velocimeters (2D-LDVs) present a promising autonomous navigation solution for land vehicles, particularly in GNSS-denied environments. However, their performance is often degraded by vehicle sideslip and outliers in 2D-LDV measurements. This paper addresses these challenges by proposing a novel fault-tolerant SINS/Dual-2D-LDV tightly coupled integration scheme. In this scheme, two 2D-LDVs are integrated with SINS to create a redundant measurement model. This model utilizes the raw measurements from both LDVs along with the vehicle’s lateral zero-velocity constraint. To handle anomalies, a fault detection method based on the Local Outlier Factor (LOF) is introduced to identify measurement outliers and violations of the zero-velocity constraint. An adaptive filter, whose gain is dynamically adjusted by the LOF value, is then employed to mitigate the impact of these anomalies on the integrated navigation solution. The effectiveness and robustness of the proposed method are validated through two sets of long-distance vehicle experiments. Results demonstrate that the proposed scheme achieves superior positioning accuracy in both horizontal and vertical directions compared to traditional approaches. Furthermore, the LOF-based fault detection method proves to be more sensitive and effective in identifying anomalies than the traditional residual chi-squared detection method, enhancing the overall reliability of the system.

Introduction

The demand for high-precision, reliable, and real-time navigation in modern vehicles has surpassed the capabilities of single-sensor systems^1,2. Integrated navigation systems, which fuse data from multiple sensors, have emerged as the dominant solution to leverage the strengths of each sensor while mitigating their individual weaknesses.

For vehicle-mounted integrated navigation systems, various sensors are typically employed, including strapdown inertial navigation systems (SINS), global navigation satellite systems (GNSS), odometers (OD), barometers, LiDAR, cameras, and others^3,4,5,6. SINS offer autonomous, six-degree-of-freedom navigation data, but their accuracy degrades over time due to initial alignment errors and sensor drift⁷. GNSS provide continuous, high-precision positioning, but as a satellite-based system, its signals are vulnerable to interference and obstruction, leading to significant performance degradation in challenging environments⁸.

For decades, the integration of SINS and GNSS has been the most effective and widely adopted method for high-quality land vehicle navigation. However, the system’s reliability is severely compromised in scenarios with poor satellite visibility, such as tunnels and urban canyons. To address this, OD are frequently used to provide velocity and distance measurements, maintaining positioning capabilities when GNSS signals are unavailable. While ODs are fully autonomous, their accuracy is susceptible to wheel-related issues like skidding, tire pressure, and wear. Other sensors, such as barometers, LiDAR, and cameras, also face limitations from environmental conditions and vehicle dynamics, which can affect their reliability^4,9.

The critical vulnerability of GNSS in specific environments highlights a fundamental challenge for autonomous navigation: the need for a robust, reliable, and high-precision solution that can operate independently of satellite signals. While SINS/OD integration is a common approach, the limitations of mechanical odometers—particularly their susceptibility to measurement inaccuracies from wheel dynamics—create a persistent need for more reliable alternatives¹⁰.

In recent years, Laser Doppler velocimeter (LDV), widely acclaimed in the field of metrology, has been used for the autonomous navigation of land vehicles. LDV emits a laser beam to the ground and determines the velocity of the vehicle by measuring the Doppler shift in the scattered light. This non-contact measurement mode makes the accuracy of LDV independent of vehicle slippage and wheel conditions, resulting in higher measurement accuracy than OD¹¹. Numerous studies have demonstrated the effectiveness and superiority of SINS/LDV integration^12,13,14,15.

Most currently used SINS/LDV integrations utilize one-dimensional LDV (1D-LDV), which only measures the forward velocity of the vehicle¹⁶. The 3D velocity measurement of the vehicle is achieved by non-holonomic constraints (NHC), which assume that the vehicle does not slip or jump during driving, keeping the lateral and vertical velocities at zero¹⁷. However, actual driving conditions often cause bumps, violating the NHC and reducing the height estimation accuracy of the integrated navigation system. Additionally, for both SINS/OD and SINS/1D-LDV integration, factors such as vehicle load changes, ground conditions, and tire inflation coefficients can change the pitch misalignment angle between the vehicle frame and the body frame, indirectly reducing the height estimation accuracy¹². To improve height estimation accuracy in SINS/1D-LDV integration, Wang et al. proposed a Schmidt state-transform Kalman filter algorithm, which enhances the observability of the vertical channel and improves height positioning accuracy¹³. Chiang et al. introduced a barometer into the integrated navigation system, significantly enhancing the height accuracy¹⁸. Although these algorithmic improvements and additional barometers can improve height estimation accuracy, they also increase algorithm complexity and economic cost. To address these issues, our prior research improved the optical path structure and modeling method of the LDV, enabling it to measure both forward and vertical velocities simultaneously, resulting in a 2D LDV^19,20. Compared to SINS/1D-LDV integration, SINS/2D-LDV integration significantly improves height accuracy^21,22,23.

In summary, LDV, particularly 2D-LDV is well-suited for constraining SINS errors in environments without GNSS signals. Previous studies by Du et al. and Xiang et al. on SINS/GNSS/1D-LDV and SINS/GNSS/2D-LDV integration, respectively, demonstrated that SINS/GNSS/LDV integration can maintain high positioning accuracy even in GNSS-obstructed environments, such as urban canyons^22,24. Additionally, Fu et al. investigated a high-precision SINS/1D-LDV integration under GNSS-denied conditions, accounting for the installation misalignment between SINS and LDV, as well as the influence of the lever arm, and analyzed system observability¹². In a 230 km long-distance experiment, this system achieved positioning accuracy better than 0.02% of the total mileage in both horizontal and vertical directions. For special applications, such as military vehicles that must operate without GNSS, the reliability of the integrated navigation system is crucial. Although SINS/LDV integration provides high-precision autonomous navigation, one of its key requirements is that the LDV operates normally and is not affected by measurement outliers. The SINS/1D-LDV integration cannot handle prolonged LDV failures or abnormal outputs. To address this limitation, Wang et al. introduced an OD to form a SINS/OD/1D-LDV integration, using a federated Kalman filter to fuse the data from all three sensors²⁵. Furthermore, in reference¹³a centralized state transformation Kalman filter was applied to fuse SINS, OD, and 1D-LDV data, significantly improving accuracy over SINS/OD and SINS/1D-LDV integrations. For 2D-LDV, which uses multiple measurement beams, raw velocity data from each beam can directly constrain SINS errors in a tightly coupled integration, improving the robustness of the system^22,23. However, it can only handle short-term failure of a single 2D-LDV beam. If both beams fail simultaneously or a single beam fails for an extended period, the system degrades to pure inertial navigation, causing rapid error divergence. Moreover, 2D-LDV cannot measure lateral velocity, so SINS/2D-LDV integration still relies on the lateral zero-velocity constraint from the NHC. Any violation of this constraint negatively impacts the system’s performance.

To enhance the reliability of autonomous navigation for land vehicles and address the limitations of existing SINS/2D-LDV integration methods, this paper proposes a fault-tolerant SINS/Dual-2D-LDV tightly coupled integration scheme. The primary contributions of this work are as follows:

• (1) A Fault-Tolerant SINS/Dual 2D-LDV Tightly Coupled Integration Scheme: Unlike previous SINS/single 2D-LDV schemes, this paper utilizes the raw measurements from two 2D-LDVs and a lateral zero-velocity constraint. This creates a redundant and multi-constrained tightly coupled measurement model, which is then combined with a fault diagnosis method and an adaptive filter to handle vehicle sideslip and 2D-LDV outliers.

• (2) A LOF-Based Adaptive Filtering Strategy: The LOF algorithm is applied to simultaneously detect measurement outliers and violations of the zero-sideslip constraint. This differs from traditional residual-based chi-squared test methods and is more effective at handling nonlinear and complex outlier scenarios. Furthermore, the LOF value is used as an adaptive factor to dynamically adjust the filter’s gain based on the severity of the outlier, allowing for more precise fault-tolerant control.

• (3) Dual-Verification: The effectiveness of the proposed method is comprehensively validated through two long-distance real-vehicle experiments conducted in different environments.

The subsequent sections of this paper are structured as follows: Sect. Proposed integration scheme introduces the proposed fault-tolerant SINS/Dual-2D-LDV tightly coupled integration scheme. Section Experiment results and analysis validates and analyzes the effectiveness of the proposed scheme through two sets of long-distance vehicle experiments. Section Conclusion provides the conclusions of this paper.

Proposed integration scheme

Before describing the proposed integrated scheme, the coordinate frames used in this paper are defined. The east-north-up geographic frame is selected as the navigation frame (n frame) for SINS calculations. The inertial measurement unit (IMU) body frame, determined by the calibrated sensitive axes of the inertial sensors, is referred to as the b frame, with its origin at the IMU’s sensitive center and its axes oriented rightward, forward, and upward, respectively. Similarly, the body frames of the first and second LDVs are denoted as the ${m_1}$ frame and ${m_2}$ frame, respectively, with their origins at the LDV’s measuring centers and their axes directed rightward, forward, and upward, respectively. The LDV beam frames for the first and second LDVs, defined by the beams of each LDV (comprising two actual beams and one virtual beam), are denoted as the $bea{m_1}$ frame and $bea{m_2}$ frame, respectively.

Fig. 1

Designed system architecture for SINS/ Dual-2D-LDV integration.

The SINS/Dual-2D-LDV tightly coupled integration scheme is shown in Fig. 1. This scheme comprises three primary components: First, the SINS/Dual-2D-LDV tightly coupled model is constructed, where the SINS is tightly coupled with each of the two 2D-LDVs to calculates the velocity difference between the two 2D-LDVs and the SINS within the $bea{m_1}$ frame and $bea{m_2}$ frame. Second, fault diagnosis is performed based on the velocity difference obtained in the first step. The LOF is used to detect the six components of the velocity differences, yielding the LOF value that reflects the degree of data abnormality. Finally, the velocity difference obtained in the first step and the LOF value obtained in the second step are input into a sequential adaptive Kalman filter. In this filter, the velocity difference serves as the measurement value, and each value is processed individually. Different processing strategies—normal filtering, adaptive filtering, or isolation—are applied based on the magnitude of the LOF value. A detailed explanation of these three components is provided below.

SINS/Dual-2D-LDV tightly coupled model

Error model analysis

The 2D-LDV features two intersecting measurement beams. Each beam measures the Doppler shift to obtain the 1D velocity along its direction. Based on the geometric relationship between the two measurement beams, the 2D velocity can be determined, as detailed in reference²¹. Figure 2 illustrates a commonly used optical path structure of a 2D-LDV, the beam relationship of 2D-LDV, and the geometric relationship between the beam frame and the 2D-LDV body frame (m frame) based on this optical path structure.

Fig. 2

(a) Optical path structure of the 2D-LDV. (b) The beam relationship of 2D-LDV. (c) Geometric relationship between the beam frame and m frame.

As shown in Fig. 2, the two measurement beams of the 2D-LDV are emitted at inclination angles ${\theta _1}$ and ${\theta _2}$, with corresponding measured velocities ${\upsilon _{beam1}}$ and ${\upsilon _{beam2}}$. The virtual beam is perpendicular to the plane formed by these two measurement beams, with a velocity ${\upsilon _{beam3}}$ of zero in its direction to satisfy the lateral zero-velocity constraint of land vehicles. Together, the two measurement beams and the virtual beam form the beam frame of the 2D-LDV. The $m^{\prime}$ frame is directly defined based on the geometric relationship between the two measurement beams of the 2D-LDV, as follows: the angle bisector between the two measurement beams serves as the Z-axis, with the upward direction being positive; the Y-axis is perpendicular to the Z-axis and lies in the plane formed by the two measurement beams, with the forward direction being positive; and the X-axis is determined by the right-hand rule, based on the Y-axis and Z-axis. According to the definition of the $m^{\prime}$ frame, there are defined transformation relationships between the $m^{\prime}$ frame, the beam frame, and the m frame. The transformation matrix ${\varvec{C}}_{m}^{{m^{\prime}}}$ from the m frame to the $m^{\prime}$ frame and the transformation matrix ${\varvec{C}}_{{m^{\prime}}}^{{beam}}$ from the $m^{\prime}$ frame to the beam frame are expressed as follows, according to reference²¹:

$${\varvec{C}}_{m}^{{m^{\prime}}}={\left[ {\begin{array}{*{20}{c}} 1&0&0 \\ 0&{\cos \alpha }&{ - \sin \alpha } \\ 0&{\sin \alpha }&{\cos \alpha } \end{array}} \right]^T}$$

(1)

$${\varvec{C}}_{{m^{\prime}}}^{{beam}}=\left[ {\begin{array}{*{20}{c}} 0&{\sin \theta }&{ - \cos \theta } \\ 0&{ - \sin \theta }&{ - \cos \theta } \\ 1&0&0 \end{array}} \right]$$

(2)

where $\theta$ denotes the angle between the angular bisector of the two measurement beams and each of the beams. $\alpha$ represents the deviation angle between the $m^{\prime}$ frame and the m frame, which is determined by the inclination angles of the two measurement beams of the 2D-LDV, and is expressed as follows:

$$\alpha =\frac{{\pi - \left( {{\theta _1}+{\theta _2}} \right)}}{2}$$

(3)

According to Eqs. (1) and (2), the following transformation relationship exists between the velocities in the beam frame and the n frame:

$${\varvec{\upsilon}^{beam}}={\left[ {\begin{array}{*{20}{c}} {{\upsilon _{beam1}}}&{{\upsilon _{beam2}}}&{{\upsilon _{beam3}}} \end{array}} \right]^T}={\varvec{C}}_{{m^{\prime}}}^{{beam}}{\varvec{C}}_{m}^{{m^{\prime}}}{\varvec{C}}_{b}^{m}{\varvec{C}}_{n}^{b}{\varvec{\upsilon}^n}$$

(4)

where ${\varvec{\upsilon}^{beam}}$and ${\varvec{\upsilon}^n}$ are the true velocities in the beam frame and n frame, respectively. ${\varvec{C}}_{b}^{m}$ and ${\varvec{C}}_{n}^{b}$ are the attitude transformation matrices from the b frame to the m frame and from the n frame to the b frame, respectively.

In practice, when considering the attitude errors of SINS, the mounting misalignment angle error between the b frame and the m frame, and the deviation of the actual beam inclination of the 2D-LDV from its design value, the used $\tilde {{\varvec{C}}}_{{m^{\prime}}}^{{beam}}$, $\tilde {{\varvec{C}}}_{b}^{m}$, and $\tilde {{\varvec{C}}}_{n}^{b}$ satisfy the following equations:

$$\tilde {{\varvec{C}}}_{{m^{\prime}}}^{{beam}}=\left[ {\begin{array}{*{20}{c}} 0&{\sin \theta }&{ - \cos \theta } \\ 0&{ - \sin \theta }&{ - \cos \theta } \\ 1&0&0 \end{array}} \right]+\left[ {\begin{array}{*{20}{c}} 0&{\cos \theta }&{\sin \theta } \\ 0&{ - \cos \theta }&{\sin \theta } \\ 0&0&0 \end{array}} \right]\Delta \theta ={\varvec{C}}_{{m^{\prime}}}^{{beam}}+{{\varvec{C}}_\theta }\Delta \theta$$

(5)

$$\tilde {{\varvec{C}}}_{b}^{m} \approx \left( {{{\varvec{I}}_3}+\delta {\varvec{\phi}_m} \times } \right){\varvec{C}}_{b}^{m}$$

(6)

$$\tilde {{\varvec{C}}}_{n}^{b} \approx {\varvec{C}}_{n}^{b}\left( {{{\varvec{I}}_3}+\varvec{\varphi} \times } \right)$$

(7)

where $\Delta \theta$ denotes the deviation between the design value of $\theta$ and its actual value, ${{\varvec{I}}_3}$ is the identity matrix of size three, $\left( \cdot \right) \times$ is employed to resolve the antisymmetric matrix, $\varvec{\varphi}$ represents the attitude error of the SINS, and $\delta {\varvec{\phi}_m}={\left[ {\begin{array}{*{20}{c}} {\delta {\phi _{mx}}}&{\delta {\phi _{my}}}&{\delta {\phi _{mz}}} \end{array}} \right]^T}$ denotes the residual mounting misalignment angle error after calibration of the SINS/2D-LDV integrated navigation system.

It is worth noting that the influence of $\Delta \theta$ on $\alpha$ is not considered in ${\varvec{C}}_{m}^{{m^{\prime}}}$, as this influence will be reflected in the pitch mounting misalignment angle error term in $\delta {\varvec{\phi}_m}$.

Based on Eqs. (4)-(7), the velocity of SINS in the beam frame can be expressed as:

$$\begin{gathered} \tilde {\varvec{\upsilon}}_{{SINS}}^{{beam}}=\tilde {{\varvec{C}}}_{{m^{\prime}}}^{{beam}}{\varvec{C}}_{m}^{{m^{\prime}}}\tilde {{\varvec{C}}}_{b}^{m}\tilde {{\varvec{C}}}_{n}^{b}\tilde {\varvec{\upsilon}}_{{SINS}}^{n} \hfill \\ \;\;\;\;\;\;\; \approx {\varvec{\upsilon}^{beam}}+{\varvec{C}}_{{m^{\prime}}}^{{beam}}{\varvec{C}}_{m}^{{m^{\prime}}}\tilde {{\varvec{C}}}_{b}^{m}{\varvec{C}}_{n}^{b}\delta \varvec{\upsilon}_{{SINS}}^{n} - {\varvec{C}}_{{m^{\prime}}}^{{beam}}{\varvec{C}}_{m}^{{m^{\prime}}}\tilde {{\varvec{C}}}_{b}^{m}{\varvec{C}}_{n}^{b}\left( {{\varvec{\upsilon}^n} \times } \right)\varvec{\varphi} - {\varvec{C}}_{{m^{\prime}}}^{{beam}}{\varvec{C}}_{m}^{{m^{\prime}}}\left( {{\varvec{\upsilon}^m} \times } \right)\delta {\varvec{\phi}_m}+{{\varvec{C}}_\theta }{\varvec{C}}_{m}^{{m^{\prime}}}\tilde {{\varvec{C}}}_{b}^{m}{\varvec{C}}_{n}^{b}{\varvec{\upsilon}^n}\Delta \theta \hfill \\ \end{gathered}$$

(8)

where $\tilde {\varvec{\upsilon}}_{{SINS}}^{n}$ and $\delta \varvec{\upsilon}_{{SINS}}^{n}$ denote the velocity and velocity error of SINS in the n frame, respectively, and satisfy the equation $\tilde {\varvec{\upsilon}}_{{SINS}}^{n}={\varvec{\upsilon}^n}+\delta \varvec{\upsilon}_{{SINS}}^{n}$. ${\varvec{\upsilon}^m}$ is the true velocity in the m frame.

When the mounting misalignment angle between the b frame and the m frame is sufficiently small—a condition that is easily satisfied—pre-calibration of the SINS/2D-LDV integration may not be required, and in this case, $\tilde {{\varvec{C}}}_{b}^{m}={{\varvec{I}}_3}$ and ${\varvec{\upsilon}^m} \approx {\varvec{\upsilon}^b}$.

SINS/Dual-2D-LDV tightly coupled state equation

The error state vector of the SINS/Dual-2D-LDV tightly coupled integration is defined as:

$${{\varvec{x}}_k}={\left[ {\begin{array}{*{20}{c}} {{\varvec{x}}_{{SINS}}^{T}}&{{\varvec{x}}_{{LDV1}}^{T}}&{{\varvec{x}}_{{LDV2}}^{T}} \end{array}} \right]^T}$$

(9)

where

$${{\varvec{x}}_{SINS}}={\left[ {\begin{array}{*{20}{c}} {{\varvec{\varphi}^T}}&{{{\left( {\delta \varvec{\upsilon}_{{SINS}}^{n}} \right)}^T}}&{{{\left( {\delta {{\varvec{p}}_{SINS}}} \right)}^T}}&{{{\left( {\varvec{\varepsilon}_{{ib}}^{b}} \right)}^T}}&{{{\left( {\nabla _{{ib}}^{b}} \right)}^T}} \end{array}} \right]^T}$$

(10)

$${{\varvec{x}}_{LDV1}}={\left[ {\begin{array}{*{20}{c}} {\delta \varvec{\phi}_{{m - LDV1}}^{T}}&{\Delta {\theta _{LDV1}}} \end{array}} \right]^T}={\left[ {\begin{array}{*{20}{c}} {\delta {\phi _{mx - LDV1}}}&{\delta {\phi _{my - LDV1}}}&{\delta {\phi _{mz - LDV1}}}&{\Delta {\theta _{LDV1}}} \end{array}} \right]^T}$$

(11)

$${{\varvec{x}}_{LDV2}}={\left[ {\begin{array}{*{20}{c}} {\delta \varvec{\phi}_{{m - LDV2}}^{T}}&{\Delta {\theta _{LDV2}}} \end{array}} \right]^T}={\left[ {\begin{array}{*{20}{c}} {\delta {\phi _{mx - LDV2}}}&{\delta {\phi _{my - LDV2}}}&{\delta {\phi _{mz - LDV2}}}&{\Delta {\theta _{LDV2}}} \end{array}} \right]^T}$$

(12)

where $\delta {{\varvec{p}}_{SINS}}$ represents the position error of the SINS, while $\varvec{\varepsilon}_{{ib}}^{b}$ and $\nabla _{{ib}}^{b}$ denote the constant biases of the gyroscopes and accelerometers, respectively. The subscripts LDV1 and LDV2 refer to the two 2D-LDVs used in the SINS/Dual-2D-LDV tightly coupled integration. Given that the 2D-LDV and IMU are rigidly fixed to the vehicle, and that the lasers used in LDVs generally exhibit very high beam stability, both $\delta {\varvec{\phi}_m}$ and $\Delta \theta$ can be treated as random constants. The corresponding error equations are as follows:

$$\begin{array}{*{20}{c}} {\delta {{\dot {\varvec{\phi}}}_{m - LDV1}}={0_{3 \times 1}}} \\ {\Delta {{\dot {\theta }}_{LDV1}}=0} \\ {\delta {{\dot {\varvec{\phi}}}_{m - LDV2}}={0_{3 \times 1}}} \\ {\Delta {{\dot {\theta }}_{LDV2}}=0} \end{array}$$

(13)

Based on Eq. (13) and the error model of SINS, the error state equation of SINS/Dual-2D-LDV tightly coupled integration can be written as

$${\dot {{\varvec{x}}}_k}={{\varvec{F}}_k}{{\varvec{x}}_k}+{{\varvec{G}}_k}{{\varvec{w}}_k}$$

(14)

where ${{\varvec{F}}_k}$, ${{\varvec{G}}_k}$, and ${{\varvec{w}}_k}$ denote the system state transition matrix, the noise transfer matrix, and the system noise vector, respectively, and are expressed as follows:

$${{\varvec{F}}_k}=\left[ {\begin{array}{*{20}{c}} {{{\varvec{F}}_{SINS}}}&{{0_{15 \times 8}}} \\ {{0_{8 \times 15}}}&{{0_{8 \times 8}}} \end{array}} \right]$$

(15)

$${{\varvec{G}}_k}=\left[ {\begin{array}{*{20}{c}} { - {\varvec{C}}_{b}^{n}}&{{0_{3 \times 3}}} \\ {{0_{3 \times 3}}}&{{\varvec{C}}_{b}^{n}} \\ {{0_{17 \times 3}}}&{{0_{17 \times 3}}} \end{array}} \right]$$

(16)

$${{\varvec{w}}_k}=\left[ {\begin{array}{*{20}{c}} {\varvec{\varepsilon}_{w}^{b}} \\ {\nabla _{w}^{b}} \end{array}} \right]$$

(17)

where ${{\varvec{F}}_{SINS}}$ is the state transfer matrix derived from the classical 15-dimensional SINS error model²⁶.$\varvec{\varepsilon}_{w}^{b}$ and $\nabla _{w}^{b}$ denote the gyroscope and accelerometer noise, respectively.

SINS/Dual-2D-LDV tightly coupled measurement equation

For the SINS/Dual-2D-LDV tightly coupled integration, the velocity difference between the 2D-LDV and SINS in the beam frame is used as the measurement value. The measurement equations for this system are constructed as follows, based on Eqs. (4) and (8):

$${{\varvec{z}}_k}=\left[ {\begin{array}{*{20}{c}} {\tilde {\varvec{\upsilon}}_{{LDV1}}^{{bea{m_1}}} - \tilde {\varvec{\upsilon}}_{{SINS}}^{{bea{m_1}}}} \\ {\tilde {\varvec{\upsilon}}_{{LDV2}}^{{bea{m_2}}} - \tilde {\varvec{\upsilon}}_{{SINS}}^{{bea{m_2}}}} \end{array}} \right] \approx \left[ {\begin{array}{*{20}{c}} {{\varvec{\upsilon}^{bea{m_1}}} - \tilde {\varvec{\upsilon}}_{{SINS}}^{{bea{m_1}}}} \\ {{\varvec{\upsilon}^{bea{m_2}}} - \tilde {\varvec{\upsilon}}_{{SINS}}^{{bea{m_2}}}} \end{array}} \right]={{\varvec{H}}_k}{{\varvec{x}}_k}+{{\varvec{v}}_k}$$

(18)

where ${{\varvec{z}}_k}$ represents the measurement vector, while ${{\varvec{v}}_k}$ denotes the measurement noise, which is zero-mean Gaussian white noise, and ${{\varvec{H}}_k}$ is the measurement transition matrix, expressed as follows:

$$\begin{gathered} {{\varvec{H}}_k}=\left[ {\begin{array}{*{20}{c}} {{\varvec{C}}_{{{{m^{\prime}}_1}}}^{{bea{m_1}}}{\varvec{C}}_{{{m_1}}}^{{{{m^{\prime}}_1}}}\tilde {{\varvec{C}}}_{b}^{{{m_1}}}{\varvec{C}}_{n}^{b}\left( {{\varvec{\upsilon}^n} \times } \right)}&{ - {\varvec{C}}_{{{{m^{\prime}}_1}}}^{{bea{m_1}}}{\varvec{C}}_{{{m_1}}}^{{{{m^{\prime}}_1}}}\tilde {{\varvec{C}}}_{b}^{{{m_1}}}{\varvec{C}}_{n}^{b}}&{{0_{3 \times 9}}} \\ {{\varvec{C}}_{{{{m^{\prime}}_2}}}^{{bea{m_2}}}{\varvec{C}}_{{{m_2}}}^{{{{m^{\prime}}_2}}}\tilde {{\varvec{C}}}_{b}^{{{m_2}}}{\varvec{C}}_{n}^{b}\left( {{\varvec{\upsilon}^n} \times } \right)}&{ - {\varvec{C}}_{{{{m^{\prime}}_2}}}^{{bea{m_2}}}{\varvec{C}}_{{{m_2}}}^{{{{m^{\prime}}_2}}}\tilde {{\varvec{C}}}_{b}^{{{m_2}}}{\varvec{C}}_{n}^{b}}&{{0_{3 \times 9}}} \end{array}} \right. \hfill \\ \;\;\;\;\;\;\;\;\;\;\left. {\begin{array}{*{20}{c}} {{\varvec{C}}_{{{{m^{\prime}}_1}}}^{{bea{m_1}}}{\varvec{C}}_{{{m_1}}}^{{{{m^{\prime}}_1}}}\left( {{\varvec{\upsilon}^{{m_1}}} \times } \right)}&{ - {{\varvec{C}}_{{\theta _1}}}{\varvec{C}}_{{{m_1}}}^{{{{m^{\prime}}_1}}}\tilde {{\varvec{C}}}_{b}^{{{m_1}}}{\varvec{C}}_{n}^{b}{\varvec{\upsilon}^n}}&{{0_{3 \times 3}}}&{{0_{3 \times 1}}} \\ {{0_{3 \times 3}}}&{{0_{3 \times 1}}}&{{\varvec{C}}_{{{{m^{\prime}}_2}}}^{{bea{m_2}}}{\varvec{C}}_{{{m_2}}}^{{{{m^{\prime}}_2}}}\left( {{\varvec{\upsilon}^{{m_2}}} \times } \right)}&{ - {{\varvec{C}}_{{\theta _2}}}{\varvec{C}}_{{{m_2}}}^{{{{m^{\prime}}_2}}}\tilde {{\varvec{C}}}_{b}^{{{m_2}}}{\varvec{C}}_{n}^{b}{\varvec{\upsilon}^n}} \end{array}} \right] \hfill \\ \end{gathered}$$

(19)

Fault detection

For the SINS/2D-LDV tightly coupled integration, when the measurement value of a single measurement beam of 2D-LDV is abnormal, the measurement value of the remaining normal measurement beam will still partially limit the velocity error of SINS. This presents a clear advantage over the SINS/2D-LDV loosely coupled integration. Therefore, the SINS/Dual-2D-LDV tightly coupled integration, which employs two 2D-LDVs, can better constrain the SINS velocity error by utilizing more normal velocity measurements when some beams of the 2D-LDV exhibit anomalies, thereby achieving higher reliability than the SINS/2D-LDV tightly coupled integration. However, measurement anomalies in the 2D-LDVs are not detected and processed, leaving their influence on the system unaddressed. Additionally, since the 2D-LDV cannot measure the lateral velocity of the vehicle, the lateral velocity measurement of the vehicle is typically realized using NHC lateral zero-velocity constraint. Consequently, any violation of this constraint will affect the performance of the SINS/Dual-2D-LDV tightly coupled integration. In summary, detecting NHC lateral zero-velocity constraint violations and 2D-LDV outliers, and appropriately mitigating their effects, is crucial for maintaining the reliability and accuracy of the integrated navigation system.

Local outlier factor

The residual chi-square test method, based on the innovation vector Mahalanobis distance, is among the most widely used outlier detection techniques in integrated navigation systems. Its detection performance is closely related to the test statistic and threshold. While this method performs well in detecting abrupt faults, it is insufficiently sensitive to slow-varying faults. In 2D-LDVs, faults initially manifest themselves as small and slowly changing, resulting in small residuals that are difficult to detect. Additionally, the residual chi-square test assumes that measurement errors follow a Gaussian distribution. However, the measurement accuracy of the 2D-LDV is closely related to the scattered light signal received by the internal detector, which can be influenced by factors such as ground conditions, lens cleanliness, the distance between the detector and the scattering point, and the operational status of the laser. When the scattered light signals are weak, the error distribution may deviate from the Gaussian assumption, leading to missed or false detections when thresholds are based on the preset chi-square distribution. Thus, it is necessary to develop new methods to detect slow-varying faults and non-Gaussian error distributions in the SINS/Dual-2D-LDV tightly coupled integration, and to reduce the constraints of assuming a specific statistical distribution on fault detection capability. The LOF method, which assesses the degree of an outlier based on the density between an object and its neighboring objects, is a density-based outlier detection technique. LOF does not require any specific distribution assumptions and has been widely applied in power system testing and industrial process monitoring^27,28,29. The specific calculation process of LOF is as follows³⁰:

Step 1: Define the K-distance: Given a dataset D, an object p, and a positive integer K, the K-distance of p is defined as the distance from the Kth nearest object q to the p in the D to p, denoted as $Kdis\left( p \right)$.

Step 2: Define the K-distance neighborhood: Given the K-distance $Kdis\left( p \right)$ of p, the K-distance neighborhood of p is the set of objects in D whose distance to p is not greater than $Kdis\left( p \right)$, i.e.,

$${N_k}\left( p \right)=\left\{ {q \in D\backslash \left\{ p \right\}|dis\left( {p,q} \right) \leqslant Kdis\left( p \right)} \right\}$$

(20)

where $dis\left( {p,q} \right)$ denotes the distance between p and q.

Step 3: Define the reachable distance: Given the K-distance $Kdis\left( q \right)$ of q, the reachable distance of p with respect to q is defined as the maximum of the $Kdis\left( q \right)$ and the distance between the p and q, denoted as:

$$reach - di{s_k}\left( {p,q} \right)=\hbox{max} \left\{ {Kdis\left( q \right),dis\left( {p,q} \right)} \right\}$$

(21)

Step 4: Based on the above definitions, the local reachable density of p is defined as the reciprocal of the average reachable distance of its K-nearest neighbors, denoted as:

$$lr{d_k}\left( p \right)={\left( {\frac{1}{k}\sum\limits_{{q \in {N_k}\left( p \right)}} {reach - di{s_k}\left( {p,q} \right)} } \right)^{ - 1}}$$

(22)

Step 5: Based on the definition of the local reachable density, the LOF value of p is calculated to evaluate its abnormal degree, denoted as:

$$LO{F_k}\left( p \right)=\frac{1}{k}\sum\limits_{{q \in {N_k}\left( p \right)}} {\frac{{lr{d_k}\left( q \right)}}{{lr{d_k}\left( p \right)}}}$$

(23)

From the above LOF calculation steps, it can be seen that the LOF of p is the average of the ratios between the local reachability density of p and that of its K-nearest neighbors. The local reachability density reflects the degree of object aggregation within a specified neighborhood. If p is a normal value, it does not significantly deviate from its neighboring objects, and its local reachable density is at the same level as that of its neighboring normal values, resulting in an LOF value close to 1. In contrast, if p is an outlier, it will deviate substantially from its neighbors, and its reachable distance to its neighboring objects is often larger. This leads to a lower local reachable density for p compared to other normal objects, resulting in an LOF value significantly greater than 1.

Fault detection solution

Since the measurement errors of the filter are reflected in the filter innovation vector, the filter innovation vector is chosen as the evaluation object for the LOF. The filter innovation vector is denoted as:

$${{\varvec{e}}_k}={{\varvec{z}}_k} - {{\varvec{H}}_k}{{\varvec{x}}_{k|k - 1}}$$

(24)

where ${{\varvec{x}}_{k|k - 1}}$ represents the one-step prediction of system state in the Kalman filter.

Based on the filter innovation vector, the dataset required for LOF is constructed as follows:

$${\varvec{D}}=\left[ {\begin{array}{*{20}{c}} {{{\varvec{D}}_1}} \\ {{{\varvec{D}}_2}} \\ {{{\varvec{D}}_3}} \\ {{{\varvec{D}}_4}} \\ {{{\varvec{D}}_5}} \\ {{{\varvec{D}}_6}} \end{array}} \right]=\left[ {\begin{array}{*{20}{c}} {{{\varvec{e}}_{k - N+1}}\left( 1 \right)}&{{{\varvec{e}}_{k - N+2}}\left( 1 \right)}& \cdots &{{{\varvec{e}}_k}\left( 1 \right)} \\ {{{\varvec{e}}_{k - N+1}}\left( 2 \right)}&{{{\varvec{e}}_{k - N+2}}\left( 2 \right)}& \cdots &{{{\varvec{e}}_k}\left( 2 \right)} \\ {{{\varvec{e}}_{k - N+1}}\left( 3 \right)}&{{{\varvec{e}}_{k - N+2}}\left( 3 \right)}& \cdots &{{{\varvec{e}}_k}\left( 3 \right)} \\ {{{\varvec{e}}_{k - N+1}}\left( 4 \right)}&{{{\varvec{e}}_{k - N+2}}\left( 4 \right)}& \cdots &{{{\varvec{e}}_k}\left( 4 \right)} \\ {{{\varvec{e}}_{k - N+1}}\left( 5 \right)}&{{{\varvec{e}}_{k - N+2}}\left( 5 \right)}& \cdots &{{{\varvec{e}}_k}\left( 5 \right)} \\ {{{\varvec{e}}_{k - N+1}}\left( 6 \right)}&{{{\varvec{e}}_{k - N+2}}\left( 6 \right)}& \cdots &{{{\varvec{e}}_k}\left( 6 \right)} \end{array}} \right]$$

(25)

where N denotes the set sample size, i.e., the window size of the dataset.

From the implementation of the LOF algorithm, it’s clear that the frequent distance calculations between data points lead to high time complexity. For SINS/Dual-2D-LDV tightly coupled integration, with a data update frequency of 100 Hz, this presents a significant challenge. While reducing the data window size can lower the computational load, it may also cause LOF to miss or incorrectly detect outliers, especially when continuous outliers exceed the neighborhood value, K. To balance computation speed and detection accuracy, we use historical normal data to construct a dataset, ${{\varvec{D}}_h}$. During real-time operations, we compare the new filter innovation vector with ${{\varvec{D}}_h}$ to identify abnormal measurements. To further enhance LOF’s detection accuracy, we classify the data in ${{\varvec{D}}_h}$ based on the vehicle’s forward velocity. This classification strategy is crucial for accounting for the impact of vehicle maneuvers on measurement accuracy. Greater vehicle maneuvers can increase measurement noise due to effects like Doppler frequency spread and a reduced number of sampling points within a signal period. Additionally, aggressive maneuvers can increase vehicle jolting, further compromising the LDV’s measurement precision. Based on these potential issues, we build our dataset from historical normal filter innovation vectors and classify them according to the vehicle’s forward velocity. This allows us to compare the current innovation vector against a more relevant and specific dataset to identify abnormal measurements. Different classification standards are applied for the dataset components related to 2D-LDV beam measurements and those related to the NHC lateral zero-velocity constraint, as shown below:

$$\begin{gathered} {{\varvec{D}}_{hj\left( {j=1,2,4,5} \right)}}=\left\{ {{{\varvec{D}}_{hj}}\left( {{\upsilon _{for}}=0} \right),} \right.{{\varvec{D}}_{hj}}\left( {0<{\upsilon _{for}} \leqslant 5} \right),{{\varvec{D}}_{hj}}\left( {5<{\upsilon _{for}} \leqslant 10} \right),{{\varvec{D}}_{hj}}\left( {10<{\upsilon _{for}} \leqslant 15} \right),{{\varvec{D}}_{hj}}\left( {15<{\upsilon _{for}} \leqslant 20} \right), \hfill \\ \left. {\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;{{\varvec{D}}_{hj}}\left( {20<{\upsilon _{for}} \leqslant 25} \right),{{\varvec{D}}_{hj}}\left( {25<{\upsilon _{for}} \leqslant 30} \right),{{\varvec{D}}_{hj}}\left( {{\upsilon _{for}} \geqslant 30} \right)} \right\} \hfill \\ \end{gathered}$$

(26)

$${{\varvec{D}}_{hj\left( {j=3,6} \right)}}=\left\{ {{{\varvec{D}}_{hj}}\left( {{\upsilon _{for}}=0} \right),{{\varvec{D}}_{hj}}\left( {{\upsilon _{for}} \ne 0} \right)} \right\}$$

(27)

where ${\upsilon _{for}}$ denotes the forward velocity of the vehicle obtained by the SINS/Dual-2D-LDV integration, in meters per second (m/s).

According to Eqs. (26) and (27) and the current ${\upsilon _{for}}$, (25) can be rewritten as

$${\varvec{D}}=\left[ {\begin{array}{*{20}{c}} {{{\varvec{D}}_{h1}}\left( {{\upsilon _{for}}} \right)}&{{{\varvec{e}}_k}\left( 1 \right)} \\ \vdots & \vdots \\ {{{\varvec{D}}_{h6}}\left( {{\upsilon _{for}}} \right)}&{{{\varvec{e}}_k}\left( 6 \right)} \end{array}} \right]$$

(28)

To identify the specific location of outliers, each component of the filter innovation vector is individually analyzed by calculating its respective LOF value. For clarity, this paper demonstrates the LOF calculation process using the first component of the filter innovation vector as an example. Based on Eq. (28), the spatial distance matrix (SDM) for the dataset D is expressed as follows:

$$\text{S}\text{D}{\text{M}_1}=\left[ {\begin{array}{*{20}{c}} {dis\left( {{{\varvec{D}}_1}\left( 1 \right),{{\varvec{D}}_1}\left( 1 \right)} \right)}& \cdots &{dis\left( {{{\varvec{D}}_1}\left( 1 \right),{{\varvec{D}}_1}\left( N \right)} \right)} \\ \vdots & \vdots & \vdots \\ {dis\left( {{{\varvec{D}}_1}\left( N \right),{{\varvec{D}}_1}\left( 1 \right)} \right)}& \cdots &{dis\left( {{{\varvec{D}}_1}\left( N \right),{{\varvec{D}}_1}\left( N \right)} \right)} \end{array}} \right]$$

(29)

In Eq. (29), each row of the SDM represents the Euclidean distances between a specific object and all other objects in the dataset. By sorting each row of the SDM in ascending order, the neighborhood set and K-distance of the object are determined based on the pre-set number of neighbors. Using Eqs. (21) to (23), the LOF value of the current innovation vector component, denoted as $LO{F_{i\left( {i=1, \cdots ,6} \right)}}\left( {{{\varvec{e}}_k}\left( i \right)} \right)$, can be calculated. The calculated LOF value is then compared with the preset threshold $T_{i}^{1}$ to assess the anomaly degree of each component of the filter measurement. If the LOF value is below the threshold, it indicates that the local density of the current measurement aligns with the historical data, signifying normal, and traditional Kalman filtering is applied. However, if the LOF value exceeds the threshold, it suggests the presence of measurement errors, prompting additional measures to mitigate the impact of these errors on the integrated navigation system.

There are two primary methods for setting the LOF threshold: an empirical approach, which is simple and effective for most outliers, and a more rigorous method using Kernel Density Estimation (KDE). In the KDE approach, a collection of LOF values from a dataset is used to build a smoothed probability density function (PDF). A false alarm rate, ${P_{LOF}}$, is then set, and the corresponding threshold, ${T_{LOF}}$, is calculated from the PDF. To avoid the added computational complexity of running KDE in real time, and since we use a dataset of historical normal data to calculate our LOF values, our final threshold is a fixed value derived from a combination of these two methods. We used multiple sets of past normal data to build several test thresholds via KDE and then averaged them to get a single, robust, and fixed value. This approach effectively combines the simplicity of the empirical method with the statistical rigor of KDE.

Sequential adaptive Kalman filter

Sequential Kalman filter

To facilitate the detection of outliers in each component of the measurements and mitigate their impact on the SINS/Dual-2D-LDV tightly coupled integration, sequential Kalman filter is used instead of standard Kalman filter. References^31,32 demonstrates the equivalence between sequential and standard Kalman filter. The process of sequential Kalman filter is as follows:

Step 1: Time update

$${\hat {{\varvec{x}}}_{k/k - 1}}={{\varvec{F}}_{k/k - 1}}{\hat {{\varvec{x}}}_{k - 1}}$$

(30)

$${{\mathbf{P}}_{k/k - 1}}={{\varvec{F}}_{k/k - 1}}{{\mathbf{P}}_{k - 1}}{\varvec{F}}_{{k/k - 1}}^{{\text{T}}}+{{\varvec{G}}_{k - 1}}{{\mathbf{Q}}_{k - 1}}{\varvec{G}}_{{k - 1}}^{{\text{T}}}$$

(31)

where ${\hat {{\varvec{x}}}_{k/k - 1}}$ and ${{\mathbf{P}}_{k/k - 1}}$ represent the one-step predicted state vector and the one-step predicted state error covariance matrix, respectively, and ${{\mathbf{Q}}_{k - 1}}$ denotes the system noise covariance matrix.

Step 2: Initialize the measurement update loop

$$\hat {{\varvec{x}}}_{k}^{{(0)}}={{\varvec{x}}_{k/k - 1}}$$

(32)

$${\mathbf{P}}_{k}^{{(0)}}={{\mathbf{P}}_{k/k - 1}}$$

(33)

Step 3: Measurement update loop for each measurement.

First rewrite (18) as:

$$\left[ {\begin{array}{*{20}{c}} {{\varvec{z}}_{k}^{{(1)}}} \\ {{\varvec{z}}_{k}^{{(2)}}} \\ {{\varvec{z}}_{k}^{{(3)}}} \\ {{\varvec{z}}_{k}^{{(4)}}} \\ {{\varvec{z}}_{k}^{{(5)}}} \\ {{\varvec{z}}_{k}^{{(6)}}} \end{array}} \right]=\left[ {\begin{array}{*{20}{c}} {{\mathbf{H}}_{k}^{{(1)}}} \\ {{\mathbf{H}}_{k}^{{(2)}}} \\ {{\mathbf{H}}_{k}^{{(3)}}} \\ {{\mathbf{H}}_{k}^{{(4)}}} \\ {{\mathbf{H}}_{k}^{{(5)}}} \\ {{\mathbf{H}}_{k}^{{(6)}}} \end{array}} \right]{{\varvec{x}}_k}+\left[ {\begin{array}{*{20}{c}} {{\varvec{v}}_{k}^{{(1)}}} \\ {{\varvec{v}}_{k}^{{(2)}}} \\ {{\varvec{v}}_{k}^{{(3)}}} \\ {{\varvec{v}}_{k}^{{(4)}}} \\ {{\varvec{v}}_{k}^{{(5)}}} \\ {{\varvec{v}}_{k}^{{(6)}}} \end{array}} \right]$$

(34)

For $l=1, \cdots ,6$(where l is the number of the measurements), perform the following:

$${\mathbf{K}}_{k}^{{(l)}}={\mathbf{P}}_{k}^{{(l - 1)}}{\left( {{\mathbf{H}}_{k}^{{(l)}}} \right)^T}{\left[ {{\mathbf{H}}_{k}^{{(l)}}{\mathbf{P}}_{k}^{{(l - 1)}}{{\left( {{\mathbf{H}}_{k}^{{(l)}}} \right)}^T}+{\mathbf{R}}_{k}^{{(l)}}} \right]^{ - 1}}$$

(35)

$$\hat {{\varvec{x}}}_{k}^{{(l)}}=\hat {{\varvec{x}}}_{k}^{{(l - 1)}}+{\mathbf{K}}_{k}^{{(l)}}({\varvec{z}}_{k}^{{(l)}} - {\mathbf{H}}_{k}^{{(l)}}\hat {{\varvec{x}}}_{k}^{{(l - 1)}})$$

(36)

$${\mathbf{P}}_{k}^{{(l)}}=({\mathbf{I}} - {\mathbf{K}}_{k}^{{(l)}}{\mathbf{H}}_{k}^{{(l)}}){\mathbf{P}}_{k}^{{(l - 1)}}$$

(37)

where ${\mathbf{K}}_{k}^{{(l)}}$ and ${\mathbf{R}}_{k}^{{(l)}}$ denote the filter gain matrix and the measurement noise covariance matrix corresponding to the l-th measurement, respectively.

Step 4: Complete the measurement update

$${\hat {{\varvec{x}}}_k}=\hat {{\varvec{x}}}_{k}^{{(l)}}$$

(38)

$${{\mathbf{P}}_k}={\mathbf{P}}_{k}^{{(l)}}$$

(39)

Adaptive filter

The vehicle’s motion state and ground conditions are variable, resulting in different degrees of violation of the NHC lateral zero-velocity constraint and varying levels of anomalies in the 2D-LDV measurements. Therefore, in addition to the threshold $T_{i}^{1}$, a second threshold $T_{i}^{2}$ is introduced to develop an adaptive filter based on the LOF value. The formula for calculating the adaptive factor in the adaptive filter is as follows:

$${\beta _{ki}}=\left\{ {\begin{array}{*{20}{c}} 1&{LO{F_i}\left( {{{\varvec{e}}_k}\left( i \right)} \right) \leqslant T_{i}^{1}} \\ {1+\frac{{LO{F_i}\left( {{{\varvec{e}}_k}\left( i \right)} \right) - T_{i}^{1}}}{{T_{i}^{2} - T_{i}^{1}}}{W_i}}&{T_{i}^{1}<LO{F_i}\left( {{{\varvec{e}}_k}\left( i \right)} \right) \leqslant T_{i}^{2}} \\ {+\infty }&{LO{F_i}\left( {{{\varvec{e}}_k}\left( i \right)} \right)>T_{i}^{2}} \end{array}} \right.$$

(40)

where ${W_i}$ is the scale factor, representing the inflation multiple of the measurement noise covariance matrix when the LOF value reaches $T_{i}^{2}$. For the SINS/Dual-2D-LDV tightly coupled integration used in this paper, empirical evidence suggests that this factor typically ranges from 1 to 4.

When the LOF value is less than $T_{i}^{1}$, the measurement is deemed normal, and the adaptive filter is not required; in this case, the adaptive factor${\beta _{ki}}$ is set to 1. If the LOF value falls between $T_{i}^{1}$ and $T_{i}^{2}$, the measurement is considered to have a low degree of anomaly. In this scenario, the measurement’s weight in the filter is reduced by inflating the measurement noise covariance matrix, which helps mitigate the impact of measurement errors on the system. When the LOF value exceeds $T_{i}^{2}$, the measurement is regarded as highly anomalous, indicating significant measurement errors, and the measurement is isolated by setting the adaptive factor to infinity.

According to Eq. (40), Eq. (35) can be rewritten as:

$${\mathbf{K}}_{k}^{{(l)}}={\mathbf{P}}_{k}^{{(l - 1)}}{\left( {{\mathbf{H}}_{k}^{{(l)}}} \right)^T}{\left[ {{\mathbf{H}}_{k}^{{(l)}}{\mathbf{P}}_{k}^{{(l - 1)}}{{\left( {{\mathbf{H}}_{k}^{{(l)}}} \right)}^T}+\beta _{{kl}}^{2}{\mathbf{R}}_{k}^{{(l)}}} \right]^{ - 1}}$$

(41)

By using Eq. (41), the filter gain matrix for anomalous measurements is reduced to mitigate the impact of outliers on the system.

Experiment results and analysis

Before conducting experimental validation, a SINS/Dual-2D-LDV tightly coupled integration onboard experiment was conducted to construct the dataset required for the LOF outlier detection method. Subsequently, two sets of onboard experiments were conducted using the same system to verify effectiveness of the proposed method. As depicted in Fig. 3, the test system consists of two independently developed 2D-LDVs, a self-developed high-precision IMU, and a high-precision dual-antenna differential GNSS system produced by BDSTAR. The main parameters of these components are detailed in Table 1.

Fig. 3

Test vehicle and experimental system.

Table 1 Specifications of the experimental apparatus.

Fig. 4

(a) Vehicle trajectories for constructing LOF dataset. (b) Vehicle forward velocity obtained from SINS/GNSS integration. (c) Vehicle lateral velocity obtained from SINS/GNSS integration.

The vehicle trajectory and the distribution of forward and lateral velocities during the vehicle’s motion, required for constructing the dataset for the LOF-based outlier detection method, are shown in Fig. 4. To ensure that the constructed dataset contains only normal data, the SINS/GNSS integration, based on a high-precision IMU and differential GNSS, was used to evaluate the 2D-LDV outliers and vehicle sideslip to remove abnormal data during the dataset construction process. Additionally, to further improve dataset accuracy, the data was classified based on the vehicle’s forward and lateral velocities. After constructing the required dataset, the following parameters were configured to ensure the efficiency and accuracy of the proposed LOF-based outlier detection method: the data window size N for LOF detection was set to 100, where 99 values were randomly selected from the classified dataset based on the current vehicle velocity to evaluate whether the current data was normal. The neighborhood value K for LOF was set to 75, the four primary detection thresholds for the 2D-LDV measurements were set to 7.5, and both primary detection thresholds for the vehicle’s lateral velocity were set to 3. A secondary detection threshold of 20 was applied to all measurements.

Fig. 5

(a) Vehicle trajectory in the first experiment. (b) 2D-LDV velocity output curve in the first experiment.

Fig. 6

Horizontal location error of the first experiment. The top is the absolute position estimation error and the bottom is the relative position error.

Fig. 7

Height positioning error of the first experiment.

To evaluate the performance of the proposed SINS/Dual-2D-LDV tightly coupled integration scheme, the following five schemes were designed for comparative analysis:

Scheme 1: SINS/2D-LDV tightly coupled integration based on the first 2D-LDV.

Scheme 2: SINS/2D-LDV tightly coupled integration based on the second 2D-LDV.

Scheme 3: SINS/Dual-2D-LDV tightly coupled integration.

Scheme 4: SINS/Dual-2D-LDV tightly coupled integration based on the robust Kalman filter from reference³³.

Scheme 5: SINS/Dual-2D-LDV tightly coupled integration scheme proposed in this paper.

The first vehicle validation experiment lasted approximately 2.15 h, covering a distance of about 170.65 km. The vehicle’s trajectory and the outputs from the two 2D-LDVs are illustrated in Fig. 5, where ${\upsilon _{LDV}}$ denotes the vehicle’s forward velocity, derived from the raw outputs of the two 2D-LDVs. Figures 6 and 7 display the horizontal and vertical positioning accuracies for the different schemes, with the maximum position error (Max) and root mean square error (RMSE) for each scheme provided in Table 2. Figures 8 and 9 present the estimation results for the misalignment angles between the 2D-LDVs and the IMU, as well as the beam inclination angle deviations of the 2D-LDVs. From Figs. 6 and 7, and Table 2, it is evident that all five schemes achieve satisfactory positioning accuracy without prior system calibration. The maximum horizontal position error is less than 60 m, which is better than 0.35‰ of the total mileage, and the corresponding RMSE is less than 0.2‰ of the total mileage. This highlights the excellent performance of the SINS/2D-LDV integration, the ultra-high measurement precision of the 2D-LDVs, and the advantages of non-contact measurement, making 2D-LDVs highly suitable for vehicular navigation. Differences in positioning accuracy between Schemes 1 and 2 are attributed to the distinct performance characteristics of the two 2D-LDVs, their varied installation relationships with the vehicle body and IMU, and discrepancies in the Kalman filter’s estimation of the misalignment angles and beam deviations of the two 2D-LDVs. The horizontal positioning accuracy of Schemes 3, 4, and 5 exceeds that of Schemes 1 and 2, indicating that utilizing two 2D-LDVs in the SINS/2D-LDV tightly coupled integration improves accuracy over using a single 2D-LDV. This improvement results from the additional information provided by the two 2D-LDVs, which helps mitigate the impact of outliers in the 2D-LDV data on the system. The positioning accuracy of Scheme 4 is slightly better than that of Scheme 3, demonstrating that the traditional robust Kalman filter, based on residual chi-square detection, can further alleviate the influence of outliers. Scheme 5 achieves the highest positioning accuracy, highlighting the effectiveness and superiority of the proposed method. Figures 8 and 9 show that all five schemes are capable of estimating the misalignment angles between the 2D-LDVs and the IMU, as well as the beam inclination angle deviations. The roll misalignment angle ${\phi _{my}}$ converges more slowly due to weaker observability, as the vehicle’s lateral and vertical maneuvers are much smaller than its forward maneuvers. Additionally, Fig. 8 reveals that the pitch misalignment angle${\phi _{my - LDV1}}$ in Scheme 1 significantly deviates from the other four schemes, explaining the considerably larger height error in Scheme 1 compared to the others.

Table 2 Comparison of the performance of the five schemes in the first test (170.56 Km).

For RMSE and Max, the left column represents absolute errors, while the right column represents relative errors.

Fig. 8

Estimation results of the beam inclination angle deviation of the first 2D-LDV and the misalignment angle between it and the IMU.

Fig. 9

Estimation results of the beam inclination angle deviation of the second 2D-LDV and the misalignment angle between it and the IMU.

Fig. 10

Fault detection results. (a)–(f): The outlier detection result of the LOF-based fault detection method, i.e., $LO{F_1}\left( {{{\varvec{e}}_k}\left( 1 \right)} \right)$-$LO{F_6}\left( {{{\varvec{e}}_k}\left( 6 \right)} \right)$. (g)–(l) The outlier detection results of the residual chi-square detection method, respectively based on ${{\varvec{e}}_k}\left( 1 \right)$-${{\varvec{e}}_k}\left( 6 \right)$.

To demonstrate the advantages of the proposed method in fault detection, Fig. 10 compares the outlier detection results of the LOF-based fault detection method with the traditional residual chi-square test based on Mahalanobis distance for the first set of experiments. Both methods assess the six components of the filter innovation vector. For the residual chi-square test, the preferred threshold is ${\chi ^2}\left( {0.1} \right)$, where 0.1 denotes the significance level, commonly used in most integrated navigation systems, is selected³⁴. At this threshold, only a few 2D-LDV anomalies are detected, and violations of the vehicle’s lateral zero-velocity constraint are not identified at all. Even when using a lower threshold,${\chi ^2}\left( {0.25} \right)$, the residual chi-square test detects only a limited number of faults. This is primarily because the 2D-LDV developed by our team has excellent performance, with rare occurrences of measurement value loss during use. The most frequent faults encountered are velocity retention during poor signal conditions and velocity zeroing when the Doppler signal is overwhelmed by noise at very low velocities. However, these faults are slow-varying and minor, while the residual chi-square test is more effective at identifying sudden, large faults and is less sensitive to gradual, minor faults. This limitation explains why Scheme 4 is less effective compared to the proposed Scheme 5. In contrast, the LOF-based fault detection method demonstrates high sensitivity to anomalies in the SINS/2D-LDV tightly coupled integration, whether they arise from 2D-LDV measurement errors or violations of the vehicle’s lateral zero-velocity constraint, underscoring the superiority of the proposed approach.

To further validate the reliability and superiority of the proposed scheme, a second vehicle experiment was conducted. Unlike the first experiment, where the SINS/Dual-2D-LDV tightly coupled integration system was not pre-calibrated, this system underwent calibration prior to the second experiment. The vehicle’s trajectory and the outputs of the two 2D-LDVs during this experiment are shown in Fig. 11. The experiment lasted 1.66 h, covering a distance of 174.77 km. The position errors of the various schemes are presented in Figs. 12 and 13, and Table 3. The results indicate that the performance of each scheme in the second experiment mirrors that of the first. Specifically, the SINS/Dual-2D-LDV tightly coupled integration outperforms the SINS/2D-LDV system, and the proposed method significantly improves the performance of the SINS/Dual-2D-LDV integration.

Fig. 11

(a) Vehicle trajectory in the second experiment. (b) 2D-LDV velocity output curve in the second experiment.

Fig. 12

Horizontal location error of the second experiment. The top is the absolute position estimation error and the bottom is the relative position error.

Fig. 13

Height positioning error of the second experiment.

Table 3 Comparison of the performance of the five schemes in the second test (174.77 Km).

For RMSE and Max, the left column represents absolute errors, while the right column represents relative errors.

Table 4 Average single-step execution times of comparison schemes.

Computing efficiency is just as critical as positioning error for evaluating different integration schemes, as it directly relates to their practicality. To this end, the single-step execution time for each integration scheme in the two experiments was calculated using a Python performance analysis tool. The hardware environment was an AMD Ryzen 7 5800 H processor with a clock speed of 3.20 GHz. The average single-step execution times for the different schemes are shown in Table 4. As the table illustrates, the average single-step execution times for Scheme 1 and Scheme 2 are comparable and lower than the other three comparative schemes. The execution time for Scheme 3 is longer than Schemes 1 and 2, but the increase is marginal, indicating that adding one 2D-LDV does not significantly increase the computational burden. The single-step execution time of Scheme 4 is longer than that of Scheme 3, suggesting that steps such as calculating the chi-squared statistic add a computational overhead. However, this increase is limited due to the low dimensionality of the measurements. The proposed Scheme 5 exhibits a substantial increase in execution time, exceeding all other schemes. This is attributed to the computational demands of frequently executing the LOF algorithm. Nevertheless, the single-step execution time for Scheme 5 is only 2.54 ms, which is well within the 10 ms limit required for a 100 Hz update frequency. This analysis demonstrates that the significant improvement in positioning performance achieved by Scheme 5 is a fully acceptable trade-off for its increased computational cost, as the algorithm’s efficiency remains well within the system’s operational limits.

Conclusion

To achieve reliable autonomous navigation for land vehicles, this paper proposes a fault-tolerant SINS/Dual-2D-LDV tightly coupled integration scheme. This scheme employs two 2D-LDVs tightly coupled with SINS to enhance the accuracy and reliability of the integrated navigation system. Additionally, LOF is used to evaluate anomalies in the 2D-LDV measurements and violations of the vehicle’s lateral zero-velocity constraint. To mitigate the impact of these anomalies and violations, an adaptive filter based on the LOF value is developed. The effectiveness of the proposed scheme was validated through two long-distance vehicular experiments, each exceeding 170 km. Results show that the proposed scheme delivers superior horizontal and vertical positioning accuracy compared to other schemes, achieving RMSE values better than 0.1‰ of the total mileage. Moreover, the proposed LOF-based outlier detection method outperforms the traditional residual chi-square test in identifying 2D-LDV measurement anomalies and violations of the lateral zero-velocity constraint. In future work, the use of 3D-LDVs could further reduce the impact of lateral zero-velocity violations, and performance could be enhanced with more advanced adaptive algorithms.