Data Association Using Innovation Projections for Landmark-Based Localization

2.1 Integrity Risk Bound for Localization Using Data Association

In safety-critical transportation applications, a vehicle’s navigation integrity risk, which reflects the probability of hazardously misleading information (HMI), can be defined at time epoch k as:

$P\left({\mathrm{HMI}}_k\right)\equiv P\left(\left({\epsilon }_k\right)>l\right)$1

where ε_k is the estimation error on one of the vehicle’s scalar position coordinates, and l is a predefined alert limit based on system safety requirements (International Civil Aviation Organization, 2006; Radio Technical Commission for Aeronautics (RTCA) Special Committee 159, 2020). Each position coordinate can be assessed individually to define a three-dimensional alert limit volume (Joerger et al., 2020; Reid et al., 2019; Working Group C, 2022).

In landmark-based localization, which requires assigning sensed features to mapped landmarks, the probabilities of correct and wrong associations (CA and WA, respectively) directly influence P(HMI_k). In prior work, we considered two mutually exclusive, exhaustive hypothesis sets: (1) CA at all times since EKF initialization (CA_K, where K ≡ 1, …, k), and (2) WA at any time over the filtering period (WA_K). Using these two hypothesis sets, we established an analytical upper bound on the integrity risk (Joerger & Pervan, 2019). An upper bound ensures that the risk prediction is conservative and does not underestimate the probability of HMI, which would be unsafe. This risk bound is expressed as:

$P\left({\mathrm{HMI}}_k\right)=P\left({\mathrm{HMI}}_k\cap {\mathrm{CA}}_K\right)+P\left({\mathrm{HMI}}_k\cap {\mathrm{WA}}_K\right)\le 1-\left(1-P\left(\left({\epsilon }_k\right)>\left(l\right){\mathrm{CA}}_K\right)\right)P\left({\mathrm{CA}}_K\right)$2

The term P (|ε_k| > l|CA_k) reflects the assumption that CA are maintained at all time steps. This probability can be directly derived from the EKF covariance matrix; although it can be used as a performance measure in robotic applications, it is insufficient for safety-critical applications, where more rigorous integrity guarantees are required.

In addition, we do not assume that ε_k is normally distributed, though we assert that it can be conservatively overbounded by a Gaussian function (Blanch et al., 2019; DeCleene, 2000; Rife et al., 2006). If σ_k denotes the standard deviation of this Gaussian overbound on ε_k, which is obtained from the EKF covariance matrix using overbounding models for measurement errors, we can write the following equation:

$P\left({\mathrm{HMI}}_k\left({\mathrm{CA}}_K\right)\right)\equiv P\left(\left({\epsilon }_k\right)>\left(l\right){\mathrm{CA}}_K\right)=2Q\left(l/{\sigma }_k\right)$3

where Q{ } is the tail probability function of the standard normal distribution.

Figure 1 shows an example automotive lane-centering application in which safety requirements are defined using an alert limit l to compute P(HMI_k) (Capua et al., 2024; Joerger et al., 2021; U.S. Department of Transportation (DOT) Federal Highway Administration (FHWA), 2017; U.S. Department of Transportation (DOT) Nation Highway Traffic Safety Administration (NHTSA), 2017). Here, the lateral positioning error ε_k is of primary concern. In this case, l is the lateral distance between the edge of the vehicle and the edge of the driving lane (Reid et al., 2019).

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 1

Integrity risk definition for automotive applications. The integrity risk is the probability of the car being outside the alert limit requirement box (blue shaded area) when it is estimated to be inside the box.

This paper focuses on the term P(CA_K) in Equation 2, which does not always equal one. Using the conditional probability formula, P(CA_K) can be expressed as:

$P\left({\mathrm{CA}}_K\right)=P\left({\mathrm{CA}}_1\cap {\mathrm{CA}}_2\cap \dots \cap {\mathrm{CA}}_k\right)=\prod _{l=1}^{k}P\left({\mathrm{CA}}_l\left({\mathrm{CA}}_{L-1}\right)\right)$4

where CA₀ is the empty event, and CA_L-1 ≡ CA₁ ∩ … ∩ CA_l-1. The goal of this paper is to derive a tight lower bound on the conditional probabilities within the product. (For CA, a lower bound is the conservative and therefore safe quantity to consider for integrity risk assessment). To simplify notation, we define P(CA) ≡ P(CA_l | CA_L-1).

2.2 Measurements, Innovations, and Normalized Innovation Squared (NIS)-Based Association

To predict P(CA), we use a model of the measurement process. Let n_L be the number of landmarks in view. For each landmark, we extract n_F measurable features, such as the landmark’s position coordinates relative to the sensor. All n measurements (where n = n_Ln_F) are then arranged into a measurement vector z:

$A_{C}z=h\left(x\right)+v$5

where h( ) is a nonlinear function of the state vector x. In Section 5, which presents the LiDAR/IMU implementation, the state vector x comprises the vehicle’s three-dimensional position, velocity, and orientation in a local East-North-Up navigation frame, as well as gyroscope and accelerometer biases. In Section 4, x includes a subset of these variables. The vector v represents the measurement noise, which is modeled using an overbounding zero-mean Gaussian distribution with covariance matrix V such that v ~ N(0, V). The matrix A_C is an unknown permutation matrix representing the uncertainty in the ordering of landmarks in z; specifically, we do not know whether the order of landmarks in z matches the order assumed in the EKF model h(x).

Nearest neighbor methods, such as those described by Bar-Shalom et al. (1990), provide heuristic estimates of the correct ordering but do not yet provide a means to directly quantify the probability of CA. These methods consider multiple possible landmark orderings. While there are (n_L!) possible orderings of the n_L landmarks, not all of these orderings are likely to occur. However, for clarity of exposition, we consider all (n_L!) orderings in this paper and leave complexity reduction for future work.

We express the (n_L!) different EKF innovation vectors, each corresponding to a different landmark ordering, as:

$\begin{array}{ccccc}{\gamma }_l=A_l{A}_{C}z-h\left(\overline{x}\right)& \text{for}& l=0,\dots ,h& \text{where}& h\equiv n_L!-1\end{array}$6

where $\overline{x}$ is the a priori prediction of x, A_C is the unknown (true) association matrix, and A_l represents the lth candidate permutation matrix from the full set of (n_L!) possibilities. We consider all (n_L!) permutation matrices A_l (i.e., for l = 0, …, h) to find the matrix that achieves A_lA_C = I, indicating the correct association. Because we consider a complete set of permutations, we can rewrite Equation (6) as:

$\begin{array}{ccc}{\gamma }_i=A_{i}z-h\left(\overline{x}\right)& \text{for}& i=0,\dots ,h\end{array}$7

For i = 0, …, h, the sample innovation vector γ_i therefore represents the difference between the permuted version of the sample measurement vector z and the EKF’s predicted measurement vector $h\left(\overline{x}\right).$

Importantly, vector γ_i is zero-mean if and only if i corresponds to the correct association. For this reason, a sensible criterion for identifying the correct association is to minimize the NIS (i.e., the norm squared of γ_i weighted by the inverse of its covariance matrix Y_i) over all values of i. The NIS metric can be calculated as:

$\begin{array}{ccc}{\gamma }_i^2\equiv {\gamma }_i^T{Y}_i^{-1}{\gamma }_i& \text{for}& i=0,\dots ,h\end{array}$8

The NIS-based data association criterion then determines the most likely association index i_* using the following equation:

$i_{\ast }=\arg \underset{i=0,\dots ,h}{\min }{\gamma }_i^2$9

While this NIS-based method is effective at identifying the most likely association, it has limitations in safety-critical applications.

2.3 Probability of Correct Association Using Normalized Innovation Squared (NIS)

In a previous paper, we derived an analytical lower bound on P(CA) for the NIS-based data association method (Joerger & Pervan, 2019). To obtain this lower bound, we introduced a known quantity that captures the predicted effect of a wrong association on the innovation vector. These predicted innovation vectors are expressed as:

$\begin{array}{ccc}{\overline{y}}_j\equiv \left(A_j-I\right)h\left(\overline{x}\right)& \text{for}& j=0,\dots ,h\end{array}$10

where I is the n × n identity matrix. Throughout the paper, we distinguish two association candidate indices: j is used when the CA index is known (i.e., for evaluating predicted innovations), whereas i is used when it is unknown (e.g., for sample innovations in Equations (6) to (9)). Following this convention, since vectors ${\overline{y}}_j$ are known, we can use the notation A₀ = I and ${\overline{y}}_0=0.$ The analytical NIS-based P(CA)-bound can then be written as:

$P\left(\mathrm{CA}\right)=1-P\left(\underset{\begin{array}{c}i=0\\ i\ne C\end{array}}{\overset{h}{\cup }}\left({\gamma }_i^2<{\gamma }_C^2\right)\right)\ge X^2\left(n+m,\frac{1}{4}\underset{j=1,\dots ,h}{\min }{\overline{y}}_j^T{Y}_j^{-1}{\overline{y}}_j\right)$11

where

subscript C

is the unknown ordering index for the correct association,

$X^2\left(n_{\mathrm{DOF}},T^2\right)$

is the chi-squared cumulative distribution function, i.e., the probability that a chi-squared-distributed random variable with n_DOF degrees of freedom is lower than T²,

n

is the number of measurements, i.e., the dimension of z,

m

is the number of estimated state parameters, i.e., the dimension of x.

The analytical bound in Equation (11) depends on the expected separation between landmarks, which is mathematically represented by the predicted minimum norm squared of the half-difference between normalized innovation vectors. For CAs, the normalized innovation is $Y_0^{-1/2}{\overline{y}}_0=0,$ and for WAs, it is $Y_j^{-1/2}{\overline{y}}_j,$ for j = 1, …, h). To illustrate this concept, Section 4 provides geometric representations of the P(CA) bound using notional, low-dimensional examples. It shows that, in the n-dimensional normalized innovation space, the NIS data association criterion defines a hypersphere centered at the origin with a radius equal to the half-distance to the closest predicted WA vector.

Deriving the bound in Equation (11) is challenging because it involves pairwise comparisons of the quadratic quantities defined in Equation (8), each of which has an association-dependent weighting matrix $Y_i^{-1}$. In addition, these quantities are correlated, so bounding the probability that $\left({\gamma }_i^2<{\gamma }_c^2\right)$ requires separating the individual, independent contributions of measurement noise v and uncertainty in prior knowledge $\overline{x}$. This leads to a chi-squared distribution with (n + m) degrees of freedom even though γ_i is only n-dimensional. (A complete derivation of this result is given by Joerger and Pervan (2019), and elements of this derivation are reproduced in Appendix A). These complexities suggest that the inequality in Equation (11) is a loose bound. Section 3 improves upon this prior research by introducing a tighter, IP-based bounding technique.

3.1 Making Better Use of Innovation Prediction Vectors

Wrong associations are structured and predictable sources of navigation error. Their effects on the innovation vectors are captured with the innovation prediction vectors ${\overline{y}}_j$ for j = 0, …, h, as defined in Equation (10). The NIS-based bound on P(CA) in Equation (11) only leverages the minimum norm of ${\overline{y}}_j$ for j ≥ 1, and the data association criterion in Equation (9) does not incorporate ${\overline{y}}_j$ at all. In contrast, the IP method derived in Sections 3.2 and 3.3 uses the magnitudes and directions of all vectors ${\overline{y}}_j$ for j = 0, …, h. These vectors inform both the data association criterion and a tighter P(CA) bound.

3.2 Data Association Criterion Using Innovation Projections (IP)

Let β be a known, deterministic vector onto which each sample innovation vector γ_i is projected. Given a vector β, for which the derivation is presented in Section 4, the correct association index i_* is found using the following IP criterion:

$\begin{array}{ccccc}{i}_{\ast }=\arg \underset{i=0,\dots ,h}{\min }{p}_i& \text{where}& p_i\equiv {\beta }^T{\gamma }_i& \text{for}& i=0,\dots ,h\end{array}$12

The scalar projections p_i are normally distributed signed quantities that represent linear combinations of the elements of the jointly normally distributed random vector γ_i.

The choice of the projection vector β is critical because it directly affects the performance of the IP data association criterion. Vector β is designed to maximize P(CA), and the notional examples in Section 4 prove that this goal can be effectively achieved. Notably, β does not need to be a unit vector: because Equation (12) depends only on pairwise comparisons of p_i, any common, positive scaling of β does not affect the outcome. We define β as the mean of the predicted innovation vectors over all WA, given by:

$\beta \equiv \frac{1}{h}\sum _{j=1}^h{\overline{y}}_j$13

3.3 Probability of Correct Association Using Innovation Projections (IP)

In this section, we derive an analytical lower bound on P(CA) using the IP method. Because the projections p_i in Equation (12) are unweighted linear combinations of elements of γ_i, we can lower-bound P(CA) without the conservative approximations required for handling quadratic forms in the NIS method. This lower bound is given by:

$P\left(\mathrm{CA}\right)=1-P\left(\underset{\begin{array}{c}i=0\\ i\ne C\end{array}}{\overset{h}{\cup }}{p}_i<p_C\right)\ge 1-\sum _{\begin{array}{c}i=0\\ i\ne C\end{array}}^{h}P\left({\beta }^T\left({\gamma }_i-{\gamma }_C\right)<0\right)$14

To evaluate this bound analytically, we define the state prediction error vector $\overline{\epsilon }\left(\overline{\epsilon }\equiv x-\overline{x}\right)$ and its covariance matrix $\overline{P}$ such that $\overline{\epsilon }\sim N\left(0,\overline{P}\right)$. We use a first-order Taylor series approximation around $\overline{x}$ to approximate the EKF’s mean measurement model as:

${\begin{array}{ccc}h\left(x\right)\approx h\left(\overline{x}\right)+H\overline{\epsilon }& \text{with}& H\equiv \left(\frac{\partial h\left(x\right)}{\partial x}\right)\end{array}}_{\overline{x}}$15

Using this approximation, the innovation vector for the correct association, which is expected to be zero-mean, can then be written as:

${\gamma }_C\stackrel{\left(7\right)}{=}h\left(x\right)+v-h\left(\overline{x}\right)\stackrel{\left(15\right)}{=}H\overline{\epsilon }+v$16

To evaluate the bound on P(CA), we can rewrite the set of random variables β^T[γ_i – γ_C] in Equation (14) (for i = 0, …, h and i ≠ C) in terms of known WA indices j. By adding and removing $A_{j}h\left(\overline{x}\right)$ and using Equations (10) and (15), we can obtain:

$\begin{array}{ccc}{\beta }^T\left(\left(A_j\left(h\left(x\right)+v\right)-h\left(\overline{x}\right)\right)-\left(H\overline{\epsilon }+v\right)\right)& \stackrel{\left(10\right),\left(15\right)}{=}& {\beta }^T{\overline{y}}_j+{\beta }^T\left(A_j-I\right)\left(H\overline{\epsilon }+v\right)\\ & & \begin{array}{cc}\text{for}& J=1,\mathrm{\dots },h\end{array}\end{array}$17

In this equation, the term on the left (expressing β^T[γ_i – γ_C]) is shown to be equal to the term on the right, which is a biased linear combination of independent, normally distributed vectors v and $\overline{\epsilon }$. The distribution of the right-hand term is known, so the P(CA) bound in Equation (14) becomes:

$P\left(CA\right)\ge 1-\sum _{j=1}^{h}Q\left(-\frac{{\beta }^T{\overline{y}}_j}{{\sigma }_j}\right)$18

where

$\begin{array}{ccc}{\sigma }_j^2\equiv {\beta }^T\left(A_j-I\right)Y_0{\left(A_j-I\right)}^T\beta & \mathrm{with}& Y_0\equiv H\overline{P}{H}^T+V\end{array}$19

Equation (19) shows that positive magnitude scaling of vector β has no impact on the ratio $-{\beta }^T{\overline{y}}_j/{\sigma }_j$ and therefore does not affect the resulting bound. Section 4 justifies the choice of β in Equation (13) and explains why, as compared to NIS, the IP criterion both improves the actual P(CA) and tightens the predicted P(CA) bound.

4.1 A First NIS-vs-IP Comparison: Two-Landmark Example

Figure 2 presents a notional benchmark example selected to illustrate the data association process. A ranging sensor (e.g., LiDAR) is located at a scalar position state x in a one-dimensional reference frame R. (The dimension of the state space is inconsequential; we keep it scalar for simplicity). The sensor produces two noisy range measurements, z₁ and z₂, that must be correctly associated with their corresponding landmarks located at known scalar positions x_A and x_B, respectively.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 2

Benchmark example with a scalar state and two landmarks.

The measurement equation assuming CA can be expressed in the standard form z = h(x) + v as:

$\begin{array}{ccc}\left(\begin{array}{c}{z}_1\\ z_2\end{array}\right)=\left(\begin{array}{c}\left(x_A-x\right)\\ \left(x_B-x\right)\end{array}\right)+\left(\begin{array}{c}{v}_1\\ v_2\end{array}\right)& \text{with}& v\sim N\left(0,I{\sigma }_v^2\right)\end{array}$20

In this n_L = 2 example, the number of possible orderings is n_L! = 2, and the number of possible WA is h = 1. Assuming that $\overline{x}\sim N\left(x,{\sigma }_{\overline{x}}^2\right)$, the known, predicted innovation vectors for the CA and WA cases are respectively given by:

$\begin{array}{ccccc}{\overline{y}}_0& =& \left(A_0-I\right)h\left(\overline{x}\right)& =& \begin{array}{cc}0& \text{and}\end{array}\\ {\overline{y}}_1& =& \left(A_1-I\right)h\left(\overline{x}\right)& =& \left(\begin{array}{cc}-1& 1\\ 1& -1\end{array}\right)h\left(\overline{x}\right)=\left(\begin{array}{c}\left(x_B-\overline{x}\right)-\left(x_A-\overline{x}\right)\\ \left(x_A-\overline{x}\right)-\left(x_B-\overline{x}\right)\end{array}\right)\end{array}$21

Because the measurement and innovation vectors are two-dimensional, we can represent the innovation space as a plane. Figure 3 shows vectors ${\overline{y}}_0\hspace{0.17em}\text{and}\hspace{0.17em}{\overline{y}}_1$ as black disks in innovation space, with ${\overline{y}}_0$ located at the origin.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 3

Two-dimensional innovation space representation of the NIS and IP methods for the two-landmark example (a) Correct and wrong associations of a single-sample innovation vector (b) 10,000 sample innovation vectors

In Figure 3(a), the sample innovation vectors γ₀ and γ₁, corresponding to CA and WA, respectively, are represented as orange vectors. Using Equations (10), (15), and (16), we can show that ${\gamma }_0={\overline{y}}_0+H\epsilon +v\hspace{0.17em}\text{and}\hspace{0.17em}{\gamma }_1={\overline{y}}_1+A_1\left(H\epsilon +v\right)$. This result confirms that both γ₀ and γ₁ are affected by noise that pulls these vectors away from ${\overline{y}}_0$ and ${\overline{y}}_1$, respectively. The green construction lines in Figure 3(a) represent the conventional normalized innovation (NI) criterion. For visualization purposes, the norms are not squared. The difference between the NI norms for CA and WA, reflecting the separation measured by the NI criterion, is shown as a green double-ended arrow. However, the NI criterion does not use the prediction ${\overline{y}}_1$ of the effect of WA on innovation vectors. In contrast, the IP method, shown with blue construction lines, uses $\beta ={\overline{y}}_1$ as the direction on which sample innovation vectors are projected. This choice is consistent with Equation (13). The resulting projections are signed variables, and the difference between the CA and WA projections is shown with a blue double-ended arrow. As can be seen in the figure, this difference is significantly larger than for the NI method, highlighting the stronger potential for the IP criterion to find the CA.

Figure 3(b) shows a more systematic, larger-scale comparison of the IP and NIS criteria. Here, we simulated 10,000 samples of v and $\overline{x}$ and represent their 10,000 corresponding innovation vectors γ_C. In this figure, samples that were correctly associated using NIS are represented in cyan; samples that were correctly associated using IP are represented in cyan and blue; and samples that were incorrectly associated using IP are shown in red. As expected for the norm-based NIS method, cyan samples form a disk centered at the origin. For the IP method, the red samples form a half-plane with a boundary line orthogonal to $\beta ={\overline{y}}_1$ located halfway between from 0 and ${\overline{y}}_1$. Notably, we observe that the IP method achieves a much higher number of correctly associated samples (cyan and blue) than the NIS method (cyan only). The actual P(CA) is therefore larger for IP than for NIS. In Sections 4.4 and 5, we will see that the predicted P(CA) bound for IP in Equation (18) is also tighter than for NIS in Equation (11).

4.2 A Second NIS-vs-IP Comparison: Three-Landmark Example

Here, we present a three-dimensional innovation space representation to provide additional insights into the IP-based association process and reinforce the suitability of β in Equation (13) as an effective direction on which to project sample innovation vectors. Figure 4 builds upon the example in Figure 2 by including an additional landmark. In this case, the three noisy measurements z₁, z₂, and z₃ must be correctly associated with their corresponding landmarks at scalar positions x_A, x_B, and x_C, respectively. The measurement equation under CA for this example is the same as Equation (20), augmented with an additional row corresponding to the third landmark: z₃ = | x_C – x | + v₃. In this example, n_L = 3, the number of possible orderings is n_L! = 6, and the number of possible WA is h = 5.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 4

Benchmark example with a scalar state and three landmarks

Figure 5 shows the innovation prediction vectors ${\overline{y}}_0$ to ${\overline{y}}_5$ as black spheres in the three-dimensional innovation space, with ${\overline{y}}_0$ at the origin. This figure reveals two key properties of innovation predictions: vectors ${\overline{y}}_0$ to ${\overline{y}}_5$ (i) span a a plane in the three-dimensional innovation space and (ii) describe a convex polygon with a vertex at the origin $\left({\overline{y}}_0=0\right).$ Section 4.3 will show that these observations can be generalized to high-dimensional innovation spaces.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 5

Three-dimensional innovation space representation of NIS and IP methods for the three-landmark example (a) Projection of a single sample innovation vector (b) 10,000 sample innovation vectors

In Figure 5(a), the pink plane contains all six vectors ${\overline{y}}_0$ to ${\overline{y}}_5$ and is bounded by dashed maroon lines where it intersects with the two planes defined by γ₍₁₎ = 0 and γ₍₃₎ = 0, where γ₍₁₎, γ₍₂₎, and γ₍₃₎ denote the innovation vector coordinates in innovation space. The two insets in the upper-right corner show two different three-dimensional perspectives to highlight that ${\overline{y}}_0$ to ${\overline{y}}_5$ are indeed in a same plane. In the main panel, the dotted maroon line marks the intersection of the pink plane with γ₍₂₎ = 0. As an example, the sample innovation vector γ₅ is represented in green, and its orthogonal projection on the pink plane is shown in blue. Because the predictable effects of WA on the innovations are captured within the pink plane, the component of the sample innovation vector that is orthogonal to the pink plane (shown as a thick red line) arises from measurement noise and is therefore irrelevant for distinguishing CA from WA. This structure reinforces the value of IP over NIS.

When defined by Equation (13), the IP design vector β lies in this pink plane and points to the centroid of the polygon formed by the innovation vectors ${\overline{y}}_j$ for j = 0, …, 5. Vector β is shown in Figure 5(b), which represents 10,000 sample innovation vectors γ₀ as in the previous example. We use the same color code as before: samples that were correctly associated using NIS are shown in cyan, samples that were correctly associated using IP are shown in both cyan and blue; and samples that were wrongly associated using IP are shown in red. As expected, cyan samples describe a sphere centered at the origin, and red samples describe volumes delimited by planes normal to the ${\overline{y}}_j$-plane. As in the two-landmark example, the actual P(CA) is significantly larger for IP than for NIS.

In Figure 6, we use two different approaches to assess the optimality of β as defined in Equation (13). First, we sample directions for vector β at regular angular intervals in the ${\overline{y}}_j$-plane and select the direction that maximizes the P(CA) bound in Equation (18). Second, we implement an interior-point optimization algorithm to solve the following minimization problem:

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 6

Assessing the optimality of vector β as defined in Equation (13).

$\begin{array}{cccc}\hat{\beta }& =& \text{arg}& \underset{\beta }{\text{min}}\sum _{j=1}^{h}Q\left(-\frac{{\beta }^T{\overline{y}}_j}{{\left({\beta }^T\left(A_j-I\right)Y_0{\left(A_j-I\right)}^T\beta \right)}^{1/2}}\right)\end{array}$22

Both methods converge to the same solution, which corresponds to the centroid of the ${\overline{y}}_j$-polygon (i.e., the solution that matches Equation (13)). This result confirms that β is optimal in this example and supports its use in three-dimensional innovation space.

4.3 Generalization to Higher-Dimensional Innovation Space

This subsection leverages the fact WA are well-structured navigation error sources to generalize the findings of Sections 4.1 and 4.2 to n-dimensional innovation space for any value of n.

The first key observation is that the vectors ${\overline{y}}_j$, for j = 0, …, h, span a subspace of the innovation space: they spanned a line in the two-dimensional innovation space in Section 4.1 and a plane in the three-dimensional innovation space in Section 4.2. This property arises because the ${\overline{y}}_j$-generating matrices (A_j − I) in Equation (10), for j = 0, …, h, are rank-deficient with rank no greater than n − 1. This rank deficiency is proved in Appendix B.

The second key observation follows from rewriting Equation (10) as:

$\begin{array}{ccc}{\overline{y}}_j+h\left(\overline{x}\right)=A_{j}h\left(\overline{x}\right)& \text{for}& j=0,\mathrm{\dots },h\end{array}$23

Here, the permutation matrices A_j for j = 0, …, h are doubly stochastic matrices that define Birkhoff polytopes (Marshall et al., 2011). These polytopes have well-known properties that we leverage in Appendix C to show that, in innovation space with n ≥ 3 dimensions, the vectors ${\overline{y}}_j$ for j = 0, …, h form the vertices of a convex polytope (e.g., a convex polygon in three-dimensional innovation space), with the ${\overline{y}}_0$-vertex located at the origin.

Together, these two observations provide a rationale for projecting sample innovations onto a direction β that extends from the origin to the centroid of the convex polytope formed by ${\overline{y}}_j$, for j = 0, …, h. This choice of β ensures significant separation between the signed innovation projections under CA versus WA. Morever, this design vector β has a closed-form, compact expression given in Equation (13).

4.4 Risk Bound Analysis Using Simulated Data

This section assesses the looseness of the NIS- and IP-based P(HMI_k) bounds over time in a simulated example of landmark-based LiDAR/IMU navigation. The scenario involves a vehicle roving between two landmarks. We assume that the initial rover position is known and that a map is available. The EKF-based algorithm is based on prior work by Hassani et al. (2018).

In Figure 7(a), the rover’s location over time is shown with black triangles. The rover drives between two point-feature landmarks represented with black circles and is equipped with a LiDAR/IMU system whose specifications are given in Table 1. We selected the error model parameter values, including a large LiDAR extracted feature range error standard deviation, to facilitate risk evaluation through direct simulation using a tractable number of random samples. The red ellipses in Figure 7(a) represent the rover’s positioning errors under CA and are inflated by a factor 75 for better visualization. The ellipses’ size and shape vary over time due to changes in the LiDAR-to-landmark geometry as the rover moves.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 7

Covariance and risk analysis using LiDAR/IMU in EKF-based localization for a rover passing two static landmarks (a) Covariance analysis assuming correct associations (b) Integrity risk bounds using NIS versus IP-based data association

The respective contributions of a LiDAR’s range and bearing angle measurements to its position estimation can be isolated for individual point-feature landmarks (Joerger & Pervan, 2009). A single range measurement provides positioning information along the line-of-sight (LOS) direction between the LiDAR sensor and the landmark, with a constant standard deviation (set to 0.3 meters for this simulation). In contrast, the contribution of the linearized angular measurements to the standard deviation of the positioning error is orthogonal to the LOS and scales with distance to the landmark. This contribution is equal to the standard deviation of the angular error, in radians, multiplied by the distance to the landmark. In this example, that contribution varies from 0.04 to 0.18 meters for LiDAR-to-landmark distances of 5 to 20 meters. When multiple landmarks are available, the positioning contribution of LiDAR measurements can be represented as an intersection of ellipses (Joerger, 2009).

As shown in Figure 7(a), the rover starts with accurate position and heading angle estimates, which degrade over time due to drift in the IMU positioning and orientation errors. At travel distances of 5 to 10 meters, the angular measurements to both landmarks help reduce the positioning error. However, at travel distances of 14 to 16 meters, the east-position estimate is not as accurate because the two landmarks provide information primarily in the north direction. This effect is mitigated as the rover continues to 16–20 meters, but then accuracy decreases again as the LiDAR-to-landmark distance increases and the IMU limits the positioning error. For longer northward travel distances (not shown), the major axis of the covariance ellipses will grow unbounded in the east direction due to the combined effects of the LiDAR’s angular measurement error and the increasing uncertainty in the vehicle orientation estimate from IMU drift, and the minor axis in the north direction will approach the LiDAR’s ranging standard deviation divided by $\sqrt{2}$.

Covariance ellipses assume that CA is always achieved, whereas the integrity risk P(HMI_k) directly accounts for P(CA). Figure 7(b) shows P(HMI_k) curves evaluated using Equation (1) through direct simulation of random state prediction and measurement errors over 10,000 Monte Carlo (MC) trials. In this example, the actual risk curve for the IP method (red pentagram markers) matches that of the NIS approach (black circles) when the number of MC trials is statistically significant–i.e., for risk values larger than 10⁻⁵. We confirmed that the actual P(CA) was equal to one for both NIS and IP and at all travel distances.

In parallel, the bounds on P(HMI_k) are derived using (2) and (11) for NIS, and using (2) and (18) for IP. These bounds are shown with solid black and red curves, respectively. The black dashed line designates P(HMI_k|CA_K), which is common to both NIS and IP. The two solid curves for the NIS and IP bounds overlap for rover travel distances smaller than 15 meters and larger than 30 meters, where the dominant term in Equation (2) is P(HMI_k|CA_K). However, for travel distances between 15 and 30 meters, the conservative P(CA) bound becomes the dominant term in the NIS curve: over this segment, the differences between the solid black line and the black circles show that the NIS P(CA) bound can be overly conservative. This is not the case for the IP approach, which achieves P(HMI_k) bound values that are orders of magnitude lower.

5.1 Indoor Testing

Figure 8 shows the indoor testbed, which consists of a sensor-equipped rover moving along a figure-eight-shaped track and an infrared camera motion capture system that provides a ground-truth reference trajectory. (The motion-capture system is not visible in the figure because it is mounted near the ceiling). We use a Velodyne VLP-16 Puck LTE LiDAR and a NovAtel IMU-IGM-A1 coupled with NovAtel’s ProPak6. The IMU is set to record at a sampling rate of 100 Hz. The motion capture system includes twelve cameras–four VICON MX-T20s and eight Vantage 5(s)–that record the locations of small retro-reflective markers placed on the sensors and landmarks. This camera system provides sub-centimeter level positioning and mapping at a 200 Hz update rate. Data from all three sensors (motion-capture cameras, LiDAR, and IMU) are time-stamped using a common computer clock and post-processed.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 8

Testbed setup (infrared motion-capture cameras at the ceiling are not shown).

Four cylindrical landmarks are used for navigation. This simplistic test setup facilitates feature extraction and was chosen to avoid introducing feature extraction errors. In this case, the features are defined by the coordinates (expressed in the navigation frame) of the intersection between each cylinder’ axis and an arbitrary horizontal plane (e.g., the ground plane). For this implementation, we use a preset map of the cylinders. Our feature extraction routine was evaluated by Hassani et al. (2019), and the test setup parameter values are listed in Table 1.

Figure 9(a) shows the true and estimated trajectories of the rover using a thin black line and a thick blue line, respectively. These trajectories overlap closely. Rover positioning uncertainty is represented with red covariance ellipses, which are inflated by a factor of 5 to facilitate visualization. Background shades of gray are used in both panels of Figure 9 to identify segments of the rover trajectory: the dark gray area designates straight segments while the white and light-gray areas designate the top and bottom loops of the trajectory, respectively.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 9

Experimental cross-track positioning covariance and risk analysis using LiDAR/ IMU in EKF-based localization for a rover moving along an indoor test track (a) Estimated trajectory and covariance ellipses using LiDAR/IMU (b) Integrity risk bounds for the NIS versus IP data association criteria using LiDAR/IMU

Figure 9(b) displays the P(HMI_k) bounds computed using the NIS-based and IP-based data association criterion in black and red, respectively. Both P(HMI_k) bounds are derived from Equation (2) to evaluate the risk that the cross-track positioning error exceeds a 0.25 m alert limit, l. For both NIS and IP, the P(CA) bound in Equation (4) decreases monotonically over time, as it is the product of per-epoch contributions. The NIS bound rises quickly, increasing to 10^-1 within the first 3 seconds, suggests that this P(CA) term quickly dominates the NIS P(HMI_k) bound. In contrast, the IP-based bound remains lower than 10^-5 throughout the test. The red curve closely follows changes in P(HMI_k|CA_K), which varies with the positioning uncertainty caused by the rover’s position relative to the landmarks as the rover moves. As in the simulation example, the P(CA) bound for IP remains close to one and is significantly higher than for NIS.

5.2 Outdoor Testing in Realistic Driving Environment

Figure 10 shows the sensor platform used to collect experimental data in an urban environment, where objects like tree trunks and lamp poles can be extracted as landmarks. In this test, two synchronized Velodyne LiDARs and a NovAtel SPAN IMU recorded gyroscope and accelerometer outputs. Data were post-processed using the tightly integrated LiDAR/IMU algorithm developed by Hassani and Joerger (2023). The NovAtel SPAN’s GNSS Real Time Kinematic (RTK) solution serves as a reference truth trajectory.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 10

Overview of the experimental outdoor testbed, including the sensor platform and testing environment

Figure 11 shows four snapshots of the recorded point clouds, with the extracted landmarks shown in red. The 130-second-long experimental run started at a location where the algorithm could extract multiple features on all sides of the vehicle, as shown in Figure 11(a). Figure 11(b), robust landmarks were extracted on one side of the road, but on the opposite side, the algorithm selected inconsistent, closely located landmarks that it struggled to track: these landmarks were temporary delineator posts used during construction. The inconsistent extractions caused poor landmark geometries for position estimation. Figure 11(c) represents a time period over which landmarks were once again well distributed around the vehicle. Finally, Figure 11(d) shows a second cluster of landmarks that the algorithm successfully extracted, but the short distance between these landmarks could potentially increase the risk of WA.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 11

Snapshots of LiDAR point clouds during the outdoor test experiment (a) at the start of the trajectory; (b) 50 s into the run, during a period of poor landmark geometries due to non-extracted clustered features; (c) during a period between observations of the two landmark clusters; and (d) during the last part of the run while passing by a second cluster of landmarks (delineator posts used during construction).

Figure 12(a) shows the true and estimated trajectories of the rover in a local East-North horizontal plane. The two trajectories match closely. The covariance ellipses are shown with an inflation factor of 20, but they remain barely discernible except during the 40–60 s interval. This period corresponds to Figure 11(b), when the extracted and associated landmark geometry was poor. Importantly, the large covariance ellipses during this period are not necessarily an integrity issue because the navigation system is aware of the large estimation error. However, large covariance ellipses may cause loss of availability because the navigation solution knows that its position estimate should not be trusted. Smaller covariance ellipses are observed throughout the rest of the run, corresponding to Figures 11(c) and 11(d).

Figure 12(b) shows the P(HMI_k) bounds over time for an alert limit of l = 0.35 m. The gray diamond markers in the background represent variations in integrity risk under the assumption that CA is always achieved. These markers correspond to the term P(HMI_k|CA_K) in Equation (3), which is directly derived from the EKF covariance and should only be used in non-safety-critical applications. The thick orange line shows the integrity risk bound obtained using the NIS method. As soon as the first landmark cluster is seen (approximately 30 seconds into the run), the NIS-based risk bound is looser than P(HMI_k|CA_K). The NIS bound increases further when the second landmark cluster is seen at 90 s. In contrast, the IP method reduces the actual integrity risk and provides a much tighter integrity risk bound, achieving an integrity risk bound orders of magnitude lower than the NIS risk bound.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 12

Experimental cross-track positioning covariance and risk analysis using LiDAR/IMU in EKF-based localization in an outdoor test scenario (a) Estimated trajectory and positioning error covariance ellipses (b) Integrity risk bounds for the IP and NIS methods