A New Approach to Unwanted-Object Detection in GNSS/LiDAR-Based Navigation

Abstract

In this paper, we develop new methods to assess safety risks of an integrated GNSS/LiDAR navigation system for highly automated vehicle (HAV) applications. LiDAR navigation requires feature extraction (FE) and data association (DA). In prior work, we established an FE and DA risk prediction algorithm assuming that the set of extracted features matched the set of mapped landmarks. This paper addresses these limiting assumptions by incorporating a Kalman filter innovation-based test to detect unwanted object (UO). UO include unmapped, moving, and wrongly excluded landmarks. An integrity risk bound is derived to account for the risk of not detecting UO. Direct simulations and preliminary testing help quantify the impact on integrity and continuity of UO monitoring in an example GNSS/LiDAR implementation.

1. Introduction

This paper describes the design, analysis, and preliminary testing of a new method to quantify safety in GNSS/LiDAR navigation systems. An integrity risk bound is derived, which accounts for failures to detect undesirable, unmapped and wrongly extracted obstacles. The paper describes an innovation-based method, which is an alternative to the solution separation approach used in [1]. In addition, the paper provides the means to quantify the impact of unwanted objects (UO) on the risk of incorrect association. This work is intended for driverless cars, or highly automated vehicles (HAV) [2,3], operating in changing environments where unknown, moving obstacles (cars, buses, and trucks) are not wanted as landmarks for localization, and may occlude other useful, mapped landmarks.

This research leverages prior analytical work carried out in civilian aviation navigation where safety is assessed in terms of integrity and continuity [4]. These performance metrics are sensor- and platform-independent. Integrity is a measure of trust in sensor information: integrity risk is the probability of undetected sensor errors causing unacceptably large positioning uncertainty [4]. Continuity is a measure of the navigation system’s ability to operate without unscheduled interruption. Both loss of integrity and loss of continuity can place the HAV in hazardous situations [4,5].

Several methods have been established to predict integrity and continuity risks in GNSS-based aviation applications [6,7,8]. Unfortunately, the same methods do not directly apply to HAVs, because ground vehicles operate under sky-obstructed areas where GNSS signals can be altered or blocked by buildings and trees.

HAVs require sensors in addition to GNSS, including LiDARs, cameras, or radars. This paper focuses on LiDARs because of their prevalence in HAVs, of their market availability, and of our prior experience. A raw LiDAR scan is made of thousands of data points, each of which individually does not carry any useful navigation information. Raw measurements must be pre-processed before they can be used to estimate HAV positioning and orientation (or pose).

A first class of algorithms establishes correlations between successive scans to estimate sensor changes in ‘pose’ (i.e., position and orientation) [9,10,11,12]. These procedures, including the Iterative Closest Point (ICP) approach [13], can become cumbersome when evaluating safety of HAVs moving over time. A second class of algorithms provides sensor localization by tracking recognizable, static features in the perceived environment (seminal references and survey papers can be found in [14,15,16,17,18,19]). Features can include, for example, lines or planes corresponding to building walls in two- or three-dimensional scans, respectively. Previous knowledge of feature parameters can be provided either from a landmark map, or from past-time estimation in Simultaneous Localization and Mapping (SLAM) [15,20]. The resulting information can then be iteratively processed using sequential estimators in SLAM (e.g., Extended Kalman filter or EKF), which is convenient in practical implementations. To estimate the HAV’s pose starting from a raw LiDAR scan, two intermediary, pre-estimator procedures must be carried out: feature extraction (FE), and data association (DA).

First, FE aims at finding the few most consistently recognizable, viewpoint-invariant, and mutually distinguishable landmarks in the raw sensor data. Second, DA aims at assigning the extracted features to the corresponding feature parameters assumed in the estimation process, i.e., at finding the ordering of mapped landmarks that matches the ordering of extracted features over successive observations. Incorrect association is a well-known problem that can lead to large navigation errors [21], thereby representing a threat to navigation integrity. FE and DA can be challenging in the presence of sensor uncertainty. This is why many sophisticated algorithms have been devised [17,18,19,21,22,23]. But, how can we prove whether FE and DA are safe for life-critical HAV navigation applications?

This research question is mostly unexplored. Several publications on multi-target tracking describe relevant approaches to evaluate the probability of correct association in the presence of measurement uncertainty [24,25]. However, these algorithms are not well suited for safety-critical HAV applications due to their lack of prediction capability, to approximations that do not necessarily upper-bound risks, and to high computational loads. Also, the risk of FE is not addressed. Overall, research on integrity and continuity of FE and DA is sparse.

This paper builds upon prior work in [1,26,27,28], where we developed an analytical integrity risk prediction method using a multiple-hypothesis innovation-based DA process. We established a compact expression for the integrity risk of LiDAR-based pose estimation over successive iterations. However, references [26,27,28] made simplifying assumptions that limit the applicability of these prior results. For example, we assumed that the set of landmarks in the a-priori map was exactly the same as the one being extracted. This assumption was relaxed in [1] where we developed an integrity-risk-minimizing data-selection method. To achieve this, we derived a bound on the risk of incorrect association, with which a subset of measurements can be used while considering potential wrong associations with all landmarks surrounding the LiDAR. This bound was used in a preliminary approach to detect UO using solution separation tests. In practice, UO such as other vehicles passing by are likely to be extracted, and may even occlude other mapped landmarks. Obstacle detection methods have been developed to mitigate the impact of such UOs (example methods are described in [29,30]). But, the safety risks of using UOs as landmarks for navigation have yet to be fully quantified.

In response, in this paper, we derive new methods to quantify the integrity risk caused by failures to detect unwanted obstacles (UO), while guaranteeing a predefined false alert risk requirement.

Section 2 of the paper provides an overview of the risk evaluation methods developed in [1,26,27,28], and of their limitations. These methods use a nearest-neighbor DA criterion [9], defined by the minimum normalized norm of the EKF innovation vectors over all possible landmark permutations. Section 3 and Section 4 deal with the situation where a mapped landmark is not extracted, but another unknown obstacle is extracted instead (e.g., case of an obstacle masking a mapped landmark). This paper assumes that UOs only mask one unknown landmark at a time as the HAV drives by. Section 3 describes the innovation-based approach employed to detect the UO (which differs from the solution separation detector employed in [1]). An integrity risk bound is then derived to incorporate the risk of not detecting a UO when one might be present. This bound is analytically evaluated in two steps in Section 4: we account for the impact of undetected UO: (a) on the probability of hazardously misleading information (HMI) under the correct association (CA) hypothesis, and (b) on the probability of incorrect association (IA). Navigation integrity performance is then assessed in Section 5 using direct simulations and preliminary testing for an example implementation using GNSS and two-dimensional LiDAR data.

2. Background: Integrity Risk Bound Accounting for Incorrect Associations

This section presents an overview of the integrity risk evaluation method described in [1,26,28], which uses a multiple-hypothesis innovation-based DA process.

2.1. Integrity Risk Definition and Integrity Risk Bound

The integrity risk, or probability of hazardous misleading information (HMI) at time $k$, is noted $P(HM{I}_k)$, and is defined in Figure 1. The safety criterion is: $P(HM{I}_k)\le I_{REQ,k}$ where $I_{REQ,k}$ is a predefined integrity risk requirement set by a certification authority (similar to requirements set for aviation applications in [4,8]). Values for $I_{REQ,k}$ that might be used in future HAV applications can be found in [5].

In [26,28], we established an analytical bound on the integrity risk, which accounts for the risk of incorrect associations. This bound is expressed as:

$$P(HM{I}_k)\le 1-\left[1-P(HM{I}_k|C{A}_K)\right]P(C{A}_K)+I_{FE,k}$$

(1)

with

$$P(HM{I}_k|C{A}_K)=2Q\left\{\ell /{\sigma }_k\right\}$$

(2)

$$P(C{A}_K)\ge {\displaystyle \prod _{l=1}^k{P}_{{\chi }^2}\left\{n_l+m_l,\frac{L_l^2{\lambda }_l^2}{4}\right\}}$$

(3)

where

$k$	is an index identifying a time step;
$K$	designates a range of indices: $K\equiv \{0,\dots ,k\}$, from filter initiation to time $k$;
$C{A}_K$	is the correct association hypothesis for all landmarks, at all times 0, ..., $k$;
$Q\left\{\right\}$	is the tail probability function of the standard normal distribution;
$\ell$	is the specified alert limit that defines a hazardous situation [4,5,8] (e.g., see Figure 1);
${\sigma }_k$	is the standard deviation of the estimation error for the vehicle state of interest (or linear combination of states);
$P_{{\chi }^2}\{dof,T\}$	is the probability that a chi-squared-distributed random variable with “dof” degrees of freedom is lower than some value T;
$n_l$	is the number of measurements at time step $l$;
$m_l$	is the number of estimated state parameters at time step $l$;
$I_{FE,l}$	is an integrity risk budget allocation, i.e., a fraction of $I_{REQ,k}$ that we choose to satisfy: $I_{FE,k}<<I_{REQ,k}$;
$L_l^2$	is the minimum mean normalized separation between landmark features that can be guaranteed with probability larger than $1-I_{FE,l}$. The normalized feature separation metric is derived in [28]. $L_l^2$ is derived at FE using a map or database of landmarks or using landmark observations at previous time-steps in SLAM;
${\lambda }_l^2$	is a mapping coefficient from separation space to EKF innovation space. This coefficient is determined by solving an eigenvalue problem in [28]. The minimum eigenvalue is taken to lower bound $P(C{A}_K)$, which is conservative;
$L_l^2{\lambda }_l^2$	forms a probabilistic lower bound on the mean innovation’s norm, which is further described in the Section 2.2.

The integrity risk bound in Equation (1) is refined in this paper to account for the presence of UOs and for failures to detect them. Equation (1) captures a key tradeoff in data association: on the one hand, using only few measurements can cause a large nominal estimation error and hence large $P(HM{I}_k|C{A}_K)$; but on the other hand, few measurements from sparsely distributed landmarks can improve $P(C{A}_K)$ because features are “separated”, distinguishable, and therefore can be robustly associated. $P(HM{I}_k)$ is unknown, but we can assess safety by comparing $I_{REQ,k}$ to the upper bound given in Equations (1)–(3), where all terms are known.

2.2. Innovation-Based Data Association

Equation (1) is derived for an innovation-based DA process, which is further described in the following paragraphs. Let $n_L$ be the total number of visible landmarks and $n_F$ the number of estimated feature parameters per landmark. Feature parameters can include landmark position, size, orientation, surface properties, etc. When using LiDAR only (we integrate GNSS in Section 5), the total number of feature parameters within the visible landmark set is: $n_k\equiv n_L{n}_F$. We can stack the actual (true) values of the extracted feature parameters for all landmarks in an $n_k\times 1$ vector ${\mathbf{z}}_k$. Let ${\widehat{z}}_k$ be an estimate of ${\mathbf{z}}_k$. We assume that the cumulative distribution function of ${\widehat{z}}_k$ can be bounded by a Gaussian function with mean ${\mathbf{z}}_k$ and covariance matrix ${\mathbf{V}}_k$ [31,32,33]. We use the notation: ${\widehat{z}}_k~N({\mathbf{z}}_k,{\mathbf{V}}_k)$.

The nonlinear measurement equation can be written in terms of the $m_k\times 1$ state parameter vector ${\mathbf{x}}_k$ as

$${\widehat{\mathbf{z}}}_k={\mathbf{h}}_k({\mathbf{x}}_k)+{\mathbf{v}}_k$$

(4)

where

${\mathbf{x}}_k$	includes vehicle pose parameters and may also include landmark feature parameters (for SLAM-type approaches);
${\mathbf{v}}_k$	is the extracted measurement noise vector: ${\mathbf{v}}_k~N({\mathbf{0}}_{n\times 1},{\mathbf{V}}_k)$, where ${\mathbf{0}}_{a\times b}$ is an $a\times b$ matrix of zeros.

The mean of ${\widehat{\mathbf{z}}}_k$ is ${\mathbf{z}}_k={\mathbf{h}}_k({\mathbf{x}}_k)$. Equation (4) can be linearized about an estimate ${\overline{\mathbf{x}}}_k$ of ${\mathbf{x}}_k$:

$${\widehat{z}}_k\approx {\mathbf{h}}_k({\overline{x}}_k)+{\mathbf{H}}_k({\mathbf{x}}_k-{\overline{\mathbf{x}}}_k)+{\mathbf{v}}_k\mathrm{where}{\mathbf{H}}_k\equiv {\frac{\partial {\mathbf{h}}_k({\mathbf{x}}_k)}{\partial {\mathbf{x}}_k}|}_{{\overline{\mathbf{x}}}_k}$$

(5)

The ordering of landmarks in ${\widehat{z}}_k$ is arbitrary and unknown. A nearest-neighbor approach (described below) is used to determine the ordering of measurement-to-state coefficients in ${\mathbf{h}}_k({\overline{x}}_k)$ and ${\mathbf{H}}_k$. Failing to find the landmark ordering that matches that of ${\widehat{z}}_k$ causes estimation errors called incorrect associations (IA).

If $n_L$ landmarks are extracted, there are $(n_L!)$ ways to arrange measurements in ${\widehat{z}}_k$, which we call $(n_L!)$ candidate associations. For clarity of exposition, we assume that the total number of mapped landmarks, or of previously observed landmarks when using SLAM, is also the number $n_L$ of extracted landmarks (procedures to address this assumption are given in [1]). Let subscript $i$ designates association hypotheses, for $i=0,\dots ,n_A$, where $n_A=n_L!-1$. We define $i=0$ the fault-free, correct association (CA) hypothesis, and the other $n_A$ hypotheses are IA. IA impacts the EKF estimation process through the innovation vector ${\mathsf{\gamma }}_{i,k}$. Vector ${\mathsf{\gamma }}_{i,k}$ is an effective indicator of CA because it is zero mean only for the correct association.

In all IA cases, the mean of ${\mathsf{\gamma }}_{i,k}$ is not zero and is expressed in terms of $n\times n$ permutation matrices $A_{i,k}$, for $i=1,\dots ,n_A$, as

$$\begin{array}{cc}\hfill {\mathsf{\gamma }}_{i,k}& ={\widehat{z}}_k-A_{i,k}{\mathbf{h}}_k({\overline{x}}_k)\hfill \\ & =y_{i,k}+v_k-A_{i,k}{H}_k{\overline{\mathsf{\epsilon }}}_k\hfill \end{array}$$

(6)

where

$$y_{i,k}\equiv {\mathbf{h}}_k({\mathbf{x}}_k)-A_{i,k}{\mathbf{h}}_k({\mathbf{x}}_k)=(I_n-A_{i,k})z_k\mathrm{and}{y}_{0,k}=0$$

(7)

where ${\overline{\mathsf{\epsilon }}}_k$ is the EKF state prediction error vector (${\overline{\mathsf{\epsilon }}}_k\equiv {\overline{x}}_k-{\mathbf{x}}_k$) and $I_a$ is the $a\times a$ identity matrix.

Let ${\overline{P}}_k$ be the EKF state prediction error covariance matrix. We select the association candidate that satisfies the nearest-neighbor association criterion [9], defined as

$$\underset{i=0,\dots ,n_A}{\min }{\gamma }_{i,k}^2$$

(8)

where

$${\gamma }_{i,k}^2\equiv {\mathsf{\gamma }}_{i,k}^T{Y}_{i,l}^{-1}{\mathsf{\gamma }}_{i,k}\mathrm{and}{Y}_{i,k}=A_{i,k}{\mathbf{H}}_k{\overline{P}}_k{H}_k^T{A}_{i,k}^T+V_k\mathrm{for}i=0,\dots ,n_A$$

(9)

The probability of correct association is the probability of the following event occurring: ${\displaystyle \underset{i=1}{\overset{n_A}{\cap }}\left\{{\gamma }_{0,k}^2\le {\gamma }_{i,k}^2\right\}}$. We can determine the a priori distributions of variables ${\gamma }_{i,k}^2$, for $i=0,\dots ,n_A$, except their mean values that are unknown. In [28], we show that the term $L_l^2{\lambda }_l^2$ used in Equation (1) is a lower bound on the mean innovation’s norm $y_{i,l}^2$ ($y_{i,l}^2\equiv y_{i,l}^T{Y}_{i,l}^{-1}{y}_{i,l}$). Equation (1) is a bound on $P(HM{I}_k)$, but it assumes that no UO is present. We first design a UO detector and derive a new $P(HM{I}_k)$ bound in Section 3, and then we establish an analytical method to evaluate the impact of undetected UOs on this new bound in Section 4.

3. Risks Involved with Unwanted Object Detection

In the presence of a UO, the innovation vector’s norm in Equation (9) is nonzero under all association hypotheses. In this case, the correct association hypothesis must be redefined. We call correct association (CA) the one where all landmarks that are not occluded by a UO are correctly associated, i.e., where the innovation vector would be zero mean if the UO was removed. The nonzero mean in the CA’s innovation vector is caused by the UO only, not by other incorrectly associated landmarks.

3.1. Innovation-Based Detector

If a UO is present, ${\gamma }_{i,k}$ does not have a mean of zero even under CA. To identify such events, we can set a threshold $T_k^2$ on the minimum innovation norm squared, or, since the process is performed over time, on the running sum of minimum innovation norms squared. Using innovations (instead of solution separations as in [1]) will facilitate evaluation of $P(C{A}_K)$ in Section 4. The UO detection test statistic is defined as

$$q_k^2={\displaystyle \sum _{l=0}^k\underset{i=0,\dots ,n_A}{\min }{\gamma }_{i,l}^2}$$

(10)

Since the innovation sequence is white, $q_k^2$ is non-centrally chi-squared distributed with $n_{DOF,k}={\displaystyle \sum _{l=0}^k{n}_l}$ degrees of freedom and noncentrality parameter (NCP) ${\mu }_{Q,k}^2$. We use the notation $q_k^2~{\chi }^2(n_{DOF,k},{\mu }_{Q,k}^2)$. ${\mu }_{Q,k}^2$, which is further discussed in Section 4. The detection threshold $T_k^2$ is set according to a continuity risk requirement $C_{REQ}$ to limit the risk of false alerts. False alerts occur when no UO is present, causing $q_k^2$’s NCP to be zero under CA. Thus, $T_k^2$ is given by

$${\displaystyle \underset{0}{\overset{T_k^2}{\int }}{\chi }_{\tau }^2\left(n_{DOF,k},0\right)d\tau }=1-C_{REQ}\mathrm{or}\mathrm{equivalently},T_k^2=P_{{\chi }^2}^{-1}\left\{n_{DOF,k},1-C_{REQ}\right\}$$

(11)

where $P_{{\chi }^2}^{-1}\left\{\right\}$ is the inverse cumulative distribution function (CDF) of the chi-squared distribution ${\chi }_{\tau }^2(n_{DOF,k},0)$ evaluated at the $1-C_{REQ}$ quantile.

If $T_k^2$ is exceeded, we interrupt the mission. (As an alternative to mission interruption, we could select a different set of landmark feature measurements as in [1,34], but this is beyond the scope of this paper.) This does not impact $P(HM{I}_k)$. However, if $T_k^2$ is not exceeded, a UO may still be present because the detection test statistic $q_k^2$ is a random, noisy variable. Navigation errors due to undetected UOs can cause the vehicle to crash.

3.2. Integrity Risk in Presence of UO

To quantify the integrity risk caused by potentially undetected UOs, the $P(HM{I}_k)$ definition in Equation (1) is modified: HMI is the joint event of the car being out of lane while no alert has been sent. The integrity risk is redefined as

$$P(HM{I}_k)=P\left(|{\widehat{\epsilon }}_k|>\ell \cap \left[{\displaystyle \underset{l=0}{\overset{k}{\cap }}{q}_l^2\le T_l^2}\right]\right)$$

(12)

where ${\widehat{\epsilon }}_k$ is the EKF state estimation error for the state of interest, e.g., for the vehicle’s lateral deviation within its lane. Because ${\widehat{\epsilon }}_k$ and $q_k^2$ are obtained after associating LiDAR data to a landmark map, we consider a set of mutually exclusive, exhaustive hypotheses of correct associations (CA) and incorrect associations (IA). We derived the following bounds:

$$\begin{array}{l}P(HM{I}_k)\le P(H{I}_k\cap N{D}_K\cap C{A}_K)+P(H{I}_k\cap N{D}_K\cap I{A}_K)+I_{FE,k}\\ \le P(H{I}_k\cap N{D}_k|C{A}_K)+P(N{D}_K\cap I{A}_K)+I_{FE,k}\end{array}$$

(13)

where

$H{I}_k$	is the event of hazardous information (HI) at time $k$, defined as $H{I}_k\equiv |{\widehat{\epsilon }}_k|>\ell$;
$N{D}_K$	is the event of no detection (ND) at all previous times 0, ..., $k$, defined as $N{D}_K\equiv {\displaystyle \underset{l=0}{\overset{k}{\cap }}{q}_l^2\le T_l^2}$;
$N{D}_K$	is the event of ND at time $k$, defined as $N{D}_K\equiv q_k^2\le T_k^2$;
$C{A}_K$	is the CA hypothesis for all landmarks, at all times 0, ..., $k$;
$I{A}_K$	is the IA hypothesis for any landmarks, at any time 0, ..., $k$.

In Section 4, we derive upper bounds on $P(H{I}_k\cap N{D}_K|C{A}_K)$ and $P(N{D}_K\cap I{A}_K)$.

4. Analytical Bounds on Risks Caused by Undetected Unwanted Objects

As stated in Section 1, this paper assumes that UOs only mask one unknown landmark at a time as the HAV drives by. This can be extended to multiple UOs masking one subset of landmarks at a time, using the procedures described in [1]. However, the performance analysis in Section 5 does not illustrate this case. The limitation is that the UO-free subset must be large enough to enable HAV pose estimation; the method requires landmark redundancy because it assumes an uncertain vehicle dynamic model and no inertial navigation system.

4.1. Risk of HMI Due to Undetected UO

We consider a set of mutually exclusive, exhaustive hypotheses $H_h$ of a UO masking a landmark $h$ (or landmark subset $h$) for $h=0,\dots ,n_H$, where $n_H$ is the total number of hypotheses. We note $H_0$ the fault-free (no UO) hypothesis. Using the law of total probability, $P(H{I}_k\cap N{D}_K|C{A}_K)$ is rewritten as

$$P(H{I}_k\cap N{D}_k|C{A}_K)={\displaystyle \sum _{h=0}^{n_H}P\left(H{I}_k\cap N{D}_k\cap H_h|C{A}_K\right)}$$

(14)

We have no prior knowledge on the probability of occurrence of $H_h$, but we can bound the sum of their occurrence probabilities by 1. Thus, $P(H{I}_k\cap N{D}_K|C{A}_K)$ can be upper-bounded using the following expression:

$$P(H{I}_k\cap N{D}_k|C{A}_K)\le \underset{h=0,\dots ,n_H}{\max }P\left(|{\widehat{\epsilon }}_k|>\ell \cap q_k^2\le T_k^2|H_h\cap C{A}_K\right)$$

(15)

Recalling that ${\widehat{\epsilon }}_k$ and $q_k^2$ are statistically independent (e.g., [35,36]), we can rewrite the bound in Equation (15) as

$$P(H{I}_k\cap N{D}_k|C{A}_K)\le \underset{h=0,\dots ,n_H}{\max }P(|{\widehat{\epsilon }}_k|>\ell |H_h\cap C{A}_K)P(q_k^2\le T_k^2|H_h\cap C{A}_K)$$

(16)

Under the correct association hypothesis ($C{A}_K$), the distributions of ${\widehat{\epsilon }}_k$ and $q_k^2$ are known except for mean values ${\widehat{\epsilon }}_k~N({\mu }_k,{\sigma }_k^2)$ and $q_k^2~{\chi }^2(n_{DOF,k},{\mu }_{Q,k}^2)$. Thus, Equation (16) can be upper-bounded using receiver autonomous integrity monitoring (RAIM) methods [6,7,34,35,36,37]. A UO causes a shift in the mean of ${\widehat{\epsilon }}_k$ and in the NCP of $q_k^2$. Large UO-induced feature measurement errors cause large ${\widehat{\epsilon }}_k$ (i.e., high risk of HI) but also cause large $q_k^2$, which makes the UO easier to detect (i.e., low risk of ND).

To analyze this tradeoff, innovation-based chi-squared RAIM methods consider the failure mode slope (FMS) [34,35,36,37]. Given a UO hypothesis $H_h$ for $h\ne 0$, the FMS is the ratio of the mean estimation error over the NCP of the test statistic $g_{h,k}\equiv {({\mu }_k^2/{\mu }_{Q,k}^2)}^{1/2}$. Recent analytical results in [35] were established in the context of GNSS/INS integration. They provide the means to recursively determine the FMS when using an EKF for estimation and a sequence of innovations for detection. We use this method to determine the bound in Equation (16) for the risk-maximizing hypothesis $H_h$ for $h=0,\dots ,n_H$, i.e., for the worst-case FMS $g_{MAX,k}\equiv \underset{h=1,\dots ,n_H}{\max }{g}_{h,k}$:

$$P(H{I}_k\cap N{D}_k|C{A}_K)\le \underset{\eta }{\max }\left[\left(Q\left\{\frac{\ell +\eta g_{MAX,k}}{{\sigma }_k}\right\}+Q\left\{\frac{\ell -\eta g_{MAX,k}}{{\sigma }_k}\right\}\right)P_{NC{\chi }^2}\left\{n_{DOF,k},{\eta }^2,T_k^2\right\}\right]$$

(17)

where $\eta$ is a search parameter (called the fault magnitude in [36]) that is easily determined at each time step $k$ using a one-dimensional search, e.g., using an interval-halving method [36], and where

$$P_{NC{\chi }^2}\left(n_{DOF,k},{\eta }^2,T_k^2\right)\equiv {\displaystyle {\int }_0^{T_k^2}{\chi }_{\tau }^2(n_{DOF,k},{\eta }^2)}d\tau$$

(18)

4.2. Risk of Incorrect Association Due to Undetected UO

This subsection aims at evaluating the other unknown term in Equation (13): $P(N{D}_K\cap I{A}_K)$. The presence of a UO can cause the risk of IA to grow without bound. In this case again, the detector is leveraged to limit the impact of UO on safety risks. However, in contrast with Section 4.1, two major challenges must be tackled to upper-bound $P(N{D}_K\cap I{A}_K)$:

(i)

the events $I{A}_K$ and $N{D}_K$ are correlated because both events depend on the same innovation vectors; and

(ii)

unlike on the left-hand side in Equation (17), there is no condition on association (no “given $C{A}_K$”), so we do not know which association is used to compute the innovations in the detection test statistic $q_k^2$.

In response, we used an approach based on the minimum detectable error (MDE) concept used in the GPS Local Area Augmentation System (LAAS) [4,38,39]. The MDE is a probabilistic bound on the NCP of the chi-squared detection test statistic. The Appendix A shows that

$$P(N{D}_k\cap I{A}_K)\le {\displaystyle \sum _{l=0}^k\left(P_{NC{\chi }^2}\left\{n_l+m_l,{\mu }_{MDE,l}^2,\frac{L_l^2{\lambda }_l^2}{4}\right\}+I_{MDE,l}\right)}$$

(19)

where ${\mu }_{MDE,l}^2$ is the MDE due to a UO at time $l$. ${\mu }_{MDE,l}^2$ can be computed using the following equation:

$${\displaystyle {\int }_0^{T_l^2}{\chi }_{\tau }^2(n_l,{\mu }_{MDE,l}^2)}d\tau =I_{MDE,l}$$

(20)

The probability $I_{MDE,l}$ is an integrity risk requirement allocation, i.e., a fraction of $I_{REQ,l}$ such that $I_{MDE,l}<<I_{REQ,l}$. ${\mu }_{MDE,l}^2$ is the smallest value that the detection test statistic NCP can take to ensure that the risk of no detection stays below $I_{MDE,l}$. ${\mu }_{MDE,l}^2$ is a probabilistic bound, not a random variable (which addresses challenge (i) above), and is independent of the association candidate (Equation (20) only depends on the number of degrees of freedom, thus addressing (ii)).

4.3. Summary of the New Integrity Risk Bound, Accounting for Presence of UO

In the presence of UOs due to wrong landmark feature extraction, the probability of hazardous misleading information (HMI) at time $k$ can be bounded by the following expression:

$$P(HM{I}_k)\le P(H{I}_k\cap N{D}_k|C{A}_K)+P(N{D}_k\cap I{A}_K)+I_{FE,k}$$

(21)

with

$$P(H{I}_k\cap N{D}_k|C{A}_K)\le \underset{\eta }{\max }\left[\left(Q\left\{\frac{\ell +\eta g_{MAX,k}}{{\sigma }_k}\right\}+Q\left\{\frac{\ell -\eta g_{MAX,k}}{{\sigma }_k}\right\}\right)P_{NC{\chi }^2}\left\{{\displaystyle \sum _{l=0}^k{n}_l},{\eta }^2,T_k^2\right\}\right]$$

(22)

$$P(N{D}_k\cap I{A}_K)\le {\displaystyle \sum _{l=0}^k\left(P_{NC{\chi }^2}\left\{n_l+m_l,{\mu }_{MDE,l}^2,\frac{L_l^2{\lambda }_l^2}{4}\right\}+I_{MDE,l}\right)}$$

(23)

where

${\mu }_{MDE,l}^2$	is derived from ${\displaystyle {\int }_0^{T_k^2}{\chi }_{\tau }^2(n_l,{\mu }_{MDE,l}^2)}d\tau =I_{MDE,l}$ and where, in addition to the variables defined under Equations (1)–(3), we used:
$\eta$	is a scalar search parameter (fault magnitude) that is varied to maximize the integrity risk at each time $k$;
$g_{MAX,k}$	is the worst-case failure mode slope (FMS) over all UO hypotheses, determined using the method given in [35];
$P_{NC{\chi }^2}\{dof,{\mu }^2,T\}$	is the probability that a non-centrally chi-squared distributed random variable with “dof” degrees of freedom and noncentrality parameter ${\mu }^2$ is lower than some value T;
$T_k^2$.	is a detection threshold set in accordance to a continuity risk requirement $C_{REQ}$ in Equation (11);
$I_{MDE,l}$	is an integrity risk budget allocation, i.e., a fraction of $I_{REQ,k}$, chosen to satisfy $I_{MDE,k}<<I_{REQ,k}.$

5. Performance Analysis

In this section, example simulations and testing introduced in [26,27,28,40,41] are employed to compare the $P(HM{I}_k)$ bounds assuming no UOs in Equations (1)–(3) versus accounting for possible UOs in Equations (21)–(23).

5.1. Direct Simulation: Vehicle Roving through a GNSS-Denied Area

This analysis investigated the safety performance of a GPS/LiDAR navigation system onboard a vehicle roving through a forest-type environment. GPS signals were blocked by the tree canopy, and low-elevation satellite signals did not penetrate under the trees. Tree trunks served as landmarks for a two-dimensional LiDAR using a SLAM-type algorithm.

The measurement vector ${\widehat{z}}_k$ in Equation (4) was augmented with GPS code and carrier measurements. The state vector ${\mathbf{x}}_k$ was augmented to include an unknown GPS receiver clock bias and carrier phase cycle ambiguities. Time-correlated GPS signals and nonlinear LiDAR data were processed in a unified time-differencing EKF derived in [33,34]. The main simulation parameter values are listed in

Table 1. Simulation parameters.

System Parameters	Values
Standard deviation of raw LiDAR ranging measurement	0.02 m
Standard deviation of raw LiDAR angular measurement	0.5 deg
LiDAR range limit	20 m
GNSS and LiDAR data sampling interval	0.5 s
Standard deviation of raw GNSS code ranging signal	1 m
Standard deviation of raw GNSS carrier ranging signal	0.015 m
GNSS multipath correlation time constant	90 s
Vehicle speed	1 m/s
Alert limit ℓ	0.5 m
Integrity risk allocation for FE, IFE,k	10−9
Integrity risk allocation for MDE, IMDE,k	10−10
Continuity risk requirement, CREQ,k	10−3

, and a differential GPS measurement error model was used, which is fully described in [41]. In this scenario, GPS and LiDARs essentially relayed each other with seamless transitions from open sky through GPS-denied areas where landmarks were modeled as poles with nonzero radii.

As shown in Figure 2, Figure 3 and Figure 4 and 6, we consistently employed the following yellow-green-blue color code: the mission started with the vehicle operating in a GPS available area (yellow-shaded). Satellite signals available during initialization enabled accurate estimation of cycle ambiguities, so that vehicle positioning uncertainty did not exceed a few centimeters. Then, as the vehicle moved and crossed the GPS- and LiDAR-available area (green-shaded) and the LiDAR-only area (blue-shaded), seamless variations in covariance were achieved. A detailed description of this simulation is given in [41]. In this scenario, the likelihood of IA is high.

First, as shown in Figure 2, we assumed that no UO was present but IAs occurred. One indicator of IA is displayed on the top of the upper left-hand-side (LHS) plot in Figure 2. It shows that the actual cross-track positioning error (thick black line) versus distance travelled exceeded the corresponding one-sigma covariance envelope (thin black line). This suggests that errors impacting positioning are not captured by the covariance.

This is confirmed on the lower part of the upper LHS chart in Figure 2, where the black curve showing the $P(H{I}_k|C{A}_K)$ bound stayed below ${10}^{-7}$. This curve can directly be derived from the EKF covariance. It does not account for IA. In contrast, the red $P(H{I}_k)$-bound curve reached a first plateau of $I_{FE,k}$= 10⁻⁹ as soon as two landmarks were visible by design of our risk evaluation method [28]. The $P(H{I}_k)$ curve then suddenly increased to 10⁻⁵ at approximately 29 m of travel distance.

To explain this sudden jump, the top right-hand-side (RHS) chart in Figure 2 shows that, at the travel distance of 29 m (i.e., at travel time = 29 s) corresponding to the large increase in predicted integrity risk, landmark “1” was hidden behind landmark “4”. To the LiDAR, landmark “1” became visible again at the next time step, which made correct measurement association with either landmark “1” or “4” extremely challenging. The $P(H{I}_k)$ bound accounted for the risk caused by such events. This is consistent with other results presented in [1,26,27,28].

The bottom LHS chart in Figure 2 shows the simulated GPS satellite geometry on an azimuth elevation plot of the sky. At travel time 29 s, the tree canopy blocked all satellite signals. The bottom RHS chart displays the simulated LiDAR measurements showing again that landmark “1” was not visible from the LiDAR’s viewpoint.

In Figure 3, the risk of having a UO occluding a landmark is taken into account. Our new integrity risk evaluation method was implemented. We could quantify the impact on P(HMI_k) of undetected UOs assuming systematic CA by measuring the difference between the dashed black line $P(H{I}_k|C{A}_K)$ derived using [28] and the solid black line $P(HM{I}_k|C{A}_K)$. We noticed again that $P(H{I}_k|C{A}_K)$ (directly derived from the EKF covariance) was a poor safety metric because it stayed below ${10}^{-7}$, whereas $P(HM{I}_k|C{A}_K)$, accounting for UOs, exceeded ${10}^{-2}$. In parallel, the red curves account for the risk of incorrect association (IA). The difference between the dashed red line and the solid red line, which respectively reached ${10}^{-5}$ and above ${10}^{-2}$, shows the impact on P(HMI_k) of undetected UOs.

To better understand the shape of the overall $P(HM{I}_k)$ bound, Figure 4 shows the contributions of each single-UO hypothesis (assuming no UO, assuming a UO masking landmark “1”, assuming a UO masking landmark “2”, etc.). In Figure 4, the color code used in the LHS graph is also employed in the RHS plot to represent the landmark involved in the corresponding fault hypothesis. Peaks in $P(HM{I}_k)$-bound contributions occurred when the landmark geometry and redundancy was too poor to ensure reliable detection of a given UO. The overall $P(HM{I}_k)$ bound was the maximum of all the contributions at each time step and is represented with a thick green line.

5.2. Preliminary Testing in an Incorrect-Association-Free Environment

Preliminary experimental testing was carried out using data collected in a structured environment shown in Figure 5. Static simple-shaped landmarks were located at locations sparse enough to ensure successful outcomes for FE and DA. Because the results presented here were free of incorrect associations, $P(HM{I}_k)$ was expected to match $P(HM{I}_k|C{A}_K)$. This test data was used to focus on the risk of UO misdetection.

Measurements from carrier phase differential GPS (CPDGPS) as well as LiDAR scanners were synchronized and recorded. In order to obtain a full 360-degree LiDAR scan, two 180-degree LiDAR scanners were assembled back-to-back. The LiDAR scanners had a specified 15–80-m range limit, a 0.5-degree angular resolution, a 5-Hz update rate, and a ranging accuracy of 1–5 cm (1 sigma) [42]. The GPS antenna was mounted on top of the front LiDAR. The lever-arm distance between the two LiDARs was accounted for. The two LiDARs and the GPS antenna were mounted on a rover also carrying the GPS receiver and data-link. An embedded computer onboard the vehicle recorded all measurements including the raw GPS data from the reference station transmitted via a wireless spread-spectrum data-link. Truth trajectory was obtained using a fixed CPDGPS solution.

The upper LHS chart in Figure 6 confirms that this is an incorrect-association-free scenario because the actual error (thick line) fits within the covariance envelope (thin line) throughout the test. In addition, the lower LHS graph in Figure 6 shows $P(HM{I}_k)$-bound contributions for each single-UO hypothesis. The six $P(HM{I}_k)$ bounds corresponding to UO hypotheses are shown using the same color code as in Figure 4, and the UO-free hypothesis is the dashed line. The color code is used on the RHS chart, which also shows the landmark geometry. In the LHS graph, $P(HM{I}_k)$ increases substantially when accounting for undetected UO (thick black curve), as compared to ignoring their potential presence (dashed red line). UO occluding landmarks “1” and “2” cause by far the largest increase in $P(HM{I}_k)$ bound. In this SLAM-type implementation where the map is built incrementally, landmarks observed early in the rover trajectory play a key role throughout the mission, which explains the method’s sensitivity to potential extraction faults on landmarks “1” and “2”. In future work, we will try to reduce the $P(HM{I}_k)$ bound using redundant information from other sensors, from additional landmarks, and from additional landmark features.

6. Conclusions

This paper presents a new approach to improve the safety of LiDAR-based navigation by quantifying the risks of missed detection of unwanted objects (UO). UOs can occlude useful landmarks, thereby causing large navigation errors. We established a bound on the integrity risk caused by UOs. First, we presented an innovation-based detector, and we established an analytical expression for the impact of undetected UO on the positioning error assuming correct association. Then, we derived a bound on the risk of incorrect association (IA) in the presence of UO. Direct simulation and preliminary testing in a structured environment demonstrated the proposed method’s ability to quantify safety risks in the presence of both UOs and IAs. It showed, for example, that the Kalman filter covariance is a poor metric of safety performance. The analysis of our preliminary experimental results suggests that additional redundant information from other sensors would be needed to safely detect UOs in the LiDAR’s surroundings.