Fault Detection Algorithm for Gaussian Mixture Noises: An Application in Lidar/IMU Integrated Localization Systems

Penggao Yan
Penggao Yan
Department of Aeronautical and Aviation Engineering, Faculty of Engineering, Hong Kong Polytechnic University, Hong Kong
Zhengdao Li
Zhengdao Li
Department of Aeronautical and Aviation Engineering, Faculty of Engineering, Hong Kong Polytechnic University, Hong Kong
Feng Huang
Feng Huang
Department of Aeronautical and Aviation Engineering, Faculty of Engineering, Hong Kong Polytechnic University, Hong Kong
Weisong Wen,
Weisong Wen,
Department of Aeronautical and Aviation Engineering, Faculty of Engineering, Hong Kong Polytechnic University, Hong Kong
and Li-Ta Hsu
Li-Ta Hsu
Department of Aeronautical and Aviation Engineering, Faculty of Engineering, Hong Kong Polytechnic University, Hong Kong
- Find this author on Google Scholar
- Find this author on PubMed
- Search for this author on this site
- For correspondence: [email protected]

NAVIGATION: Journal of the Institute of Navigation
March 2025,
72
(1)
navi.684;
DOI: https://doi.org/10.33012/navi.684

Abstract

Fault detection is crucial to ensure the reliability of localization systems. However, conventional fault detection methods usually assume that noises in the system have a Gaussian distribution, limiting their effectiveness in real-world applications. This study proposes a fault detection algorithm for an extended Kalman filter (EKF)-based localization system by modeling non-Gaussian noises as a Gaussian mixture model (GMM). The relationship between GMM-distributed noises and the measurement residual is rigorously established through error propagation, which is utilized to construct the test statistic for a chi-squared test. The proposed method is applied to an EKF-based two-dimensional light detection and ranging/inertial measurement unit integrated localization system. Experimental results in a simulated urban environment show that the proposed method exhibits a 30% improvement in the detection rate and a 17%–23% reduction in the detection delay, compared with the conventional method with Gaussian noise modeling.

Keywords

1 INTRODUCTION

Gaussian noise modeling is not sufficient for developing fault detection methods

Fault detection is essential for localization and navigation systems in some safety-critical applications (Joerger & Pervan, 2016; Osechas et al., 2012; Pervan et al., 1998; Wang et al., 2016), for detecting the occurrence of faults in a system and determining the time at which faults occur (Gao et al., 2015). Fault detection methods can be primarily classified into model-based, knowledge-based, and signal-based methods (Gao et al., 2015). Among these methods, model-based methods, particularly statistical analysis of residuals, appear to be the most popular approach for detecting faults in localization and navigation systems (Angus, 2006; Gao et al., 2020; Puchalski & Giernacki, 2022; Yang et al., 2013), as system models are usually well established and known to designers in these applications (Gao et al., 2015). However, conventional model-based methods often assume that the noises in the system have a Gaussian distribution (Hsu et al., 2017; Joerger et al., 2014; Osechas et al., 2012; Pervan et al., 1998; Walter & Enge, 1995).

For example, Walter and Enge (1995) constructed a test statistic by using the sum of the squares of the range residual errors, which was utilized in a chi-squared test to detect potential faults in pseudorange measurements. The measurement noise was assumed to have a multivariate Gaussian distribution for normal conditions in which satellites have no malfunctions. Wang et al. (2016) designed a fault detection method for a navigation system based on the chi-squared test and the sequential probability ratio test. In their work, all system process noise and measurement noise were modeled as zero-mean Gaussian distributions. Unfortunately, noises in the real world usually have non-Gaussian properties. Examples can be found in global navigation satellite systems (GNSSs) (Rife & Pervan, 2012), micro-electromechanical system (MEMS) inertial sensors (Hou & El-Sheimy, 2003; Lethander & Taylor, 2023), and light detection and ranging (lidar) sensors (Xu et al., 2018). Thus, unrealistic Gaussian assumptions can result in increased false alarm rates (FARs) or degraded fault detection rates (FDRs) in real-world applications, limiting the reliability and effectiveness of methods aiming to prevent systems from faults.

The Gaussian mixture model (GMM) is promising but remains under-explored in fault detection research

The modeling of non-Gaussian noise has received increasing attention, and its application in the field of localization and navigation, including multi-sensor fusion, robust localization, and integrity monitoring, has been extensively explored (Davis & Blair, 2015; Langel et al., 2020; Rife, 2018; Wen et al., 2021). One of the most popular approaches is to model noises as a GMM, which represents a probability distribution as a weighted combination of multiple Gaussian distributions. Examples include works by Ali-Loytty and Sirola (2007), who proposed a Gaussian sum filter (GSF) method for hybrid positioning with non-Gaussian noises by approximating the prior density of the state as a Gaussian mixture, Pfeifer and Protzel (2019), who proposed a robust sensor fusion algorithm by adaptively tuning the GMM parameters of the noise distribution, and Blanch et al. (2008), Gao et al. (2022), and Yun et al. (2008), who developed over-bounding algorithms and derived protection levels for integrity monitoring by modeling measurement noises as a GMM. Nevertheless, few studies have considered non-Gaussian noises in fault detection. One example is the work by Yun et al. (2008), who developed a fault detection method using a GSF. In particular, the measurement noise was modeled as a GMM, and several parallel Kalman filters were developed to handle each Gaussian component. The fault detection process was realized by comparing the one-sided tail probability of the residual from the GSF with a predefined threshold. Wang et al. (2022) developed a similar algorithm, except that their approach involved summing the residual of each filter according to the mixture weight and subsequently taking the summation for a chi-squared test to identify potential faults. Numerical experiments have shown that these approaches exhibit improved detection performance compared with conventional Gaussian methods. However, the improvement can be attributed to at least two factors: 1) the difference between the GSF-based detection method and the conventional detection method based on the chi-squared test; 2) the difference between GMM-based noise modeling and Gaussian-based noise modeling. It is challenging to differentiate whether these improvements stem from the differences in the detection methods themselves or from the differences in noise modeling.

Contributions of this paper

Recently, Hashemi and Ruths (2019) proposed a fault detection method for a linear time-invariant control system with non-Gaussian noise. In their work, the residual of the observer in the linear time-invariant system is modeled as a GMM and used to construct the test statistics for a chi-squared test. This architecture is consistent with the conventional Gaussian method based on the chi-squared test, providing valuable insights for fairly comparing the effects of non-Gaussian noise modeling on fault detection problems. Inspired by their work, this study aims to extend the idea to fault detection problems in localization systems with non-Gaussian noises. Specifically, we establish the relationship between the GMM-distributed noises and the residual in an extended Kalman filter (EKF)-based localization system. Then, we transform the residual to a variable whose distribution approaches a standard multivariate normal (MVN) distribution. The Mahalanobis distance from the transformed variable to a standard MVN distribution is taken as the test statistic for a chi-squared test to detect faults in the measurements. The proposed method is then applied to a localization system constructed by integrating lidar measurements and inertial measurement unit (IMU) measurements via the EKF. For a fair comparison of different detection algorithms, a simulated urban environment is constructed based on the three-dimensional (3D) simulator CARLA (Dosovitskiy et al., 2017), making it possible to simulate GMM-distributed noises and ensuring reproducibility of the experiments. Finally, we compare the proposed method with the conventional Gaussian method in the simulated environment, evaluating their performance for two types of measurement failures. The contributions of our study are two-fold:

A fault detection algorithm designed for GMM noises. This study presents a fault detection algorithm for localization systems with Gaussian mixture noises, including a comprehensive analysis of the relationship between noises and residuals, a transformation process for the residual based on the law of total covariance, and a chi-squared test. We prove that the measurement residual is a linear combination of the measurement and process noises and also exhibits a GMM distribution. In addition, the proposed method shares the same methodology as the conventional Gaussian method. Thus, the substantial difference in fault detection performance between these two methods is attributed to the difference in noise modeling, making it possible to fairly evaluate the effects of different noise modeling approaches on fault detection tasks in localization systems.
A simulated platform for a fair comparison of fault detection algorithms. This study establishes a simulated urban environment based on the 3D simulator CARLA, which provides a fault-free environment for lidar-based localization systems, guaranteeing a fair comparison for different fault detection algorithms. In addition, this platform enables fault injection at specified time periods, providing a valuable tool for evaluating the detection performance for different types of failure in different scenarios.

The remainder of this paper is organized as follows. Section 2 presents the fault detection method for a localization system with GMM noises. We first establish the relationship between residuals of the EKF and the multivariate GMM-distributed noises (Section 2.1), providing a theoretical basis for developing a transformation method based on GMM assumptions (Section 2.2). A fault detection algorithm based on the chi-squared test is then developed in Section 2.3. In Section 3, we apply the proposed fault detection method to an EKF-based lidar/IMU integrated localization system, which primarily includes the construction of the sensor platform, an IMU motion model (Section 3.1), and a two-dimensional (2D) lidar measurement model (Section 3.2). In Section 4, a simulated environment is constructed for fairly comparing the performance of fault detection algorithms, and the fault detection performance of the proposed method is examined in this simulated environment for two types of measurement failures. Finally, Section 5 presents a summary of this work.

2 FAULT DETECTION WITH GMM NOISE MODELING

In this section, we present a fault detection algorithm for EKF-based localization systems with noises characterized by a GMM. We first establish the relationship between noises in the system and the measurement residual in the EKF through error propagation. We prove that the measurement residual is a linear combination of the measurement and process noises through error propagation and that its distribution is also a GMM. A transformation method is then constructed to transform the residual to a variable whose distribution approaches a standard MVN distribution. Finally, we calculate the Mahalanobis distance from the transformed variable to a standard MVN distribution, which is taken as the test statistic for a chi-squared test to detect faults in the measurements.

2.1 Residual Analysis in the EKF

2.1.1 Relationship Between the Residual and Noises

A statistical analysis of residuals is vital in model-based fault detection methods for localization and navigation systems (Angus, 2006; Joerger & Pervan, 2016; Puchalski & Giernacki, 2022; Rife, 2013). A general measurement model in a localization system can be written as follows:

$y_{k} = h (x_{k}) + η_{k}$ 1

where x_k is the system state at time k, y_k and η_k are the measurement and noise vectors at time k, respectively, and h(·) is the measurement function. A general state propagation model can be written as follows:

$x_{k} = f (x_{k - 1}, u_{k - 1}, v_{k - 1})$ 2

where f(·) is the state propagation function, u_k−1 is the external input at time k −1, and v_k−1 is the process noise at time k −1. In an EKF-based localization system, the propagation equations are given by the following:

${\hat{x}}_{k}^{-} = f ({\hat{x}}_{k - 1}^{+}, u_{k - 1}, 0)$ 3a

$P_{k}^{-} = F_{k - 1} P_{k - 1} F_{k - 1}^{T} + G_{k - 1} Q_{k - 1} G_{k - 1}^{T}$ 3b

where ${\hat{x}}_{k - 1}^{+}$ is the estimated state at time k −1, ${\hat{x}}_{k}^{-}$ is the predicted state at time k, P_k−1 is the covariance matrix of the estimated state given by the EKF at time k −1, $P_{k}^{-}$ is the predicted covariance matrix of the estimated state at time k, F_{k −1 is the state transition matrix, G_k−1} is the noise Jacobian matrix with respect to v_k−1, and Q_k−1 is the covariance matrix of v_k−1. If the EKF receives measurements y_k at time k, the Kalman gain K_k is obtained by the following equation (Daum, 2005):

$K_{k} = P_{k}^{-} H_{k}^{T} {(H_{k} P_{k}^{-} H_{k}^{T} + R_{k})}^{- 1}$ 4

where R_k is the covariance matrix of η_k. The estimated state at time k is obtained as follows:

${\hat{x}}_{k}^{+} = {\hat{x}}_{k}^{-} + K_{k} (y_{k} - h ({\hat{x}}_{k}^{-}))$ 5

where H_k is the Jacobian matrix of h(x_k). The covariance of the estimated state is given by the following:

$P_{k} = (I - K_{k} H_{k}) P_{k}^{-}$ 6

The residual corresponding to the measurements at time k is as follows:

$r_{k} = y_{k} - h ({\hat{x}}_{k}^{-})$ 7

By substituting Equation (1) into Equation (7) and taking the first-order Taylor expansion at ${\hat{x}}_{k}^{-}$ , we can obtain the following:

$r_{k} = H_{k} (x_{k} - {\hat{x}}_{k}^{-}) + η_{k}$ 8

where H_k is the Jacobian matrix of h(·) defined with respect to x_k. By substituting Equations (2) and (3) into Equation (8) and applying a first-order Taylor expansion at the point $({\hat{x}}_{k - 1}^{+}, u_{k - 1}, 0)$ , we obtain the following:

$r_{k} = H_{k} (F_{k - 1} (x_{k - 1} - {\hat{x}}_{k - 1}^{+}) + G_{k - 1} v_{k - 1}) + η_{k}$ 9

where F_k−1 is the Jacobian matrix of f(·) defined with respect to x_k−1 and G_k−1 is the Jacobian matrix of f(·) defined with respect to v_k−1. In Equation (9), ${\hat{x}}_{k - 1}^{+}$ can be obtained by either state propagation or a measurement update, as discussed below.

(1) ${\hat{x}}_{k - 1}^{+}$ is obtained by state propagation.

In this condition, the EKF does not receive external measurements (such as lidar measurements). The estimated state is given by the predicted state, i.e., ${\hat{x}}_{k - 1}^{+} = {\hat{x}}_{k - 1}^{-}$ . Therefore, we have the following:

$x_{k - 1} - {\hat{x}}_{k - 1}^{+} = x_{k - 1} - {\hat{x}}_{k - 1}^{-}$ 10

By substituting Equations (2) and (3) into Equation (10) and taking a first-order Taylor expansion at $({\hat{x}}_{k - 2}^{+}, u_{k - 2}, 0)$ , we obtain the following:

$x_{k - 1} - {\hat{x}}_{k - 1}^{+} = F_{k - 2} (x_{k - 2} - {\hat{x}}_{k - 2}^{+}) + G_{k - 2} v_{k - 2}$ 11

Repeating the operation in Equations (9)–(11) yields the following:

$r_{k} = H_{k} (\prod_{i = 1}^{m} F_{k - i} (x_{k - m} - {\hat{x}}_{k - m}^{+}) + \sum_{i = 2}^{m} \prod_{j = 1}^{i - 1} F_{k - j} G_{k - i} v_{k - i} + G_{k - 1} v_{k - 1}) + η_{k}$ 12

where m is the discrete time interval between the last measurement and the current measurement. To simplify the expression, we set the higher-order terms inside the parentheses (i.e., terms with more than two matrix multiplications) to zero and obtain the following:

$r_{k} = H_{k} F_{k - 1} G_{k - 2} v_{k - 2} + H_{k} G_{k - 1} v_{k - 1} + η_{k}$ 13

(2) ${\hat{x}}_{k - 1}^{+}$ is obtained by a measurement update.

In this case, we obtain the following expression by substituting Equation (5) into Equation (9):

$x_{k - 1} - {\hat{x}}_{k - 1}^{+} = x_{k - 1} - ({\hat{x}}_{k - 1}^{-} + K_{k - 1} (y_{k - 1} - h ({\hat{x}}_{k - 1}^{-})))$ 14

By substituting Equation (1) into Equation (14) and taking the first-order Taylor expansion at ${\hat{x}}_{k - 1}^{-}$ , we have the following:

$x_{k - 1} - {\hat{x}}_{k - 1}^{+} = (I - K_{k - 1} H_{k - 1}) (x_{k - 1} - {\hat{x}}_{k - 1}^{-}) - K_{k - 1} η_{k - 1}$ 15

Therefore, we have the following relationship:

$\begin{matrix} r_{k} = H_{k} F_{k - 1} (I - K_{k - 1} H_{k - 1}) (x_{k - 1} - {\hat{x}}_{k - 1}^{-}) \\ - H_{k} F_{k - 1} K_{k - 1} η_{k - 1} + H_{k} G_{k - 1} v_{k - 1} + η_{k} \end{matrix}$ 16

If we initialize the Kalman filter at t = k −1, we have $E [{\hat{x}}_{k - 1}^{-}] = x_{k - 1} . {\hat{x}}_{k - 1}^{-}$ is the unbiased estimator of x_k−1, and its distribution can be represented by a Gaussian distribution $N (x_{k - 1}, π_{0})$ , where π₀ is the covariance matrix of ${\hat{x}}_{k - 1}^{-}$ . We then apply the following definition:

$e_{k - 1} = x_{k - 1} - {\hat{x}}_{k - 1}^{-}$ 17

with $e_{k - 1} \sim N (0, π_{0})$ . Equation (16) can then be written as follows:

$\begin{matrix} r_{k} = H_{k} F_{k - 1} (I - K_{k - 1} H_{k - 1}) e_{k - 1} \\ - H_{k} F_{k - 1} K_{k - 1} η_{k - 1} + H_{k} G_{k - 1} v_{k - 1} + η_{k} \end{matrix}$ 18

As can be seen, r_k is a linear combination of η_k, η_k−1, $v_{k_{1}}$ , and e_k−1. Assuming that the covariance of the initial state, i.e., π₀, is considerably small, we have e_k−1 ≈ 0. Then, H_kF_k−1 (I – K_k−1H_k−1)e_k−1 has only a limited impact on Equation (18). If we know the ground truth of x_k−1 (which is possible in a simulation environment, as shown in Section 4), we can initialize the Kalman filter at t = k −1 with ${\hat{x}}_{k - 1}^{-} = x_{k - 1}$ . Then, r_k approaches a linear combination of η_k, η_k−1, and $v_{k_{1}}$ :

$r_{k} = η_{k} - H_{k} F_{k - 1} K_{k - 1} η_{k - 1} + H_{k} G_{k - 1} v_{k - 1}$ 19

This perfect initialization can be approximately regarded as the setting of $E [{\hat{x}}_{k - 1}^{-}] = x_{k - 1}$ with a very small π₀.

If we do not initialize the Kalman filter at t = k −1, we can iteratively apply Equations (7)–(19) until the first initialization of the EKF:

$E [{\hat{x}}_{1}^{-}] = x_{1}$ 20

We will find that r_k approaches a linear combination of v_k−1,…, v₁ and η_k, η_k−1,…η₁ and that their coefficients are related to H_k, H_k−1,… H₁, F_k−1,…, F₁, G_k−1,..G₁, and K_k−1,..K₁.

2.1.2 Distribution of the Residual

To focus on the development of the fault detection algorithm, we simply take the case that ${\hat{x}}_{k - 1}^{+}$ is obtained by state propagation for further analysis and set the higher-order terms (> 2) to zero. Then, Equation (13) is the final expression of the residual at time k. We use the following relationships:

$V_{k - 1} = H_{k} G_{k - 1}$ 21

and:

$N_{k - 1} = H_{k} F_{k - 1} G_{k - 2}$ 22

Then, Equation (13) can be written as follows:

$r_{k} = V_{k - 1} v_{k - 1} + N_{k - 1} v_{k - 2} + η_{k}$ 23

In this study, v_k−1, v_k−2, and η_k are assumed to be non-Gaussian noises, and we use the multivariate GMM to model them. Assuming that v_k−1 and v_k−2 are independent and identically distributed (i.i.d.), we can obtain the probability density functions (PDFs) of η and v as follows:

$f_{η} (x) = \sum_{j = 1}^{m_{1}} p_{j}^{η} N (x ∣ μ_{j}^{η}, π_{j}^{η})$ 24a

$f_{v} (x) = \sum_{j = 1}^{m_{2}} p_{j}^{v} N (x ∣ μ_{j}^{v}, π_{j}^{v})$ 24b

where $\sum_{j = 1}^{m_{1}} p_{j}^{η} = 1$ , $\sum_{j = 1}^{m_{2}} p_{j}^{v} = 1$ ; m₁ and m₂ are the number of Gaussian modes for η and v, respectively; $p_{j}^{η}$ and $p_{j}^{v}$ are the mixture weights (i.e., the prior probability of each Gaussian mode); $N (\cdot ∣ μ_{j}^{η}, π_{j}^{η})$ and $N (\cdot ∣ μ_{j}^{v}, π_{j}^{v})$ are the PDFs of each Gaussian mode; $μ_{j}^{η} \in ℝ^{n_{1}}$ and $μ_{j}^{v} \in ℝ^{n_{2}}$ are the means of each Gaussian mode; $π_{j}^{η} \in ℝ^{n_{1} \times n_{1}}$ and $π_{j}^{v} \in ℝ^{n_{2} \times n_{2}}$ are the covariance matrices of each Gaussian mode; n₁ is the dimension of the measurement noise; and n₂ is the dimension of the process noise. Inspired by the work of Hashemi and Ruths (2019), one can prove that r_k has a multivariate GMM distribution by applying the convolution theorem to the characteristic function of each component of the residual (refer to Appendix B for details):

$f_{r_{k}} (x) = \sum_{a = 1}^{m_{2}} \sum_{b = 1}^{m_{2}} \sum_{c = 1}^{m_{1}} p_{a b c} N (x ∣ μ_{a b c}, π_{a b c})$ 25

where:

$p_{a b c} = p_{a}^{v} p_{b}^{v} p_{c}^{η}$ 26a

$μ_{a b c} = V_{k - 1} μ_{a}^{v} + N_{k - 1} μ_{b}^{v} + μ_{c}^{η}$ 26b

$π_{a b c} = V_{k - 1} π_{a}^{v} V_{k - 1}^{T} + N_{k - 1} π_{b}^{v} N_{k - 1}^{T} + π_{c}^{η}$ 26c

2.2 Transformation Methods

2.2.1 Transformation Based on GMM Assumptions

Section 2.1 showed that the residual is a multivariate GMM-distributed random variable. To align with the architecture of the chi-squared detector, we transform the residual to a new random variable by subtracting its total mean and subsequently pre-multiplying by the principal square root matrix of the total covariance (Hashemi & Ruths, 2019):

$T_{g} = Σ^{- 1 / 2} (r_{k} - μ)$ 27

where μ is the total mean and ∑^−1/2 is the principal square root matrix of the total covariance (∑) of the residual, respectively. According to the law of total covariance (Weiss et al., 2006), μ and ∑ are given by the following:

$μ = \sum_{a = 1}^{m_{2}} \sum_{b = 1}^{m_{2}} \sum_{c = 1}^{m_{1}} p_{a b c} μ_{a b c}$ 28a

$Σ = \sum_{a = 1}^{m_{2}} \sum_{b = 1}^{m_{2}} \sum_{c = 1}^{m_{1}} p_{a b c} π_{a b c} + (μ - μ_{a b c}) {(μ - μ_{a b c})}^{T}$ 28b

In Appendix C, we demonstrate that the transformed variable has a distribution that approaches the standard MVN distribution when each Gaussian component in the GMM-distributed residual has a small difference in the covariance. Therefore, we limit the proposed transformation method to specific applications in which the measurement error can be modeled by a GMM and the covariance of each component is similar. By setting the number of Gaussian components in the GMM to one, the above transformation formulation can be easily extended to situations in which the residual is formulated as a Gaussian distribution (Da, 1994; Liu et al., 2017).

2.2.2 Transformation Based on Gaussian Assumptions

To evaluate the impact of GMM noise modeling on fault detection performance, we also establish a baseline transformation method that utilizes Gaussian assumptions for the noises. Specifically, the measurement noise $η_{k}^{0}$ and the process noise $v_{k}^{0}$ at time k are modeled as zero-mean Gaussian noises as follows:

$η_{k}^{0} \sim N (0, π_{η}^{0})$ 29a

$v_{k}^{0} \sim N (0, π_{v}^{0})$ 29b

where $π_{η}^{0} \in ℝ^{n_{1} \times n_{1}}$ and $π_{v}^{0} \in ℝ^{n_{2} \times n_{2}}$ are the covariance matrices and can be obtained by calculating the variance of samples generated from the noise distribution in Equation (24) through a Monte Carlo simulation. Following the derivation in Section 2.1, the measurement residual at time k is given by the following:

$r_{k}^{0} = V_{k - 1} v_{k - 1}^{0} + N_{k - 1} v_{k - 2}^{0} + η_{k}^{0}$ 30

Let $Σ_{0} = V_{k - 1} π_{v}^{0} V_{k - 1}^{T} + N_{k - 1} π_{v}^{0} N_{k - 1}^{T} + π_{η}^{0}$ ; then, the distribution of the measurement residual is given as follows:

$f_{r_{k}^{0}} (x) = N (x ∣ 0, Σ_{0}^{- 1 / 2})$ 31

which is a zero-mean Gaussian distribution. Similar to Equation (27), $r_{k}^{0}$ can be transformed to a standard MVN-distributed random variable as follows:

$T_{g}^{0} = Σ_{0}^{- 1 / 2} r_{k}^{0}$ 32

Figure 1 plots the transformation process of the residual based on the two different assumptions. Obviously, the substantial difference between the two transformation methods lies in the method for modeling noise. This important feature enables us to develop a fault detection method with GMM noise modeling that shares the same methodology as the conventional Gaussian method, providing a way to fairly evaluate the effects of different noise modeling approaches on fault detection tasks in a localization system.

FIGURE 1

Transformation methods based on GMM assumptions and Gaussian assumptions

2.3 Fault Detection Based on the Chi-Squared Test

The two transformations in Section 2.2, although different, yield a standardized measurement residual that is assumed to have a standard MVN distribution. The n₁ × 1 vector T_g can be written as follows:

$T_{g} = [T_{g, 1}, T_{g, 2}, \dots, T_{g, n_{1}}]$ 33

where $T_{g, 1}, T_{g, 2}, \dots, T_{g, n_{1}}$ are assumed to be mutually independent, standard normal random variables. We can construct the following test statistic t_g:

$t_{g} = T_{g}^{T} T_{g} = T_{g, 1}^{2} + T_{g, 2}^{2} + \dots + T_{g, n_{1}}^{2}$ 34

Because $T_{g, 1}, T_{g, 2}, \dots, T_{g, n_{1}}$ are mutually independent, t_g follows the chi-squared distribution with n₁ degrees of freedom (DOFs). From another perspective, Equation (34) represents the square of the Mahalanobis distance of T_g from the standard MVN distribution, which is known to have the same chi-squared distribution. Similarly, we can construct the test statistic $t_{g}^{0}$ in the context of the Gaussian assumption as follows:

$t_{g}^{0} = T_{g}^{0^{T}} T_{g}^{0}$ 35

Before performing the hypothesis test with the constructed test statistic, we give a brief introduction to the chi-squared distribution. Its cumulative distribution function is given by the following:

$F (x; k) = \frac{γ (\frac{k}{2}, \frac{x}{2})}{Γ (\frac{k}{2})}$ 36

where γ(·) is the lower incomplete gamma function, Γ(·) is the gamma function, and k is the DOF. For a certain value of TD ∈ ℝ, the probability of the chi-squared statistic being less than TD is given by P(x < TD) = F(TD; k), and the associated p-value is given by 1 – F(TD; k). The value of TD and the corresponding p-value are interrelated, and their association can be determined from tables of the chi-squared distribution (Fisher & Yates, 1953). The chi-squared test for fault detection associated with the test statistic t_g at a given significance level α is as follows:

$H_{1} : t_{g} > T D_{α}$ 37a

$H_{0} : t_{g} \leq T D_{α}$ 37b

where TD_α is determined by the following:

$P (t_{g} > T D_{α} ∣ H_{0}) = α$ 38

A similar hypothesis test can be conducted with the test statistic $t_{g}^{0}$ , which is defined by the similar formulations in Equations (37)–(38) and is omitted for the sake of clarity. For convenience, we denote the fault detection method based on t_g as the total Gaussian–GMM method, and we denote the method based on $t_{g}^{0}$ as the Gaussian method.

In Equation (38), the significance level α can also be interpreted as the desired FAR in the context of fault detection when the assumption that T_g has a standard MVN distribution is valid. However, this assumption is unrealistic in real-world applications. For example, the GMM cannot perfectly model the non-Gaussian noises in lidar measurements. Furthermore, the elements in the residual vector could be correlated, which violates the assumption and eventually results in degradation of the fault detection performance. Therefore, some criteria must be met to examine the real performance of the fault detection results. In this study, two criteria, the FAR and FDR in a period, are formulated as follows:

$F A R = \frac{n_{F P}}{n_{F P} + n_{T N}}$ 39a

$F D R = \frac{n_{T P}}{n_{T P} + n_{F N}}$ 39b

where n_TP is the number of faulty events that are classified as faulty (true positive), n_TN is the number of fault-free events that are classified as normal (true negative), n_FP is the number of fault-free events that are classified as faulty (false positive), and n_FN is the number of faulty events that are classified as normal (false negative).

3 APPLICATION IN AN IMU/LIDAR INTEGRATED LOCALIZATION SYSTEM

This section presents the application of the proposed fault detection in an IMU/lidar integrated system. The architecture of the fault detection process in the localization system is illustrated in Figure 2. A lidar/IMU integrated localization system is constructed by utilizing EKFs (Daum, 2005), where the state propagation equation is constructed from the kinematic model of the IMU motion and the measurement function is constructed by matching the extracted line features from seven 2D lidar points to the plane in the prior map. Notably, 2D lidars possess fewer moving parts and components, making them more reliable and easier to maintain in comparison to 3D lidars. This characteristic not only helps to prevent hardware faults but also simplifies the development and evaluation of fault detection methods. In addition, 2D lidars are low-cost but can only provide limited ranging measurements. As a result, it is of great importance to reliably detect faulty measurements from 2D lidars. In this EKF-based localization system, the process and measurement noises are modeled as a multivariate GMM. In Section 2, the nominal residual (defined as the residual of a fault-free system) was proven to be a multivariate GMM. Therefore, we leverage the law of total covariance to transform the residual into a variable whose distribution approaches a standard MVN distribution. Finally, we calculate the Mahalanobis distance from the transformed variable to a standard MVN distribution, which is taken as the test statistic for a chi-squared test to detect faults in the measurements.

FIGURE 2

(a) Architecture of fault detection in the lidar/IMU integrated localization system; (b) sensor platform of the localization system; Ele: elevation angle

The sensor platform of the localization system is shown in Figure 2(b), where an IMU is placed at the chassis and seven 2D lidars are distributed around the vehicle. The center location and the elevation angle of each lidar in the IMU frame ({I}) are given in the embedded table. Instead of defining seven lidar frames for each 2D lidar, for clarity, we only define a single lidar frame ({L}) that is fixed at the center of the fourth lidar. Because the seven 2D lidars have similar measurement models, such a definition can more clearly illustrate the main idea without a loss of generality. Following this definition, we denote ^Lp_I as the translation from {I} to {L} and ^Lq_I as the rotation (the rotation matrix is $_{I}^{L} R$ ) from {I} to {L}. ^Ip_L and $_{L}^{I} R$ are extrinsic calibration parameters calibrated in the setup stage of the system. Note that the extrinsic calibration parameters for each 2D lidar are different and are affected by the center location and the elevation angle of the 2D lidar plane.

The state vector x of the system is defined as follows:

$x = {[^{G} p_{I}^{T},^{G} v_{I}^{T},^{I} q_{G}^{T}, b_{a}^{T}, b_{g}^{T}]}^{T}$ 40

where ^Gp_I and ^Gv_I are the position and velocity vectors of the IMU frame ({I}) in the world frame ({G}), respectively, ^Iq_G =[q_w, q_x, q_y, q_z]^T denotes the rotation from {G} to {I} in terms of quaternions, and $b_{a}^{T}$ and $b_{g}^{T}$ are biases of the accelerometer and gyroscope measurements, respectively. In addition, the rotation matrix associated with ^Iq_G is denoted by $_{G}^{I} R$ . The world frame is fixed at the center of the pre-built point cloud map in the east–north–up coordinate system. For clarity, all symbols used in this chapter are listed in Appendix A. The following sections briefly introduce the motion model and the measurement model of the IMU/lidar integrated system and, more importantly, determine the exact form of F, G, and H, which are used to construct the test statistic for the fault detection process.

3.1 Motion Model

The kinematic model of the IMU motion in {G} is adopted to propagate the vehicle state (Lefferts et al., 1982). The discrete-time state propagation equation is given by the following:

$\begin{matrix} x_{k} = f (x_{k - 1}, u_{k - 1}, v_{k - 1}) \\ = [\begin{matrix} ^{G} p_{I, k - 1} +^{G} v_{I, k - 1} Δ t + \frac{1}{2} (_{I}^{G} {\hat{R}}_{k - 1} (a_{m, k - 1} - b_{a, k - 1} - n_{a}) +^{G} g) Δ t^{2} \\ ^{G} v_{I, k - 1} + (_{I}^{G} {\hat{R}}_{k - 1} (a_{m, k - 1} - b_{a, k - 1} - n_{a}) +^{G} g) Δ t \\ \exp {(\frac{Δ t}{2} Ω [w_{m, k - 1} - b_{g, k - 1} - n_{g}])}^{I} q_{G, k - 1} \\ b_{a, k - 1} + n_{w a} Δ t \\ b_{g, k - 1} + n_{w g} Δ t \end{matrix}] \end{matrix}$ 41

where Δt is a short time period; ^Gp_I,k−1, ^Gv_k−1, ^Iq_G,k−1, b_a,k−1, and b_g,k−1 are system states at the discrete time k −1; a_m,k−1 and w_m,k−1 are the acceleration and gyroscope measurements, respectively; n_a and n_g are the noises of the acceleration and gyroscope measurements, respectively; n_wa and n_wg are zero-mean Gaussian white noise; $v_{k - 1} = {[n_{a}^{T}, n_{g}^{T}, n_{w a}^{T}, n_{w g}^{T}]}^{T}$ is the process noise vector at time k −1; $_{I}^{G} {\hat{R}}_{k - 1}$ represents the estimation of the vehicle orientation at time k −1; and Ω[·] is a 4×4 skew symmetric matrix:

$Ω [w] = [\begin{matrix} - {[w]}_{\times} & w \\ - w^{T} & 0 \end{matrix}]$ 42

where [·]_× denotes the standard vector cross-product.

By linearizing Equation (41) with respect to x_k−1 and v_k−1, we can obtain the state transition matrix F_k−1 and the noise Jacobian matrix G_k−1 as follows:

$F_{k - 1} = [\begin{matrix} I_{3} & I_{3} Δ t & 0 & - {\frac{1}{2}}_{I}^{G} {\hat{R}}_{k - 1} Δ t^{2} & 0 \\ 0 & I_{3} & 0 & -_{I}^{G} {\hat{R}}_{k - 1} Δ t & 0 \\ 0 & 0 & \frac{\partial g (^{I} {\hat{q}}_{G, k - 1}, w_{m, k - 1})}{\partial^{I} q_{G, k - 1}} & 0 & \frac{\partial g (^{I} {\hat{q}}_{G, k - 1}, w_{m, k - 1})}{\partial w_{G I, k - 1}} \\ 0 & 0 & 0 & I_{3} & 0 \\ 0 & 0 & 0 & 0 & I_{3} \end{matrix}]$ 43a

$G_{k - 1} = [\begin{matrix} - {\frac{1}{2}}_{I}^{G} {\hat{R}}_{k - 1} Δ t^{2} & 0 & 0 & 0 \\ -_{I}^{G} {\hat{R}}_{k - 1} Δ t & 0 & 0 & 0 \\ 0 & \frac{\partial g (^{I} {\hat{q}}_{G, k - 1}, w_{m, k - 1})}{\partial w_{G I, k - 1}} & 0 & 0 \\ 0 & 0 & I_{3} Δ t & 0 \\ 0 & 0 & 0 & I_{3} Δ t \end{matrix}]$ 43b

where $\frac{\partial g (^{I} {\hat{q}}_{G, k - 1}, w_{m, k - 1})}{\partial^{I} q_{G, k - 1}}$ and $\frac{\partial g (^{I} {\hat{q}}_{G, k - 1}, w_{m, k - 1})}{\partial w_{G I, k - 1}}$ are derived in Appendix E.2.

3.2 Measurement Model

The laser scan plane of a 2D lidar intersects with several planes. The intersection is called a line segment, which is fitted by a set of 2D lidar points, as shown in Figure 3(a). We adopt the method of Pfister et al. (2003) to extract line segments from raw 2D lidar measurements and find their associated planes. Details are provided in Appendix D.2. The 2D lidar measurement model is constructed by finding the shortest vector in the laser scan plane from the origin of {G} to the plane (Hesch et al., 2010; Zhao & Farrell, 2013), as illustrated in Figure 3(b). Assuming that the laser scan plane is intersected by plane Π_i at line $^{L} ξ_{i}^{⊥}$ in {L}, we can find the shortest vector $^{L} x_{i} = ρ_{i}^{L} ξ_{i}$ in the lidar scan plane from the origin of {L} to Π_i, where ^Lξ_i is the unit vector, ρ_i is the length of ^Lx_i, and the point M is the intersection of ^Lx_i and $^{L} ξ_{i}^{⊥}$ . The laser beam ^Lx_i in the scan laser plane can be represented by ranging and bearing parameters (ρ_i, ϕ_i) in the polar coordinate system, as shown in Figure 3(c). Alternatively, ^Lx_i can also be written as ^Lx_i = [ρ_i cos ϕ_i ρ_i sin ϕ_i 0]^T in {L}. The shortest vector from the origin of {G} to plane Π_i is denoted by d_i^Gπ_i and is intersected by Π_i at point N, where ^Gπ_i is the normal of the plane in {G} and d_i is the length of d_i^Gπ_i. The vector from M to N in {G} is denoted by ^Gt_i. The measurement model can be written as follows:

FIGURE 3

(a) A demonstration of extracting line segments from 2D lidar points; (b) a schematic for constructing the 2D lidar measurement model by finding the shortest vector in the laser scan plane from the origin of {L} to plane Π_i; (c) laser plane with the polar coordinate system

$ϕ_{i} = h_{1}^{i} (x) + {\tilde{ϕ}}_{i} = \arctan (sgn (^{L} d_{i}) \frac{a_{2}^{i}}{a_{1}^{i}}) + {\tilde{ϕ}}_{i}$ 44a

$ρ_{i} = h_{2}^{i} (x) + {\tilde{ρ}}_{i} = \frac{|^{L} d_{i} |}{\sqrt{a_{1}^{i 2} + a_{2}^{i 2}}} + {\tilde{ρ}}_{i}$ 44b

$^{L} d_{i} = d_{i} -^{G} π_{i}^{T} (^{G} p_{I} +_{I}^{G} R^{I} p_{L})$ 44c

$^{L} a_{i} =_{G}^{L} R^{G} π_{i} = {[a_{1}^{i}, a_{2}^{i}, a_{3}^{i}]}^{T}$ 44d

where the i-th measurement y_i = [ϕ_i, ρ_i]^T gives the ranging and bearing parameters associated with the shortest vector in the laser plane intersected by Π_i, ${\tilde{ϕ}}_{i}$ and ${\tilde{ρ}}_{i}$ are measurement noises, and sgn(·) is the sign function. Details are provided in Appendix D.1.

Let $h^{i} (x_{k}) = {[h_{1}^{i} (x_{k}) h_{2}^{i} (x_{k})]}^{T}$ be the measurement function of the i-th measurement at time k, $η_{i, k} = {[{\tilde{ϕ}}_{i, k} {\tilde{ρ}}_{i, k}]}^{T}$ be the associated measurement noise, ${\hat{x}}_{k}^{-} = {[^{G} {\hat{p}}_{I}^{_{-} T},^{G} v_{I}^{_{-} T},^{I} {\hat{q}}_{G}^{_{-} T}, {\hat{b}}_{a}^{T}, {\hat{b}}_{g}^{T}]}^{T}$ be the predicted state, and $H_{k}^{i}$ be the Jacobian matrix of hⁱ(x_k) defined with respect to x_k and evaluated at ${\hat{x}}_{k}^{-}$ :

$H_{k}^{i} = \frac{\partial h^{i} ({\hat{x}}_{k}^{-})}{\partial x_{k}}$ 45

As proven in Appendix E.3, $H_{k}^{i}$ is given by the following:

$H_{k}^{i} = [\begin{matrix} 0_{1 \times 3} & 0_{1 \times 3} & \frac{1}{{\hat{μ}}_{i, k}^{-}} {\hat{λ}}_{i, k}^{- T}_{I}^{L} R J_{q}^{*} (^{I} {\hat{q}}_{G, k}^{-},^{G} π_{i, k}) & 0_{1 \times 6} \\ - sgn (^{L} {\hat{d}}_{i, k}^{-}) \frac{^{G} π_{i, k}^{T}}{\sqrt{{\hat{μ}}_{i, k}^{-}}} & 0_{1 \times 3} & - sgn (^{L} {\hat{d}}_{i, k}^{-}) \frac{^{G} π_{i, k}^{T}}{\sqrt{{\hat{μ}}_{i, k}^{-}}} J_{q} (^{I} {\hat{q}}_{G, k}^{-},^{I} p_{L}) + |^{L} {\hat{d}}_{i, k}^{-} | {\hat{k}}_{i, k}^{T}_{I}^{L} R J_{q}^{*} (^{I} {\hat{q}}_{G, k}^{-},^{G} π_{i, k}) & 0_{1 \times 6} \end{matrix}]$ 46

where:

$\begin{matrix} {\hat{μ}}_{i, k}^{-} = a_{1}^{i - 2} + a_{2}^{i - 2}, λ_{i, k}^{- T} = sgn (^{L} {\hat{d}}_{i, k}^{-}) [- {\hat{a}}_{2}^{i -}, {\hat{a}}_{1}^{i -}, 0] \\ {\hat{κ}}_{i, k}^{T} = - {\hat{μ}}_{i, k}^{- \frac{3}{2}} [{\hat{a}}_{1}^{i -}, {\hat{a}}_{2}^{i -}, 0],^{I} {\hat{a}}_{i, k}^{-} =_{I}^{L} R_{G}^{I} {\hat{R}}_{k}^{- G} π_{i, k} = {[{\hat{a}}_{1}^{i -}, {\hat{a}}_{2}^{i -}, {\hat{a}}_{3}^{i -}]}^{T} \end{matrix}$ 47

For multiple measurements $y_{k} = {[y_{1, k}^{T}, y_{2, k}^{T}, \dots, y_{n, k}^{T}]}^{T}$ at time k, the measurement function is written as follows:

$y_{k} = h (x_{k}) + η_{k}$ 48

where:

$h (x_{k}) = {[h^{1} {(x_{k})}^{T}, h^{2} {(x_{k})}^{T}, \dots, h^{n} {(x_{k})}^{T}]}^{T}$ 49a

$η_{k} = {[η_{1, k}^{T}, η_{2, k}^{T}, \dots, η_{n, k}^{T}]}^{T}$ 49b

and n is the total number of measurements at time k. Then, the Jacobian matrix of h(x_k) can be written as follows:

$H_{k}^{T} = {[H_{k}^{1^{T}}, H_{k}^{2^{T}}, \dots H_{k}^{n^{T}}]}^{T}$ 50

It is worth noting that the linearization process presented here differs from that of Hesch et al. (2010) and Zhao and Farrell (2013). In this work, we linearize the measurement function at the nominal state in terms of the position vector and quaternion, as shown in Equations (E27) and (E41) in Appendix E.3, which is more direct and intuitive than the approach in which linearization is performed in the error state (Hesch et al., 2010; Zhao & Farrell, 2013). In addition, linearization at the nominal state is beneficial for establishing clear relationships between the measurement noise and the state and between the measurement noise and the residual, both of which are shown in Section 2.1.

4 NUMERICAL EXPERIMENTS

4.1 Construction of the Simulation Platform

In this study, we build a simulated urban environment based on the 3D simulator CARLA (Dosovitskiy et al., 2017). Through automatic and manual checking, we can create a fault-free scenario in the simulated environment and inject specified faults at a specified time. The main reasons for constructing the simulation platform are listed below:

The measurement noise of lidars is uncontrollable in the real world. To simulate GMM-distributed noises and ensure reproducibility of the experiments, it is crucial to have a fully controllable environment to examine the performance of the proposed method.
Faults in real data sets are unpredictable. To the best of our knowledge, no method can always detect all faults. To control the fault types and the time at which faults occur, it is essential to use a simulation tool to intentionally generate faults of interest at a given time.

Snapshots of the constructed simulation platform are shown in Figure 4. The simulated vehicle is equipped with seven 2D lidars and a simulated IMU sensor, as shown in Figure 4(b). The measurements of 2D lidars are simulated by ray-cast technology, which can accurately reflect the real position of the point hit by each laser beam. Users can include additional noise in the 2D lidar measurements. The simulated sensors can directly output readings of physical parameters (such as angular velocity and acceleration) of the vehicle through CARLA. Additional noises are incorporated into the readings with customized configurations, eventually simulating the IMU sensor. In the experiment, the noise from 2D lidar measurements and IMU measurements is configured to be multivariate GMM-distributed. The detailed configuration will be described in Section 4.2. Recall from Section 3.2 that the 2D lidar measurement model requires pre-stored plane information. In the simulated platform, it is much easier to extract this information without introducing additional errors. We accomplish this by exporting the 3D objects in the simulated environment to Blender (Blender Optimization Community, 2018), an open-source 3D modeling and rendering software, to extract the faces of all objects of interest, such as buildings, walls, and roads. As shown in Figure 4(c), the face (plane) information, including the normal, center, and vertices, can be accurately extracted by automatic operation and manual checking in Blender.

FIGURE 4

Simulated platform

(a) The simulated urban environment is constructed based on CARLA, where the designed track of the ground vehicle is marked as the red line. (b) The simulated vehicle moves along the road by manual control or programmed control. (c) All faces of 3D objects in the simulated environment can be extracted and accurately represented by Blender with semi-automatic checking.

4.2 Experimental Setup

Based on the constructed simulation platform, a ground vehicle is programmed to move along the track in Figure 4(a) at a constant speed of 30 km/h. The operation time is 57 s, and the length of the trajectory is 314.3 m. Figure 4(a) presents the simulated trajectory. In the experiment, the output frequency of the lidars is 10 Hz, and lidar measurement noises are configured to have a multivariate GMM distribution. To set the covariance of the 2D lidar measurement noises, we refer to the research conducted by the team from Nagoya University (Carballo et al., 2020), which evaluated the performance of mainstream lidar products. They found that the root-mean-square error of the ranging measurements varies roughly from 0.01 m to 0.08 m within a measured distance of 100 m. Because the bearing measurements can be directly obtained by referring to the rotation frequency of the lidar, we assume that the noise of the bearing measurements is relatively small. In this study, we use the GMM to set the distribution of the measurement noise $η_{i} = {[{\tilde{ϕ}}_{i}, {\tilde{ρ}}_{i}]}^{T}$ . The PDF of the noise η_i in each 2D lidar measurement is given by the following:

$f_{η_{i}} (\cdot) = p_{1} N ([\begin{matrix} 0 \\ μ_{1} \end{matrix}], [\begin{matrix} {0.0003}^{2} & 0 \\ 0 & δ_{1}^{2} \end{matrix}]) + p_{2} N ([\begin{matrix} 0 \\ μ_{2} \end{matrix}], [\begin{matrix} {0.0003}^{2} & 0 \\ 0 & δ_{2}^{2} \end{matrix}])$ 51

where the bearing noise is set as zero-mean Gaussian with a standard deviation of 0.0003 rad and the range noise is determined by p = [p₁, p₂], μ = [μ₁, μ₂], and δ =[δ₁, δ₂]. In this experiment, we evaluate four settings for p, μ, and δ, as shown in Table 1. Taking the noise setting N1 as a baseline, we explore scenarios in which the difference between mixture weights is reduced (N2), the separation between components is decreased (N3), and the disparity between the variance of the components is reduced (N4). The PDFs of the range noises in the four settings are displayed in Figure 5. In addition, a degraded case for which the range noise is generated from a Gaussian distribution is also evaluated, as presented in Table 1.

FIGURE 5

Distribution of range noises for 2D lidar measurements in four GMM settings

View this table:

TABLE 1

Settings for Range Noises of Lidar Measurements

In this study, we assume that each lidar measurement is mutually independent. Therefore, for n lidar measurements, the PDF of the noise can be expressed as follows:

$f_{η} (\cdot) = p_{1} N ({[\begin{matrix} 0 \\ ⋮ \\ 0 \end{matrix}]}_{2 n \times 1}, [\begin{matrix} M_{1} & \dots & 0 \\ ⋮ & ⋱ & ⋮ \\ 0 & \dots & M_{1} \end{matrix}]) + p_{2} N ({[\begin{matrix} 0 \\ ⋮ \\ 0 \end{matrix}]}_{2 n \times 1}, [\begin{matrix} M_{2} & \dots & 0 \\ ⋮ & ⋱ & ⋮ \\ 0 & \dots & M_{2} \end{matrix}])$ 52

where M₁ and M₂ are the covariance matrix of each Gaussian component in the single-measurement case, as shown in Equation (51). In contrast, the PDF of the process noise $v = {[n_{a}^{T}, n_{g}^{T}, n_{w a}^{T}, n_{w g}^{T}]}^{T} \in ℝ^{12}$ is set as a zero-mean multivariate Gaussian distribution with the parameters shown in Table 2, which are simulated based on a MEMS-based IMU (Analog Devices, 2018). The output frequency of the simulated IMU is 100 Hz.

View this table:

TABLE 2

IMU Parameters

In the EKF setting, we use a perfect initial pose to initialize the EKF. In other words, the EKF is assumed to be converged at the initialization and will continue being converged in the following time steps if fault-free measurements are received. Although this is a strong assumption, it is applicable in the simulated environment. By making such an assumption, we can focus on the analysis of the residual instead of the convergence of the localization solution. In this study, the standard deviation of the estimated position is set to be 0.05 m, and the estimated orientation represented by quaternions is set as 0.02 at time k. To set R and Q, we calculate the total covariance of η and v and set their non-diagonal elements to zero.

In this study, we conducted simulations for two types of single-measurement failure, step failure and slope failure, to examine the fault detection performance across varied scenarios. The coefficients relating to these failures are listed in Table 3. In particular, the slope failure denotes a fault characterized by an escalating magnitude over time (Wang et al., 2016).

View this table:

TABLE 3

Lidar Range Measurement Failure Coefficients

4.3 Localization Performance

In the fault-free simulated environment, we examine the localization performance of the proposed lidar/IMU integrated localization system. Figure 6 presents the absolute translation error of the positioning results under five different settings of measurement noise. It is obvious that the localization performance remains relatively consistent across all noise settings. Throughout most epochs, the absolute translation error is below 0.1 m. In addition, the mean absolute translation error is approximately 0.06 m throughout the entire period. However, the absolute translation error shows an increase during the periods of 23–30 s and 45–48 s, coinciding with the vehicle’s significant turns, which are depicted in the thumbnail of Figure 6. This occurrence can be attributed to the lack of features at the turning locations, resulting in an insufficient number of measurements and, thus, poor observability (Joseph & Murthy, 2019; Sanandaji et al., 2014). Additional sensors, such as cameras and radars, can be employed to capture environmental information to improve the localization performance at these locations. However, such enhancements fall beyond the scope of this work.

FIGURE 6

Absolute translation error of the positioning results in the fault-free simulated environment

4.4 Fault Detection Performance

In the experiment, we evaluate the fault detection performance of the proposed method with five settings of measurement noise: four settings are generated from a multivariate GMM distribution, and one setting represents a degraded case in which the measurement noise is generated from a Gaussian distribution.

4.4.1 Step Failure Detection Analysis

Table 4 shows the fault detection results for a step failure when the desired FAR α is set as 0.05. In group A2, the FDR of the proposed total Gaussian-GMM method for each noise setting exceeds 85%, which is significantly larger than that of the Gaussian method. In particular, for the N2 noise setting, the FDR of the total Gaussian-GMM method surpasses that of the Gaussian method by 30%, showing the most substantial improvement among all noise configurations. As shown in Section 4.2, N2 represents the scenario in which the difference between mixture weights is reduced compared with the baseline setting N1, suggesting that the influence of the Gaussian component with a large variance on the noise distribution is amplified. Compared with the Gaussian method, the total Gaussian-GMM method can effectively capture such changes in the characteristics of the noise distribution, thereby exhibiting better detection performance. Nevertheless, the preeminence of the total Gaussian-GMM method in terms of the FDR is accompanied by the concession of an ascending FAR. In all noise settings, the FAR of the total Gaussian-GMM method increases slightly but is still comparable to the desired FAR (α = 0.05). Because reducing the missed detection rate is typically prioritized over false positives for multi-sensor navigation systems, it may be acceptable to sacrifice the FAR to a certain extent.

View this table:

TABLE 4

Statistical Results of FAR and FDR for a Step Failure (α = 0.05)

In group A1, both methods yield a substantially lower FDR compared with that in group A2. This result occurs because the amplitude of the injected faults in group A1 is notably smaller than that in group A2, thereby hindering the ability of both methods to detect a fault. Despite this challenge, the total Gaussian-GMM method still exhibits a distinct advantage in detecting faults, showing a much higher FDR than the Gaussian method. This result implies that the total Gaussian-GMM method has greater sensitivity to small faults. For the degraded case in which the measurement noise is generated from a Gaussian distribution, the total Gaussian-GMM method exhibits the same performance as the Gaussian method, as anticipated. The detection results for α = 0.01 and α = 0.001 are listed in Appendix F; both sets of results concur with the findings outlined in Table 4.

Figure 7 displays statistic curves and diagnosis results for the two methods with the N1 noise setting for step failure (group A2). The number of valid line measurements from the lidars is also plotted in the figures, and the chi-squared test is only applied when the number of line measurements is no less than 12. As can be seen, the chi-squared statistics of the total Gaussian-GMM method substantially exceed those of the Gaussian method over the failure period of 4–20 s. In addition, the diagnosis results show that the total Gaussian-GMM method switches less frequently between fault and normal states within the failure period, suggesting that the total Gaussian-GMM method is more stable in fault detection tasks with step failures.

FIGURE 7

Fault detection results for (a) the total Gaussian-GMM method and (b) the Gaussian method with the N1 noise setting for a step failure (group A2)

In the diagnosis result plot, “1” indicates a fault, and “0” indicates normal. The failure period is 4–20 s, which is marked by the shaded area.

4.4.2 Slope Failure Detection Analysis

The detection results for a slope failure are listed in Table 5, where the delayed time refers to the interval between the commencement of the fault injection and the first stable detected epoch (i.e., the subsequent diagnosis consistently declares a fault throughout the remaining injection period). In group B1, the total Gaussian-GMM method demonstrates a greater sensitivity to slope failure than the Gaussian method. For the N1, N2, and N4 noise settings, the detection delay for the total Gaussian-GMM method is reduced by 17%–23% compared with that of the Gaussian method. Note that the delayed times for the two methods are the same for the N3 noise setting, in which there is a small separation between components. Recalling the transformation method based on the law of total covariance in Section 2.2.1, it is obvious that the second term of the total covariance tends to be zero if all components have a similar mean. In such cases, the total covariance of the GMM distribution approaches the variance of the fitted Gaussian distribution; consequently, the two fault detection methods will exhibit similar performance. This phenomenon is also verified in the step failure experiments under the same noise setting (N3), where the total Gaussian-GMM method exhibits only a 3% and 4% increase in FDR compared with the Gaussian method for groups A1 and A2, respectively. Similar observations are made for group B2, with the total Gaussian-GMM method generally exhibiting a shorter delayed time than the Gaussian method. For the Gaussian noise setting, the total Gaussian-GMM method degrades to the Gaussian method, yielding the same delayed time.

View this table:

TABLE 5

Delayed Time of Fault Detection for a Slope Failure (α = 0.05)

The statistic curve and diagnosis result for the N1 noise setting with slope failure (group B1) are depicted in Figure 8. It can be observed that the chi-squared statistics of both methods consistently rise during the failure period at 34–44 s because the magnitude of the fault increases with time. One distinct difference between the two methods can be observed in the diagnosis curve: the diagnosis state of the total Gaussian-GMM method switches more frequently during the unstable period than that of the Gaussian method, indicating that the total Gaussian-GMM method is more sensitive to small faults. However, such sensitivity will lead to an increase in false detection epochs during the normal period.

FIGURE 8

Fault detection results for (a) the total Gaussian-GMM method and (b) the Gaussian method with the N1 noise setting for a slope failure (group B1)

In the diagnosis result plot, “1” indicates a fault, and “0” indicates normal. The failure period is 34–44 s, which is marked by the shaded area.

5 CONCLUSION

This study proposes a fault detection method for an EKF-based localization system with GMM noises. The measurement and process noises are modeled as a multivariate GMM distribution, and the residual of the EKF is derived as a linear combination of these noises, which is shown to be multivariate GMM-distributed. Based on the law of total covariance, the residual is transformed into a variable whose distribution approximates a standard MVN distribution. The Mahalanobis distance from the transformed variable to a standard MVN distribution is calculated and taken as the test statistic for a chi-squared test to detect potential faults. The proposed fault detection method is then applied to a lidar/IMU integrated localization system based on the EKF, where the measurement function is constructed by fitting line measurements from seven 2D lidar points to the plane and state propagation is achieved by a kinematic model of IMU motion.

In a simulated urban environment constructed via the 3D simulator CARLA, we examined the detection performance of the proposed method for two types of measurement failures, assessing each under four types of GMM noise settings. In the step failure experiment, the FDR of the proposed total Gaussian-GMM method surpassed that of the conventional Gaussian method by up to 30%, demonstrating the effectiveness of the proposed method in detecting small faults. In addition, the detection delay for the total Gaussian-GMM method was reduced by 17%–23% compared with the Gaussian method for three of the four noise settings in the slope failure experiment, demonstrating the greater sensitivity of the proposed method to a gradually changing failure. Because the total Gaussian-GMM method shares the same fault detection methodology as the Gaussian method, the difference in detection performance between the two methods is attributed to the difference in noise modeling. Therefore, the experimental results suggest that GMM-based noise modeling is beneficial for fault detection tasks in localization systems with non-Gaussian noises, motivating further research in this direction.

This study has several limitations, which also point out future research directions. In deriving the relationship between the residual and noises, we ignored higher-order terms to reduce the computation load. However, this approximation underestimates the uncertainty of the residual, thereby increasing the probability of false detection events in the fault detection task. For integrity applications, some integrity and continuity budgets should be allocated to risks associated with ignoring higher-order terms. A possible solution is to determine the bounding relationship between higher-order terms and to then scale the lower-order terms to compensate for the effects of higher-order terms on the uncertainty of the residual. In addition, we assume that the transformation method based on the law of total covariance can transform the GMM-distributed residual to a variable whose distribution approaches a standard MVN distribution. However, this assumption holds only when each Gaussian component in the GMM-distributed residual has a small difference in the covariance. This constraint is satisfied in the lidar/IMU integrated system. However, when the measurement noise exhibits significantly heavy tails, the transformation method cannot guarantee similarity, thereby hindering the performance of the proposed fault detection algorithm. Further efforts should be made to develop an advanced transformation method to ensure similarity between the transformed distribution and the standard MVN distribution, widening the application of non-Gaussian fault detection algorithms in other localization systems, such as GNSS/IMU integrated systems. Moreover, this study focuses on the detection of single-measurement failure. Future work can extend the proposed fault detection algorithm to multiple-fault detection problems by adopting exhaustive search or greedy search algorithms as reported by Blanch et al. (2015). Last but not least, this study gives little concern to the effects of the geometrical configuration of 2D lidars on state estimation and fault detection performance. For practical purposes, future research could investigate the optimal geometrical configuration based on the simulated platform constructed in this study.

HOW TO CITE THIS ARTICLE

Yan, P., Li, Z., Huang, F., Wen, W., and Hsu, L. -T. (2025). Fault detection algorithm for Gaussian mixture noises: An application in lidar/IMU integrated localization systems. NAVIGATION, 72(1). https://doi.org/10.33012/navi.684

APPENDIX A NOTATION

View this table:

TABLE A1

Description of Symbols and Operators

APPENDIX B DISTRIBUTION OF THE RESIDUAL

Equation (23) shows that r_k is a linear combination of independent random variables V_k−1 v_k−1, N_k−1 v_k−2, and η_k. Therefore, the PDF of r_k is the convolution of the PDF of these random variables, which can be written as follows:

$f_{r_{k}} = f_{v_{k - 1} v} * f_{N_{k - 1} v} * f_{η}$ B1

where * is the convolution operator. Because v_k−1 and v_k-2 are assumed to be i.i.d., we drop the time index in the above equation. The characteristic functions of r_k, η, and v are given as follows:

$φ_{r_{k}} (ω) = \int_{- \infty}^{\infty} f_{r_{k}} (x) e^{i ω^{T} x} d x$ B2a

$\begin{matrix} φ_{η} (ω) & = \int_{- \infty}^{\infty} f_{η} (x) e^{i ω^{T} x} d x = \sum_{j = 1}^{m_{1}} p_{j}^{η} \int_{- \infty}^{\infty} N (x ∣ μ_{j}^{η}, π_{j}^{η}) e^{i ω^{T} x} d x \\ = \sum_{j = 1}^{m_{1}} p_{j}^{η} φ_{N} (ω) = \sum_{j = 1}^{m_{1}} p_{j}^{η} e^{i ω^{T} μ_{j}^{η} - \frac{1}{2} ω^{T} π_{j}^{η} ω} \end{matrix}$ B2b

$φ_{v} (ω) = \int_{- \infty}^{\infty} f_{v} (x) e^{i ω^{T} x} d x = \sum_{j = 1}^{m_{2}} p_{j}^{v} e^{i ω^{T} μ_{j}^{v} - \frac{1}{2} ω^{T} π_{j}^{v} ω}$ B2c

where $φ_{N} (ω)$ is the characteristic function of a multi-normal variable. Let y = Ax + b be the linear transformation of the random vector x, where A is a constant matrix and b is a constant vector. The characteristic function of y is given by the following:

$φ_{y} (ω) = e^{i ω^{T} b} φ_{x} (A^{T} ω)$ B3

Therefore, we have the following relationship:

$φ_{N_{k - 1} v} (ω) = φ_{v} (N_{k - 1}^{T} ω) = \sum_{j = 1}^{m_{2}} p_{j}^{v} e^{i ω^{T} N_{k - 1} μ_{j}^{v} - \frac{1}{2} ω^{T} N_{k - 1} π_{j}^{v} N_{k - 1}^{T} ω}$ B4a

$φ_{V_{k - 1} v} (ω) = φ_{v} (V_{k - 1}^{T} ω) = \sum_{j = 1}^{m_{2}} p_{j}^{v} e^{i ω^{T} V_{k - 1} μ_{j}^{v} - \frac{1}{2} ω^{T} V_{k - 1} π_{j}^{v} V_{k - 1}^{T} ω}$ B4b

According to the convolution theorem, we have the following:

$φ_{r_{k}} (ω) = φ_{v} (V_{k - 1}^{T} ω) \cdot φ_{v} (N_{k - 1}^{T} ω) \cdot φ_{η} (ω)$ B5

where · indicates point-wise multiplication. Substituting Equations (B2b) and (B4) into Equation (B5), we can obtain the following:

$φ_{r_{k}} (ω) = \sum_{a = 1}^{m_{2}} \sum_{b = 1}^{m_{2}} \sum_{c = 1}^{m_{1}} p_{a b c} e^{i ω^{T} μ_{a b c} - \frac{1}{2} ω^{T} π_{a b c} ω}$ B6

where:

$p_{a b c} = p_{a}^{v} p_{b}^{v} p_{c}^{η}$ B7a

$μ_{a b c} = V_{k - 1} μ_{a}^{v} + N_{k - 1} μ_{b}^{v} + μ_{c}^{η}$ B7b

$π_{a b c} = V_{k - 1} π_{a}^{v} V_{k - 1}^{T} + N_{k - 1} π_{b}^{v} N_{k - 1}^{T} + π_{c}^{η}$ B7c

Performing the inverse operation shown in Equation (B2b) yields the following:

$φ_{r_{k}} (ω) = \sum_{a = 1}^{m_{2}} \sum_{b = 1}^{m_{2}} \sum_{c = 1}^{m_{1}} p_{a b c} \int_{- \infty}^{\infty} N (x ∣ μ_{a b c}, π_{a b c}) e^{i ω^{T} x} d x$ B8

Therefore, we obtain the following:

$f_{r_{k}} (x) = \sum_{a = 1}^{m_{2}} \sum_{b = 1}^{m_{2}} \sum_{c = 1}^{m_{1}} p_{a b c} N (x ∣ μ_{a b c}, π_{a b c}) d x$ B9

which indicates that r_k has a multivariate GMM distribution.

APPENDIX C SIMILARITY TO A STANDARD MVN DISTRIBUTION

To evaluate the similarity between the transformed distribution and the standard MVN distribution, we calculate the discretized Jensen—Shannon divergence between the two distributions (MacKay, 2003). Let P(x) and Q(x) be the probability mass functions of distributions P and Q, respectively. The discretized Jensen—Shannon divergence (D_JS) between P and Q defined on an identical sample space is given as follows:

$\begin{matrix} D_{J S} (P ‖ Q) & = \frac{1}{2} D_{K L} (P ‖ M) + \frac{1}{2} D_{K L} (Q ‖ M) \\ D_{K L} (P ‖ Q) & = \sum_{x \in S} P (x) \log (\frac{P (x)}{Q (x)}) \end{matrix}$ C1

where $M = \frac{1}{2} (P + Q)$ and D_KL is the Kullback–Leibler divergence (relative entropy). The minimum value (0) of D_JS indicates that the two distributions are the same, whereas the maximum value (1) indicates that the two distributions are completely different (MacKay, 2003; Manning & Schutze, 1999). In our work, P refers to the transformed distribution from a bivariate GMM (BGMM) and Q refers to the standard MVN distribution. To comprehensively evaluate the similarity between the transformed distribution and the standard MVN distribution, we first consider the 2D case and then extend our findings to higher-dimensional cases.

(1) Transformation of 2D BGMM

The PDF of a 2D BGMM can be formalized as follows:

$f (x) = p_{1} f_{N} (x; μ_{1}, Σ_{1}) + (1 - p_{1}) f_{N} (x; μ_{2}, Σ_{2})$ C2

where f_N(x; μ₁, ∑₁) and f_N(x; μ₂, ∑₂) are the PDFs of the first and second Gaussian components, μ₁ and μ₂ are the corresponding mean values, ∑₁ and ∑₂ are the corresponding standard deviations, and p₁ and 1 − p₁ are the mixing weights of the two Gaussian components. Equation (C2) is determined by thirteen parameters, i.e., p₁, two elements in μ₁, two elements in μ₂, four elements in ∑₁, and four elements in ∑₂, which span a thirteen-dimensional parameter space. It is difficult to determine the parameter combinations in this thirteen-dimensional parameter space such that the transformed distribution resembles the standard MVN distribution. Therefore, we use a reparameterization method to reduce the dimension of the parameter space (Li & Schwartzman, 2018). Specifically, we use four parameters, k₁, k₂, ρ, and p₁, to define a 2D BGMM, as shown in Table C1. Figure C1 shows 50% density contours of the reparameterized BGMM with ρ = 0.5 and p₁ = 0.5. As can be seen, p₁ determines the weight of the two Gaussian components, k₁ determines the distances between the centers of the two ellipses, k₂ controls the scale of the ellipse of the second Gaussian component, and ρ determines the ellipse eccentricity of the second Gaussian component.

View this table:

TABLE C1

Reparameterization of the 2D BGMM

50% density contours of the reparameterized BGMM with ρ = 0.5 and p1 = 0.5 The purple ellipse represents the density contour of component 1, whereas the red ellipse represents the density contour of component 2.

FIGURE C1

50% density contours of the reparameterized BGMM with ρ = 0.5 and p₁ = 0.5 The purple ellipse represents the density contour of component 1, whereas the red ellipse represents the density contour of component 2.

Based on the reparameterized BGMM, we conduct a Monte Carlo simulation to evaluate the similarity between the transformed distribution from a BGMM and the standard MVN distribution. Specifically, for each setting of (p₁, k₁, k₂, ρ), we randomly generate N samples {x₁, x₂, …, x_N} from the 2D BGMM, where N is set to 10⁵ in our experiments. Then, we apply the proposed transformation method to each sample:

$T_{i, g} = Σ^{- 1 / 2} (x_{i} - μ)$ C3

where μ is the total mean and ∑^−1/2 is the principal square root matrix of the total covariance (∑) of the BGMM. According to the law of total covariance (Weiss et al., 2006), μ and ∑ are given by the following:

$μ = p_{1} μ_{1} + (1 - p_{1}) μ_{2}$ C4a

$Σ = p_{1} Σ_{1} + (1 - p_{1}) Σ_{2} + \sum_{j = 1}^{2} (μ - μ_{j}) {(μ - μ_{j})}^{T}$ C4b

The transformed sample distribution is denoted by {T_1,g, T_2,g, …, T_N,g}. Then, we randomly generate N samples {y₁, y₂, …, y_N} from a standard 2D MVN distribution. Let P = {T_1,g, T_2,g, …, T_N,g} and Q = {y₁, y₂, …, y_N}. We can employ Equation (C1) to calculate the overall similarity between P and Q. Because D_JS is defined for a one-dimensional distribution, we will show the similarity between P and Q in each dimension in the following paragraphs.

Figure C2 shows the D_JS between the first dimension of the transformed distribution and the standard 2D MVN distribution. The results are presented in the form of heatmaps with ρ ∈ {0, 0.5, 1} and k₁ ∈ {0.01,0.1,1,10}, where p₁ and k₂ vary continuously. Interestingly, the lowest D_JS is centered in the region of k₂ ∈ [1,3] (except for the case of ρ = 1, k₁ = 10), where the two Gaussian components have a similar covariance. In this region, the total covariance in Equation (C4b) is similar to the covariance of each component, thereby accurately characterizing the uncertainty of the BGMM. Similar results are shown in Figure C3, which presents the D_JS between the second dimension of the transformed distribution and the standard 2D MVN distribution.

(2) Transformation of Higher-Dimensional BGMM

We further examine the similarity between the transformed distribution from a K-dimensional (K > 2) BGMM and the standard MVN distribution. Because it is impossible to conduct experiments with all values of K (which are infinite), we choose K to be 27, which is the average number of 2D lidar measurements in the simulated experiments in Section 4. We reparameterize the 27-dimensional (27D) BGMM using three parameters, p₁, k₁, and k₂, as shown in Table C2. Similar to the 2D BGMM experiments, we conduct a Monte Carlo simulation to obtain the transformed sample distribution for different settings of p₁, k₁, and k₂. To follow the analysis procedure utilized in the 2D BGMM experiments, we must calculate the D_JS between the transformed distribution and the standard MVN distribution for each dimension, yielding 27 heatmaps. As can be anticipated, this process would complicate the analysis. Therefore, we use an alternative approach to evaluate the similarity between the transformed distribution and the standard MVN distribution.

FIGURE C2

D_JS between the first dimension of the transformed distribution and the standard MVN distribution

View this table:

TABLE C2

Reparameterization of the 27D BGMM

Let the transform sample distribution be P = {T_1,g, T_2,g, …, T_N,g}. For each sample T_i,g in P, we calculate its inner product with itself, i.e., $T_{i, g}^{T}$ T_i,g. Then, we obtain the distribution $P_{1} = {T_{1, g}^{T} T_{1, g}, T_{2, g}^{T} T_{2, g}, \dots, T_{N, g}^{T} T_{N, g}}$ . If the distribution P approaches the standard MVN distribution, the distribution P₁ should approach a chi-squared distribution with 27 DOFs. Therefore, we can evaluate the similarity between the distribution P₁ and the chi-squared distribution with 27 DOFs. Figure C4 shows the D_JS between the distribution P₁ and the chi-squared distribution with 27 DOFs in the form of heatmaps with k₁ ∈ {0.01,0.1,1,10}, where p₁ and k₂ vary continuously. Similar to the trends in the 2D BGMM experiments, the lowest D_JS is centered in the region of k₂ ∈ [1,3]. Therefore, the P₁ distribution is similar to the chi-squared distribution with 27 DOFs in this region, suggesting that the transformed distribution from a 27D BGMM is similar to the 27D MVN distribution.

FIGURE C3

D_JS between the second dimension of the transformed distribution and the standard MVN distribution

DJS between the distribution P1 and the chi-squared distribution with 27 DOFs

FIGURE C4

D_JS between the distribution P₁ and the chi-squared distribution with 27 DOFs

Through the analysis in the 2D and 27D BGMM experiments, we find that the similarity assumption holds when each Gaussian component in the GMM-distributed residual has a small difference in the covariance. This constraint is satisfied in the lidar/IMU integrated system because the lidar measurement error is slightly heavy-tailed (Toschi et al., 2015), indicating that the difference in the covariance between the two Gaussian components is insignificant.

APPENDIX D LIDAR PROCESSING

Appendix D.1 Least-Norm Solution for Lidar Measurement Models

Following the notation in Figure 3(b) and corresponding definitions in Section 3.2, we assume that the extrinsic calibration parameters $(^{I} p_{L},_{L}^{I} R)$ and the vehicle pose $(^{G} p_{I},_{I}^{G} R)$ are known, that the plane Π_i associated with the current lidar scan plan is found based on the vehicle pose $(^{G} p_{I},_{I}^{G} R)$ by a matching algorithm, and that d_i and ^Gπ_i are known. We have the following constraints:

(1) Plane constraint (Zhao & Farrell, 2013)

^Lx_i is a laser beam; thus, it must be in the lidar scan plane (Zhao & Farrell, 2013). Let ^Lz_L =[0 0 1]^T be the normal of the lidar scan plane; then, we have the following:

$^{L} z_{L}^{T L} x_{i} = 0$ D1

(2) Distance constraint (Hesch et al., 2010)

As can be seen in Figure 3(b), the vector from {G} to point M can be obtained by either of the following:

$^{G} p_{I} +_{I}^{G} R (^{I} p_{L} +_{L}^{I} R^{L} x_{i})$ D2

or:

$d_{i}^{G} π_{i} +^{G} t_{i}$ D3

both of which are equivalent. Therefore, we have the following:

$^{G} p_{I} +_{I}^{G} R (^{I} p_{L} +_{L}^{I} R^{L} x_{i}) = d_{i}^{G} π_{i} +^{G} t_{i}$ D4

By projecting Equation (D4) onto $^{G} π_{i}^{T}$ , we obtain the following:

$^{G} π_{i}^{T} (^{G} p_{I} +_{I}^{G} R (^{I} p_{L} +_{L}^{I} R^{L} x_{i})) =^{G} π_{i}^{T} d_{i}^{G} π_{i} +^{G} π_{i}^{T G} t_{i}$ D5

Because ^Gπ_i ⊥ ^G t_i and ^G π_i is a unit vector, we can write the following expression:

$^{G} π_{i}^{T}_{L}^{G} R^{L} x_{i} = d_{i} -^{G} π_{i}^{T} (^{G} p_{I} +_{I}^{G} R^{I} p_{L})$ D6

By combining Equations (D1) and (D6), we have the following:

$A^{L} x_{i} = b$ D7

where:

$A = {[\begin{matrix} ^{L} z_{L}^{T} \\ ^{G} π_{i}^{T}_{L}^{G} R \end{matrix}]}_{2 \times 3} b = {[\begin{matrix} 0 \\ ^{L} d_{i} \end{matrix}]}_{2 \times 1}$ D8

$^{L} d_{i} = d_{i} -^{G} π_{i}^{T} (^{G} p_{I} +_{I}^{G} R^{I} p_{L})$ D9

Because ^Lx_i =[ρ_i cosφ_i ρ_i sinφ_i 0]^T is the shortest vector in the lidar scan plane from the origin of {L} to Π_i, we can solve the following optimization problem to estimate the values of ρ_i and φ_i :

$\begin{matrix} ^{L} x_{i}^{*} = \arg \min_{L_{x_{i}}} {‖^{L} x_{i} ‖}^{2} \\ s.t. A^{L} x_{i} = b \end{matrix}$ D10

where $^{L} x_{i}^{*}$ is the least-norm solution of the linear equation A^Lx_i = b (Cline & Plemmons, 1976). The solution is as follows:

$^{L} x_{i}^{*} = A^{T} {(A A^{T})}^{- 1} b$ D11

We take the following:

$^{L} a_{i} =_{G}^{L} R^{G} π_{i} = {[a_{1}^{i}, a_{2}^{i}, a_{3}^{i}]}^{T}$ D12

which is a unit vector. We can rewrite Equation (D8) as follows:

$A = [\begin{matrix} 0 & 0 & 1 \\ a_{1}^{i} & a_{2}^{i} & a_{3}^{i} \end{matrix}]$ D13

Therefore, we obtain the following:

$A^{T} {(A A^{T})}^{- 1} = \frac{[\begin{matrix} - a_{1}^{i} a_{3}^{i} & a_{1}^{i} \\ - a_{2}^{i} a_{3}^{i} & a_{2}^{i} \\ 1 - a_{3}^{i 2} & 0 \end{matrix}]}{1 - a_{3}^{i 2}}$ D14

Because $^{L} a_{i} = {[a_{1}^{i}, a_{2}^{i}, a_{3}^{i}]}^{T}$ is a unit vector, $a_{1}^{i 2} + a_{2}^{i 2} + a_{3}^{i 2} = 1$ . Thus, we have the following relationship:

$^{L} x_{i}^{*} = \frac{1}{a_{1}^{i 2} + a_{2}^{i 2}} [\begin{matrix} - a_{1}^{i} a_{3}^{i} & a_{1}^{i} \\ - a_{2}^{i} a_{3}^{i} & a_{2}^{i} \\ 1 - a_{3}^{i 2} & 0 \end{matrix}] [\begin{matrix} 0 \\ ^{L} d_{i} \end{matrix}] = [\begin{matrix} a_{1}^{i} \\ a_{2}^{i} \\ 0 \end{matrix}] \frac{^{L} d_{i}}{a_{1}^{i 2} + a_{2}^{i 2}}$ D15

$ρ_{i}^{*} [\begin{matrix} \cos ϕ_{i}^{*} \\ \sin ϕ_{i}^{*} \\ 0 \end{matrix}] = [\begin{matrix} a_{1}^{i} \\ a_{2}^{i} \\ 0 \end{matrix}] \frac{^{L} d_{i}}{a_{1}^{i 2} + a_{2}^{i 2}} = [\begin{matrix} \frac{a_{1}}{\sqrt{a_{1}^{i 2} + a_{2}^{i 2}}} \\ \frac{a_{2}}{\sqrt{a_{1}^{i 2} + a_{2}^{i 2}}} \\ 0 \end{matrix}] \frac{^{L} d_{i}}{\sqrt{a_{1}^{i 2} + a_{2}^{i 2}}}$ D16

Therefore, the least-norm solution can be obtained as follows:

$ρ_{i}^{*} = \frac{|^{L} d_{i} |}{\sqrt{a_{1}^{i 2} + a_{2}^{i 2}}}$ D17a

$ϕ_{i}^{*} = \arctan (sgn (^{L} d_{i}) \frac{a_{2}^{i}}{a_{1}^{i}})$ D17b

Appendix D.2 Line Extraction and Feature Association

(1) Extracting line segments from 2D lidar points

To extract line segments from raw 2D lidar measurements, we adopt the method of Pfister et al. (2003), which is summarized as follows. A line segment L_i is represented by three parameters, ρ_i, φ_i, and S_i, where (ρ_i, φ_i) gives the ranging and bearing parameters associated with the vector perpendicular to L_i in the polar coordinate system and S_i is the distance from the line perpendicular to the line segment’s center, as shown in Figure 3(a). For the j-th 2D lidar point, we denote the distance from the line segment as $δ_{j}^{ρ}$ and its distance from the center of the line segment projected on the line segment as $δ_{j}^{S}$ . The best-fitted line segment is found by minimizing $δ_{j}^{ρ}$ and $δ_{j}^{S}$ of all lidar points by the maximum likelihood estimation.

(2) Associating line segments with planes

As discussed in Section 3.2, the laser scan plane intersects with several planes, and these intersections are the line segments. The parameters of these line segments can be predicted according to the current pose of the vehicle, as shown in Equation (44). Therefore, the association of line segments and planes can be achieved by examining the closeness between the predicted line segment and the extracted line segment. For the r-th plane Π_r, the line segment predicted by Equation (44) is parameterized by ${\hat{y}}_{r} = {[ϕ_{r}, ρ_{r}]}^{T}$ . The extracted line segment can also be parameterized by $y_{i} = {[ϕ_{i}, ρ_{i}]}^{T}$ in the polar coordinate system. The Mahalanobis distance from ${\hat{y}}_{r}$ to y_i is given by ${‖ {\hat{y}}_{r} - y_{i} ‖}_{P_{y_{i}}}$ , where $P_{y_{i}}$ is the covariance matrix of the extracted line segment. The best-fit plane can be found by minimizing ${‖ {\hat{y}}_{r} - y_{i} ‖}_{P_{y_{i}}}$ . Assuming that the extracted line segment has zero-mean Gaussian noise, we find that the Mahalanobis distance follows a chi-squared distribution with two DOFs. If the r-th plane fails to satisfy ${‖ {\hat{y}}_{r} - y_{i} ‖}_{P_{y_{i}}} < δ_{a}$ , the r-th plane is excluded from the association process. If none of the planes satisfy this condition, the extracted line segment y_i is excluded when the final set of measurements is constructed. The threshold δ_a is determined by a given significance level, which is set as 0.05 (Pfister et al., 2003).

APPENDIX E LINEARIZATION OF AN EKF-BASED LIDAR/IMU INTEGRATED SYSTEM

Appendix E.1 Derivative of the Rotation Matrix

Let g = [g_x, g_y, g_z]^T, $q = {[q_{w}, q_{v}^{T}]}^{T}$ , and q_v =[q_x, q_y, q_z]^T ; then, the rotation matrix corresponding to q is as follows:

$R {q} = [\begin{matrix} 1 - 2 (q_{y}^{2} + q_{z}^{2}) & 2 (q_{x} q_{y} - q_{w} q_{z}) & 2 (q_{x} q_{z} + q_{w} q_{y}) \\ 2 (q_{x} q_{y} + q_{w} q_{z}) & 1 - 2 (q_{x}^{2} + q_{z}^{2}) & 2 (q_{y} q_{z} - q_{w} q_{x}) \\ 2 (q_{x} q_{z} - q_{w} q_{y}) & 2 (q_{y} q_{z} + q_{w} q_{x}) & 1 - 2 (q_{x}^{2} + q_{y}^{2}) \end{matrix}]$ E1

The derivative of R^T{q}g with respect to q can be solved by performing an element-wise derivation:

$\frac{\partial R^{T} {q} g}{\partial q} = 2 [u_{g} {[u_{g} + q_{w} g]}_{\times} + (q_{v} \cdot g) I_{3} - g q_{v}^{T}]$ E2

where:

$u_{g} = g \times q_{v} = {[g]}_{\times} q_{v}$ E3

Because R{q} = R^T {q*}, where q* is the conjugate of q, we can obtain the derivative of R{q}g with respect to q:

$\frac{\partial R {q} g}{\partial q} = \frac{\partial R^{T} {q^{*}} g}{\partial q} = \frac{\partial R^{T} {q^{*}} g}{\partial q^{*}} \frac{\partial q^{*}}{\partial q}$ E4

where:

$\frac{\partial q^{*}}{\partial q} = [\begin{matrix} \frac{\partial q^{*}}{\partial q_{w}} & \frac{\partial q^{*}}{\partial q_{x}} & \frac{\partial q^{*}}{\partial q_{y}} & \frac{\partial q^{*}}{\partial q_{z}} \end{matrix}] = diag [1, - 1, - 1, - 1]$ E5

Therefore, we have the following:

$\frac{\partial R {q} g}{\partial q} = \frac{\partial R^{T} {q^{*}} g}{\partial q^{*}} diag [1, - 1, - 1, - 1]$ E6

For convenience, we introduce two symbols:

$J_{q} (q, g) = \frac{\partial R^{T} {q} g}{\partial q}$ E7a

$J_{q}^{*} (q, g) = \frac{\partial R {q} g}{\partial q}$ E7b

Appendix E.2 Linearization of the Rotation Part of the State Propagation Equation

The rotation part in the state propagation equation in Equation (41) is given by the following:

$^{I} q_{G, k + 1} = g (^{I} q_{G, k}, w_{G I, k}) = \exp {(\frac{Δ t}{2} Ω [w_{G I, k}])}^{I} q_{G, k}$ E8a

$w_{G I, k} = w_{m, k} - b_{g, k} - n_{g}$ E8b

where exp(·) is a matrix function defined by the following:

$\exp (M) = \sum_{i = 0}^{n} \frac{M^{n}}{n!}$ E9

Therefore, Equation (E8a) can be expanded as follows:

$g (^{I} q_{G, k}, w_{G I, k}) = {(I_{4} + \frac{1}{2} Ω [w_{G I, k}] Δ t + O ({(Ω [w_{G I, k}])}^{2}))}^{I} q_{G, k}$ E10

By keeping the first-order term and neglecting the higher-order terms in the above equations, we have the following:

$g (^{I} q_{G, k}, w_{G I, k}) = {(I_{4} + \frac{1}{2} Ω [w_{G I, k}] Δ t)}^{I} q_{G, k} = {[\begin{matrix} I_{3} - \frac{1}{2} Δ t {[W_{G I, k}]}_{\times} & \frac{1}{2} Δ t w_{G I, k} \\ - \frac{1}{2} Δ t w_{G I, k}^{T} & 1 \end{matrix}]}^{I} q_{G, k}$ E11

Let $^{I} {\hat{q}}_{G, k} = [{\hat{q}}_{w, k}, {\hat{q}}_{x, k}, {\hat{q}}_{y, k}, {\hat{q}}_{z, k}]$ be the estimated vehicle orientation in terms of quaternions at time k; then, the Jacobian matrix of g(^Iq_G,k, w_GI,k) with respect to ^Iq_G,k can be obtained by taking the first-order Taylor expansion at the point $(^{I} {\hat{q}}_{G, k}, w_{m, k})$ :

$\frac{\partial g (^{I} {\hat{q}}_{G, k}, w_{m, k})}{\partial^{I} q_{G, k}} = [\begin{matrix} I_{3} - \frac{1}{2} Δ t {[w_{G I, k}]}_{\times} & \frac{1}{2} Δ t w_{G I, k} \\ - \frac{1}{2} Δ t w_{G I, k}^{T} & 1 \end{matrix}]$ E12

Similarly, the Jacobian matrix of g(^Iq_G,k, w_GI,k) evaluated at $(^{I} {\hat{q}}_{G, k}, w_{m, k})$ with respect to w_GI,k can be obtained as follows:

$\begin{matrix} \frac{\partial g (^{I} {\hat{q}}_{G, k}, w_{m, k})}{\partial w_{G I, k}} & = [\frac{\partial g (^{I} {\hat{q}}_{G, k}, w_{m, k})}{\partial w_{x, k}}, \frac{\partial g (^{I} {\hat{q}}_{G, k}, w_{m, k})}{\partial w_{y, k}}, \frac{\partial g (^{I} {\hat{q}}_{G, k}, w_{m, k})}{\partial w_{z, k}}] \\ = \frac{Δ t}{2} [\begin{matrix} {\hat{q}}_{z, k} & - {\hat{q}}_{y, k} & {\hat{q}}_{x, k} \\ {\hat{q}}_{y, k} & {\hat{q}}_{z, k} & - {\hat{q}}_{w, k} \\ - {\hat{q}}_{x, k} & {\hat{q}}_{w, k} & {\hat{q}}_{z, k} \\ - {\hat{q}}_{w, k} & - {\hat{q}}_{x, k} & - {\hat{q}}_{y, k} \end{matrix}] \end{matrix}$ E13

Appendix E.3 Linearization of the Measurement Function

As defined in Section 3.2, hⁱ(x_k) consists of two parts: $h_{1}^{i} (x)$ and $h_{2}^{i} (x)$ . We will derive $\frac{\partial h_{1}^{i} ({\hat{x}}_{k}^{-})}{\partial x_{k}}$ and $\frac{\partial h_{2}^{i} ({\hat{x}}_{k}^{-})}{\partial x_{k}}$ separately. To simplify our notation, we omit the indices i and k in x_k, ^Gp_I,k, ^Iq_G,k, ${\hat{x}}_{k}^{-}$ , $^{I} {\hat{p}}_{G, k}^{-}$ , $^{I} {\hat{q}}_{G, k}^{-}$ , $a_{1}^{i, k}$ , $a_{2}^{i, k}$ , and $a_{3}^{i, k}$ during the following derivation without a loss of generality.

(1) First part of the Jacobian matrix

The first part of the Jacobian matrix $H_{k}^{i}$ defined in Equation (45) can be written as follows:

$\frac{\partial h_{1}^{i} ({\hat{x}}^{-})}{\partial x} = [\frac{\partial h_{1}^{i} ({\hat{x}}^{-})}{\partial^{G} p_{I}}, 0_{1 \times 3}, \frac{\partial h_{1}^{i} ({\hat{x}}^{-})}{\partial^{I} q_{G}}, 0_{1 \times 6}]$ E14

where:

$h_{1}^{i} (x) = \arctan (sgn (^{L} d_{i, k}) \frac{a_{2}}{a_{1}})$ E15

x, ^Gp_I, and ^Iq_G are defined in Equation (40), $^{L} a_{i, k} =_{G}^{L} R^{G} π_{i, k} = {[a_{1}, a_{2}, a_{3}]}^{T}$ is the discrete form of ^La_i defined in Equation (D12), and ^Ld_i,k is the discrete form of ^Ld_i defined in Equation (D9). Applying the chain rule yields the following:

$\frac{\partial h_{1}^{i} (\cdot)}{\partial^{I} q_{G}} = \frac{\partial h_{1}^{i} (\cdot)}{\partial^{L} a_{i, k}} \cdot \frac{\partial^{L} a_{i, k}}{\partial^{I} q_{G}}$ E16

By substituting Equation (E15) into Equation (E16), the first term of Equation (E16) can be written as follows:

$\frac{\partial h_{1}^{i} (\cdot)}{\partial^{L} a_{i, k}} = \frac{\partial \arctan (sgn (^{L} d_{i, k}) \frac{a_{2}}{a_{1}})}{\partial^{L} a_{i, k}}$ E17

where sgn(·) is the sign function and $\frac{d sgn (x)}{d x} = 2 δ (x)$ . Now, Equation (E17) can be reduced as follows:

$\frac{\partial h_{1}^{i} (\cdot)}{\partial^{L} a_{i, k}} = {(1 + \frac{a_{2}^{2}}{a_{1}^{2}})}^{- 1} (2 δ (^{L} d_{i, k}) \frac{\partial^{L} d_{i, k}}{\partial^{L} a_{i, k}} \cdot \frac{a_{2}}{a_{1}} + sgn (^{L} d_{i, k}) \frac{\partial \frac{a_{2}}{a_{1}}}{\partial^{L} a_{i, k}})$ E18

According to Equations (D6) and (D9), we have the following:

$^{L} d_{i, k} = d_{i, k} -^{G} π_{i, k}^{T} (^{G} p_{I} +_{I}^{G} R^{I} p_{L}) =^{G} π_{i, k}^{T}_{L}^{G} R^{L} x_{i, k}^{*}$ E19

where $_{L}^{G} R^{L} x_{i, k}^{*}$ represents the shortest vector from the origin of {L} to plane Π_i in {G} at time k, which cannot be zero. In addition, ^Gπ_i,k is the normal of the planar surface, which cannot be zero. Therefore, ^Ld_i,k ≠ 0, and thus, δ (^Ld_i,k) = 0. Then, Equation (E18) is simplified as follows:

$\frac{\partial h_{1}^{i} (\cdot)}{\partial^{L} a_{i, k}} = sgn (^{L} d_{i, k}) \frac{\partial \frac{a_{2}}{a_{1}}}{\partial^{L} a_{i, k}} {(1 + \frac{a_{2}^{2}}{a_{1}^{2}})}^{- 1}$ E20

where:

$\frac{\partial \frac{a_{2}}{a_{1}}}{\partial^{L} a_{i, k}} = [\frac{\partial \frac{a_{2}}{a_{1}}}{\partial a_{1}}, \frac{\partial \frac{a_{2}}{a_{1}}}{\partial a_{2}}, \frac{\partial \frac{a_{2}}{a_{1}}}{\partial a_{3}}] = [- \frac{a_{2}}{a_{1}^{2}}, \frac{1}{a_{1}}, 0]$ E21

Therefore, Equation (E20) has the following expression:

$\frac{\partial h_{1}^{i} (\cdot)}{\partial^{L} a_{i, k}} = \frac{1}{μ_{i, k}} λ_{i, k}^{T}$ E22

where:

$μ_{i, k} = a_{1}^{2} + a_{2}^{2},$ E23a

$λ_{i, k}^{T} = sgn (^{L} d_{i, k}) [- a_{2}, a_{1}, 0]$ E23b

The second term in Equation (E16) is given as follows:

$\frac{\partial^{L} a_{i, k}}{\partial^{I} q_{G}} = \frac{\partial_{G}^{L} R^{G} π_{i, k}}{\partial^{I} q_{G}} =_{I}^{L} R \frac{\partial_{G}^{I} R^{T G} π_{i, k}}{\partial^{I} q_{G}}$ E24

The above equation contains the partial derivative of the rotation matrix with respect to the quaternion, which can be solved by taking the derivative of the rotation matrix in an element-wise manner, as shown in Appendix E.1. Therefore, Equation (E24) can be derived by using the conclusion in Appendix E.1:

$\frac{\partial^{L} a_{i, k}}{\partial^{I} q_{G}} =_{I}^{L} R J_{q}^{*} (^{I} q_{G},^{G} π_{i, k})$ E25

where:

$J_{q}^{*} (^{I} q_{G},^{G} π_{i, k}) = J_{q} (^{I} q_{G}^{*},^{G} π_{i, k}) diag [1, - 1, - 1, - 1]$ E26a

$J_{q} (^{I} q_{G},^{G} π_{i, k}) = 2 [u_{π}, {[u_{π} + q_{w}^{G} π_{i, k}]}_{\times} + (q_{v} \cdot^{G} π_{i, k}) I_{3} -^{G} π_{i, k} q_{v}^{T}]$ E26b

$u_{π} =^{G} π_{i, k} \times q_{v} = {[^{G} π_{i, k}]}_{\times} q_{v}$ E26c

$^{I} q_{G} = {[q_{w}, q_{v}^{T}]}^{T} = {[q_{w}, q_{x}, q_{y}, q_{z}]}^{T}$ E26d

Substituting Equations (E22) and (E25) into Equation (E16) yields the following:

$\frac{\partial h_{1}^{i} (\cdot)}{\partial^{I} q_{G}} = \frac{1}{μ_{i, k}} λ_{i, k I}^{T}^{L} R J_{q}^{*} (^{I} q_{G},^{G} π_{i, k})$ E27

Similarly, according to the chain rule, we have the following:

$\frac{\partial h_{1}^{i} (\cdot)}{\partial^{G} p_{I}} = \frac{\partial h_{1}^{i} (\cdot)}{\partial^{L} a_{i, k}} \cdot \frac{\partial^{L} a_{i, k}}{\partial^{G} p_{I}}$ E28

Because $^{L} a_{i, k} =_{G}^{L} R^{G} π_{i, k}$ , which indicates that ^La_i,k is only related to the rotation part of the state, Equation (E28) has the following expression:

$\frac{\partial h_{1}^{i} (\cdot)}{\partial^{G} p_{I}} = 0$ E29

By substituting Equations (E27) and (E29) into Equation (E14), we obtain the final expression of the first part of the Jacobian matrix:

$\frac{\partial h_{1}^{i} ({\hat{x}}^{-})}{\partial x} = [0_{1 \times 6}, \frac{1}{{\hat{μ}}_{i, k}^{-}} {\hat{λ}}_{i, k}^{- T}_{I}^{L} R J_{q}^{*} (^{I} {\hat{q}}_{G}^{-},^{G} π_{i, k}), 0_{1 \times 6}]$ E30

where ${\hat{μ}}_{i, k}^{-}$ is defined in Equation (E23a) and ${\hat{λ}}_{i, k}^{-}$ is defined in Equation (E23b), both of which are evaluated at ${\hat{x}}^{-}$ .

(2) Second part of the Jacobian matrix

The second part of the Jacobian matrix $H_{k}^{i}$ can be written as follows:

$\frac{\partial h_{2}^{i} ({\hat{x}}^{-})}{\partial x} = [\frac{\partial h_{2}^{i} ({\hat{x}}^{-})}{\partial^{G} p_{I}}, 0_{1 \times 3}, \frac{\partial h_{2}^{i} ({\hat{x}}^{-})}{\partial^{I} q_{G}}, 0_{1 \times 6}]$ E31

where:

$h_{2}^{i} (x) = \frac{|^{L} d_{i, k} |}{\sqrt{a_{1}^{2} + a_{2}^{2}}}$ E32

We first consider the derivative with respect to the rotation part:

$\frac{\partial h_{2}^{i} (\cdot)}{\partial^{I} q_{G}} = \frac{1}{\sqrt{a_{1}^{2} + a_{2}^{2}}} \frac{{\partial |}^{L} d_{i, k} ∣}{\partial^{I} q_{G}} + |^{L} d_{i, k} | \frac{\partial {(a_{1}^{2} + a_{2}^{2})}^{- \frac{1}{2}}}{\partial^{I} q_{G}}$ E33

In the first part of Equation (E33), we have the following:

$\frac{\partial |^{L} d_{i, k} |}{\partial^{I} q_{G}} = sgn (^{L} d_{i, k}) \frac{\partial^{L} d_{i, k}}{\partial^{I} q_{G}}$ E34

Substituting Equation (D9) into Equation (E34) yields the following:

$\frac{\partial |^{L} d_{i, k} |}{\partial^{I} q_{G}} = sgn (^{L} d_{i, k}) \frac{\partial (d_{i, k} -^{G} π_{i, k}^{T} (^{G} p_{I} +_{I}^{G} R^{I} p_{L}))}{\partial^{I} q_{G}}$ E35

According to Appendix E.1, we have the following relationship:

$\frac{\partial_{I}^{G} R^{I} p_{L}}{\partial^{I} q_{G}} = \frac{\partial_{G}^{I} R^{T I} p_{L}}{\partial^{I} q_{G}} = J_{q} (^{I} q_{G},^{I} p_{L})$ E36

By substituting Equation (E36) into Equation (E35), we obtain the following:

$\frac{\partial |^{L} d_{i, k} |}{\partial^{I} q_{G}} = - sgn {(^{L} d_{i, k})}^{G} π_{i, k}^{T} J_{q} (^{I} q_{G},^{I} p_{L})$ E37

In the second part of Equation (E33), the chain rule is applied:

$\frac{\partial {(a_{1}^{2} + a_{2}^{2})}^{- \frac{1}{2}}}{\partial^{I} q_{G}} = \frac{\partial {(a_{1}^{2} + a_{2}^{2})}^{- \frac{1}{2}}}{\partial^{L} a_{i, k}} \cdot \frac{\partial^{L} a_{i, k}}{\partial^{I} q_{G}}$ E38

By expanding ^La_i into its element-wise form, we obtain the following:

$\frac{\partial {(a_{1}^{2} + a_{2}^{2})}^{- \frac{1}{2}}}{\partial^{L} a_{i, k}} = [\frac{\partial {(a_{1}^{2} + a_{2}^{2})}^{- \frac{1}{2}}}{\partial a_{1}}, \frac{\partial {(a_{1}^{2} + a_{2}^{2})}^{- \frac{1}{2}}}{\partial a_{2}}, \frac{\partial {(a_{1}^{2} + a_{2}^{2})}^{- \frac{1}{2}}}{\partial a_{3}}] = - {(a_{1}^{2} + a_{2}^{2})}^{- \frac{3}{2}} [a_{1}, a_{2}, 0]$ E39

We then substitute Equations (E25) and (E39) into Equation (E38) as follows:

$\frac{\partial {(a_{1}^{2} + a_{2}^{2})}^{- \frac{1}{2}}}{\partial^{I} q_{G}} = - {(a_{1}^{2} + a_{2}^{2})}^{- \frac{3}{2}} {[a_{1}, a_{2}, 0]}_{I}^{L} R J_{q}^{*} (^{I} q_{G},^{G} π_{i, k})$ E40

By substituting Equations (E37) and (E40) into Equation (E33), the final expression of Equation (E33) is written as follows:

$\frac{\partial h_{2}^{i} (\cdot)}{\partial^{I} q_{G}} = - sgn (^{L} d_{i, k}) \frac{^{G} π_{i, k}^{T}}{\sqrt{μ_{i, k}}} J_{q} (^{I} q_{G},^{I} p_{L}) + |^{L} d_{i, k} | κ_{i, k I}^{T}^{L} R J_{q}^{*} (^{I} q_{G},^{G} π_{i, k})$ E41

where:

$κ_{i, k}^{T} = - μ_{i, k}^{- \frac{3}{2}} [a_{1}, a_{2}, 0]$ E42

Similarly, we have the following:

$\frac{\partial h_{2}^{i} (\cdot)}{\partial^{G} p_{I}} = \frac{1}{\sqrt{a_{1}^{2} + a_{2}^{2}}} \frac{\partial |^{L} d_{i, k} |}{\partial^{G} p_{I}} + |^{L} d_{i, k} | \frac{\partial {(a_{1}^{2} + a_{2}^{2})}^{- \frac{1}{2}}}{\partial^{G} p_{I}}$ E43

In the first part of Equation (E43), we have the following:

$\frac{\partial |^{L} d_{i, k} |}{\partial^{G} p_{I}} = sgn (^{L} d_{i, k}) \frac{\partial^{L} d_{i, k}}{\partial^{G} p_{I}}$ E44

We substitute Equation (D9) into Equation (E44) as follows:

$\frac{\partial |^{L} d_{i, k} |}{\partial^{G} p_{I}} = sgn (^{L} d_{i, k}) \frac{\partial (d_{i, k} -^{G} π_{i, k}^{T} (^{G} p_{I} +_{I}^{G} R^{I} p_{L}))}{\partial^{G} p_{I}} = - sgn {(^{L} d_{i, k})}^{G} π_{i, k}^{T}$ E45

In the second part of Equation (E43), because $^{L} a_{i, k} =_{G}^{L} R^{G} π_{i, k} = {[a_{1}, a_{2}, a_{3}]}^{T}$ , a₁ and a₂ are only related to the rotation part of the state. Therefore, we have the following:

$\frac{\partial {(a_{1}^{2} + a_{2}^{2})}^{- \frac{1}{2}}}{\partial^{G} p_{I}} = 0$ E46

Substituting Equations (E44) and (E46) into Equation (E43) yields the following:

$\frac{\partial h_{2}^{i} (\cdot)}{\partial^{G} p_{I}} = - sgn (^{L} d_{i, k}) \frac{^{G} π_{i, k}^{T}}{\sqrt{μ_{i, k}}}$ E47

By substituting Equations (E41) and (E47) into Equation (E31), we obtain the final expression of the second part of the Jacobian matrix:

$\frac{\partial h_{2}^{i} ({\hat{x}}^{-})}{\partial x} = [- sgn (^{L} {\hat{d}}_{i, k}^{-}) \frac{^{G} π_{i, k}^{T}}{\sqrt{{\hat{μ}}_{i, k}^{-}}}, 0_{1 \times 3}, - sgn (^{L} {\hat{d}}_{i, k}^{-}) \frac{^{G} π_{i, k}^{T}}{\sqrt{{\hat{μ}}_{i, k}^{-}}} J_{q} (^{I} {\hat{q}}_{G}^{-},^{I} p_{L}) + |^{L} {\hat{d}}_{i, k}^{-} | {\hat{κ}}_{i, k}^{T}_{I}^{L} R J_{q}^{*} (^{I} {\hat{q}}_{G}^{-},^{G} π_{i, k}), 0_{1 \times 6}]$ E48

where ${\hat{κ}}_{i, k}^{T}$ is defined in Equation (E42) and evaluated at ${\hat{x}}^{-}$ .

By re-arranging Equations (E30) and (E48), the Jacobian matrix of hⁱ(x_k) defined with respect to x_k and evaluated at ${\hat{x}}_{k}^{-}$ can be written as follows:

$H_{k}^{i} = [\begin{matrix} 0_{1 \times 3} & 0_{1 \times 3} & \frac{1}{{\hat{μ}}_{i, k}^{-}} {\hat{λ}}_{i, k}^{- T}_{I}^{L} R J_{q}^{*} (^{I} {\hat{q}}_{G}^{-},^{G} π_{i, k}) & 0_{1 \times 6} \\ - sgn (^{L} {\hat{d}}_{i, k}^{-}) \frac{^{G} π_{i, k}^{T}}{\sqrt{μ_{i, k}^{-}}} & 0_{1 \times 3} & - sgn (^{L} {\hat{d}}_{i, k}^{-}) \frac{^{G} π_{i, k}^{T}}{\sqrt{{\hat{μ}}_{i, k}^{-}}} J_{q} (^{I} {\hat{q}}_{G}^{-},^{I} p_{L}) + |^{L} {\hat{d}}_{i, k}^{-} | {\hat{κ}}_{i, k}^{T}_{I}^{L} R J_{q}^{*} (^{I} {\hat{q}}_{G}^{-},^{G} π_{i, k}) & 0_{1 \times 6} \end{matrix}]$ E49

APPENDIX F ADDITIONAL RESULTS FOR FAULT DETECTION

View this table:

TABLE F1

Statistical Results of FAR and FDR for a Step Failure (α = 0.01)

View this table:

TABLE F2

Statistical Results of FAR and FDR for a Step Failure (α = 0.001)

This is an open access article under the terms of the Creative Commons Attribution License, which permits use, distribution and reproduction in any medium, provided the original work is properly cited.

REFERENCES

↵
Ali-Loytty, S., & Sirola, N. (2007). Gaussian mixture filters and hybrid positioning. Proc. of the 20th International Technical Meeting of the Satellite Division of the Institute of Navigation (ION GNSS 2007), Fort Worth, TX, 562–570. https://www.ion.org/publications/abstract.cfm?articleID=7554
Google Scholar
↵
Analog Devices. (2018). ADIS16465: Precision MEMS IMU module data sheet (Rev. C). https://www.analog.com/media/en/technicaldocumentation/data-sheets/adis16465.pdf
Google Scholar
↵
Angus, J. E. (2006). RAIM with multiple faults. NAVIGATION, 53(4), 249–257. https://doi.org/10.1002/j.2161-4296.2006.tb00387.x
Google Scholar
↵
Blanch, J., Walker, T., & Enge, P. (2015). Fast multiple fault exclusion with a large number of measurements. Proc. of the 2015 International Technical Meeting of the Institute of Navigation, Dana Point, CA, 696–701. https://www.ion.org/publications/abstract.cfm?articleID=12662
Google Scholar
↵
Blanch, J., Walter, T., & Enge, P. (2008). Position error bound calculation for GNSS using measurement residuals. IEEE Transactions on Aerospace and Electronic Systems, 44(3), 977–984. https://doi.org/10.1109/TAES.2008.4655357
Google Scholar
↵
Blender Optimization Community. (2018). Blender - a 3D modelling and rendering package. Blender Foundation. Stichting Blender Foundation, Amsterdam. http://www.blender.org
Google Scholar
↵
Carballo, A., Lambert, J., Monrroy, A., Wong, D., Narksri, P., Kitsukawa, Y., Takeuchi, E., Kato, S., & Takeda, K. (2020). LIBRE: The multiple 3D lidar dataset. Proc. of the 2020 IEEE Intelligent Vehicles Symposium (IV), Las Vegas, NV, 1094–1101. https://doi.org/10.1109/IV47402.2020.9304681
Google Scholar
↵
Cline, R. E., & Plemmons, R. J. (1976). L2-solutions to underdetermined linear systems. SIAM Review, 18(1), 92–106. https://doi.org/10.1137/1018004
Google Scholar
↵
Da, R. (1994). Failure detection of dynamical systems with the state chi-square test. Journal of Guidance, Control, and Dynamics, 17(2), 271–277. https://doi.org/10.2514/3.21193
Google Scholar CrossRef
↵
Daum, F. (2005). Nonlinear filters: Beyond the Kalman filter. IEEE Aerospace and Electronic Systems Magazine, 20(8), 57–69. https://doi.org/10.1109/MAES.2005.1499276
Google Scholar
↵
Davis, B., & Blair, W. D. (2015). Gaussian mixture approach to long range radar tracking with high range resolution. Proc. of the 2015 IEEE Aerospace Conference, Big Sky, MT, 1–9. https://doi.org/10.1109/AERO.2015.7119222
Google Scholar
↵
Dosovitskiy, A., Ros, G., Codevilla, F., Lopez, A., & Koltun, V. (2017). CARLA: An open urban driving simulator. Proc. of the 1st Conference on Robot Learning (CoRL), Mountain View, CA, 1–16. ArXiv. https://doi.org/10.48550/arXiv.1711.03938
Google Scholar
↵
Fisher, R. A., & Yates, F. (1953). Statistical tables for biological, agricultural and medical research. Hafner Publishing Company. https://books.google.com/books/about/Statistical_Tables_for_Biological_Agricu.html?id=qfw9AAAAYAAJ
Google Scholar
↵
Gao, G., Gao, S., Hong, G., Peng, X., & Yu, T. (2020). A robust INS/SRS/CNS integrated navigation system with the chi-square test-based robust Kalman filter. Sensors, 20(20), 5909. https://doi.org/10.3390/s20205909
Google Scholar
↵
Gao, Z., Ding, S. X., & Cecati, C. (2015). Real-time fault diagnosis and fault-tolerant control. IEEE Transactions on Industrial Electronics, 62(6), 3752–3756. https://doi.org/10.1109/TIE.2015.2417511
Google Scholar
↵
Gao, Z., Fang, K., Wang, Z., Guo, K., & Liu, Y. (2022). An error overbounding method based on a Gaussian mixture model with uncertainty estimation for a dual-frequency ground-based augmentation system. Remote Sensing, 14(5), 1111. https://doi.org/10.3390/rs14051111
Google Scholar
↵
Hashemi, N., & Ruths, J. (2019). Generalized chi-squared detector for LTI systems with non-Gaussian noise. Proc. of the 2019 American Control Conference (ACC), Philadelpha, PA, 404–410. https://doi.org/10.23919/ACC.2019.8815139
Google Scholar
↵
Hesch, J. A., Mirzaei, F. M., Mariottini, G. L., & Roumeliotis, S. I. (2010). A laser-aided inertial navigation system (L-INS) for human localization in unknown indoor environments. Proc. of the 2010 IEEE International Conference on Robotics and Automation, Anchorage, AK, 5376–5382. https://doi.org/10.1109/ROBOT.2010.5509693
Google Scholar
↵
Hou, H., & El-Sheimy, N. (2003). Inertial sensors errors modeling using Allan variance. Proc. of the 16th International Technical Meeting of the Satellite Division of the Institute of Navigation (ION GPS/GNSS 2003), Portland, OR, 2860–2867. https://www.ion.org/publications/abstract.cfm?articleID=5475
Google Scholar
↵
Hsu, L. T., Tokura, H., Kubo, N., Gu, Y., & Kamijo, S. (2017). Multiple faulty GNSS measurement exclusion based on consistency check in urban canyons. IEEE Sensors Journal, 17(6), 1909–1917. https://doi.org/10.1109/JSEN.2017.2654359
Google Scholar
↵
Joerger, M., Chan, F.-C., & Pervan, B. (2014). Solution separation versus residual-based RAIM. NAVIGATION, 61(4), 273–291. https://doi.org/10.1002/navi.71
Google Scholar
↵
Joerger, M., & Pervan, B. (2016). Fault detection and exclusion using solution separation and chi-squared ARAIM. IEEE Transactions on Aerospace and Electronic Systems, 52(2), 726–742. https://doi.org/10.1109/TAES.2015.140589
Google Scholar
↵
Joseph, G., & Murthy, C. R. (2019). Measurement bounds for observability of linear dynamical systems under sparsity constraints. IEEE Transactions on Signal Processing, 67(8), 1992–2006. https://doi.org/10.1109/TSP.2019.2899812
Google Scholar
↵
Langel, S., Crespillo, O. G., & Joerger, M. (2020). Overbounding sequential estimation errors due to non-Gaussian correlated noise. Proc. of the 33rd International Technical Meeting of the Satellite Division of the Institute of Navigation (ION GNSS+ 2020), 1054–1067. https://doi.org/10.33012/2020.17576
Google Scholar
↵
Lefferts, E., Markley, F., & Shuster, M. (1982). Kalman filtering for spacecraft attitude estimation. Journal of Guidance, Control, and Dynamics, 5(5), 417–429. https://doi.org/10.2514/3.56190
Google Scholar CrossRef Web of Science
↵
Lethander, K. A., & Taylor, C. N. (2023). Conservative estimation of inertial sensor errors using Allan variance data. NAVIGATION, 70(3). https://doi.org/10.33012/navi.563
Google Scholar
↵
Li, M., & Schwartzman, A. (2018). Standardization of multivariate Gaussian mixture models and background adjustment of PET images in brain oncology. The Annals of Applied Statistics, 12(4), 2197–2227. https://doi.org/10.1214/18-AOAS1149
Google Scholar
↵
Liu, C., Wang, H., Li, N., & Yu, Y. (2017). Sensor fault diagnosis of GPS/INS tightly coupled navigation system based on state chi-square test and improved simplified fuzzy ARTMAP neural network. Proc. of the 2017 IEEE International Conference on Robotics and Biomimetics (ROBIO), Macau, Macao, 2527–2532. https://doi.org/10.1109/ROBIO.2017.8324800
Google Scholar
↵
MacKay, D. J. (2003). Information theory, inference and learning algorithms. Cambridge University Press. https://www.cambridge.org/us/universitypress/subjects/computer-science/pattern-recognition-and-machine-learning/information-theory-inference-and-learning-algorithms?format=HB&isbn=9780521642989
Google Scholar
↵
Manning, C., & Schutze, H. (1999). Foundations of statistical natural language processing. MIT Press. https://mitpress.mit.edu/9780262133609/foundations-of-statistical-natural-language-processing/
Google Scholar
↵
Osechas, O., Misra, P., & Rife, J. (2012). Carrier-phase acceleration RAIM for GNSS satellite clock fault detection. NAVIGATION, 59(3), 221–235. https://doi.org/10.1002/navi.17
Google Scholar
↵
Pervan, B. S., Pullen, S. P., & Christie, J. R. (1998). A multiple hypothesis approach to satellite navigation integrity. NAVIGATION, 45(1), 61–71. https://doi.org/10.1002/j.2161-4296.1998.tb02372.x
Google Scholar
↵
Pfeifer, T., & Protzel, P. (2019). Expectation-maximization for adaptive mixture models in graph optimization. 2019 International Conference on Robotics and Automation (ICRA), 3151–3157. https://doi.org/10.1109/ICRA.2019.8793601
Google Scholar
↵
Pfister, S., Roumeliotis, S., & Burdick, J. (2003). Weighted line fitting algorithms for mobile robot map building and efficient data representation. Proc. of the 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422), Taipei, Taiwan, 1304–1311. https://doi.org/10.1109/ROBOT.2003.1241772
Google Scholar
↵
Puchalski, R., & Giernacki, W. (2022). UAV fault detection methods, state-of-the-art. Drones, 6(11), 330. https://doi.org/10.3390/drones6110330
Google Scholar
↵
Rife, J., & Pervan, B. (2012). Overbounding revisited: Discrete error-distribution modeling for safety-critical GPS navigation. IEEE Transactions on Aerospace and Electronic Systems, 48(2), 1537–1551. https://doi.org/10.1109/TAES.2012.6178077
Google Scholar
↵
Rife, J. H. (2013). The effect of uncertain covariance on a chi-square integrity monitor. NAVIGATION, 60(4), 291–303. https://doi.org/10.1002/navi.45
Google Scholar
↵
Rife, J. H. (2018). Overbounding risk for quadratic monitors with arbitrary noise distributions. IEEE Transactions on Aerospace and Electronic Systems, 54(4), 1618–1627. https://doi.org/10.1109/TAES.2018.2798258
Google Scholar
↵
Sanandaji, B. M., Wakin, M. B., & Vincent, T. L. (2014). Observability with random observations. IEEE Transactions on Automatic Control, 59(11), 3002–3007. https://doi.org/10.1109/TAC.2014.2351693
Google Scholar
↵
Toschi, I., Rodríguez-Gonzálvez, P., Remondino, F., Minto, S., Orlandini, S., & Fuller, A. (2015). Accuracy evaluation of a mobile mapping system with advanced statistical methods. The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, 40, 245–253. https://doi.org/10.5194/isprsarchives-XL-5-W4-245-2015
Google Scholar
↵
Walter, T., & Enge, P. (1995). Weighted RAIM for precision approach. Proc. of the 8th International Technical Meeting of the Satellite Division of the Institute of Navigation (ION GPS 1995), Palm Springs, CA, 1995–2004. https://www.ion.org/publications/abstract.cfm?articleID=2524
Google Scholar
↵
Wang, J., Xu, C., Shi, M., & Lu, Z. (2022). Protection level for precise point positioning based on Gaussian mixture model. Proc. of the China Satellite Navigation Conference (CSNC 2022), Beijing, China, 45–55. https://doi.org/10.1007/978-981-19-2580-1_4
Google Scholar
↵
Wang, R., Xiong, Z., Liu, J., Xu, J., & Shi, L. (2016). Chi-square and SPRT combined fault detection for multisensor navigation. IEEE Transactions on Aerospace and Electronic Systems, 52(3), 1352–1365. https://doi.org/10.1109/TAES.2016.140860
Google Scholar
↵
Weiss, N., Holmes, P., & Hardy, M. (2006). A course in probability. Pearson Addison Wesley. https://books.google.com.hk/books?id=Be9fJwAACAAJ
Google Scholar
↵
Wen, W., Pfeifer, T., Bai, X., & Hsu, L. T. (2021). Factor graph optimization for GNSS/INS integration: A comparison with the extended Kalman filter. NAVIGATION, 68(2), 315–331. https://doi.org/10.1002/navi.421
Google Scholar
↵
Xu, F., Wang, J., Zhu, D., & Tu, Q. (2018). Speckle noise reduction technique for lidar echo signal based on self-adaptive pulse-matching independent component analysis. Optics and Lasers in Engineering, 103, 92–99. https://doi.org/10.1016/j.optlaseng.2017.12.002
Google Scholar
↵
Yang, L., Knight, N. L., Li, Y., & Rizos, C. (2013). Optimal fault detection and exclusion applied in GNSS positioning. The Journal of Navigation, 66(5), 683–700. https://doi.org/10.1017/S0373463313000155
Google Scholar
↵
Yun, Y., Yun, H., Kim, D., & Kee, C. (2008). A Gaussian sum filter approach for DGNSS integrity monitoring. The Journal of Navigation, 61(4), 687–703. https://doi.org/10.1017/S0373463308004906
Google Scholar
↵
Zhao, S., & Farrell, J. A. (2013). 2D LIDAR aided INS for vehicle positioning in urban environments. Proc. of the 2013 IEEE International Conference on Control Applications (CCA), Hyderabad, India, 376–381. https://doi.org/10.1109/CCA.2013.6662778
Google Scholar

PDF

Previous Next

In this Issue

Print
Download PDF
Article Alerts

Alerts for this Article

User Name *

Password *

Sign In to Email Alerts with your Email Address
Email *
Email Article

Email This Article

Thank you for your interest in spreading the word on NAVIGATION: Journal of the Institute of Navigation.
NOTE: We only request your email address so that the person you are recommending the page to knows that you wanted them to see it, and that it is not junk mail. We do not capture any email address.
Your Email *

Your Name *

Send To *

Enter multiple addresses on separate lines or separate them with commas.

You are going to email the following
Fault Detection Algorithm for Gaussian Mixture Noises: An Application in Lidar/IMU Integrated Localization Systems
Message Subject
(Your Name) has sent you a message from NAVIGATION: Journal of the Institute of Navigation

Message Body
(Your Name) thought you would like to see the NAVIGATION: Journal of the Institute of Navigation web site.

Your Personal Message
Citation Tools
Citation Tools
Fault Detection Algorithm for Gaussian Mixture Noises: An Application in Lidar/IMU Integrated Localization Systems

Penggao Yan

Penggao Yan

Zhengdao Li

Zhengdao Li

Feng Huang

Feng Huang

Weisong Wen,

Weisong Wen,

and Li-Ta Hsu

Li-Ta Hsu

Institute of Navigation

Mar 2025,

72

(1)

navi.684;

DOI: 10.33012/navi.684

Citation Manager Formats

BibTeX
Bookends
EasyBib
EndNote (tagged)
EndNote 8 (xml)
Medlars
Mendeley
Papers
RefWorks Tagged
Ref Manager
RIS
Zotero

Share
Share This Article
Fault Detection Algorithm for Gaussian Mixture Noises: An Application in Lidar/IMU Integrated Localization Systems

Penggao Yan

Penggao Yan

Zhengdao Li

Zhengdao Li

Feng Huang

Feng Huang

Weisong Wen,

Weisong Wen,

and Li-Ta Hsu

Li-Ta Hsu

Institute of Navigation

Mar 2025,

72

(1)

navi.684;

DOI: 10.33012/navi.684

Share This Article:
Post
Like 0
Bookmark this Article

SAVE ITEM IN SELECTED FOLDER.

User Name *

Password *

Remember my user name & password.

Jump to section

No citing articles found.

Google Scholar

Show more Original Article

PDF Help

Keywords

2D lidar/IMU-based localization, chi-squared test, EKF, fault detection, Gaussian mixture model, non-Gaussian noise

Fault Detection Algorithm for Gaussian Mixture Noises: An Application in Lidar/IMU Integrated Localization Systems

Abstract

1 INTRODUCTION

Gaussian noise modeling is not sufficient for developing fault detection methods

The Gaussian mixture model (GMM) is promising but remains under-explored in fault detection research

Contributions of this paper

2 FAULT DETECTION WITH GMM NOISE MODELING

2.1 Residual Analysis in the EKF

2.1.1 Relationship Between the Residual and Noises

2.1.2 Distribution of the Residual

2.2 Transformation Methods

2.2.1 Transformation Based on GMM Assumptions

2.2.2 Transformation Based on Gaussian Assumptions

2.3 Fault Detection Based on the Chi-Squared Test

3 APPLICATION IN AN IMU/LIDAR INTEGRATED LOCALIZATION SYSTEM

3.1 Motion Model

3.2 Measurement Model

4 NUMERICAL EXPERIMENTS

4.1 Construction of the Simulation Platform

4.2 Experimental Setup

4.3 Localization Performance

4.4 Fault Detection Performance

4.4.1 Step Failure Detection Analysis

4.4.2 Slope Failure Detection Analysis

5 CONCLUSION

HOW TO CITE THIS ARTICLE

APPENDIX A NOTATION

APPENDIX B DISTRIBUTION OF THE RESIDUAL

APPENDIX C SIMILARITY TO A STANDARD MVN DISTRIBUTION

(1) Transformation of 2D BGMM

(2) Transformation of Higher-Dimensional BGMM

APPENDIX D LIDAR PROCESSING

Appendix D.1 Least-Norm Solution for Lidar Measurement Models

Appendix D.2 Line Extraction and Feature Association

APPENDIX E LINEARIZATION OF AN EKF-BASED LIDAR/IMU INTEGRATED SYSTEM

Appendix E.1 Derivative of the Rotation Matrix

Appendix E.2 Linearization of the Rotation Part of the State Propagation Equation

Appendix E.3 Linearization of the Measurement Function

APPENDIX F ADDITIONAL RESULTS FOR FAULT DETECTION

REFERENCES

In this Issue

NAVIGATION: Journal of the Institute of Navigation

Alerts for this Article

Email This Article

Citation Tools

Fault Detection Algorithm for Gaussian Mixture Noises: An Application in Lidar/IMU Integrated Localization Systems

Citation Manager Formats

Share This Article

Fault Detection Algorithm for Gaussian Mixture Noises: An Application in Lidar/IMU Integrated Localization Systems

SAVE ITEM IN SELECTED FOLDER.

Jump to section

Related Articles

Cited By...

More in this TOC Section

Similar Articles

Keywords