## Summary

This paper describes the design, analysis, and experimental evaluation of a new landmark-based localization method that integrates light detection and ranging (lidar) with an inertial measurement unit (IMU). We develop a tight IMU/lidar integration scheme that exploits the complementary properties of the two sensors to facilitate safety risk evaluation. Lidar localization updates limit the IMU error drift over time while IMU data improve lidar position and orientation (or pose) prediction, thereby reducing the risk of incorrectly associating perceived features with mapped landmarks. In addition, lidar return-light intensity measurements are incorporated to better distinguish landmarks and to further reduce the risk of incorrect associations. We analyze the integrity performance of the localization algorithm using an automated testbed that generates analytical and empirical pose estimation error distributions.

## 1 INTRODUCTION

In this paper, we develop, analyze, and test new position and orientation (or pose) estimation and integrity monitoring methods using data from light detection and ranging (lidar) and an inertial measurement unit (IMU). Testing is performed in a mapped laboratory environment.

This research is intended for safety evaluation in autonomous vehicles such as automated driving systems (ADSs). ADS testing is necessary but insufficient to provide navigation safety guarantees because accumulating “autonomously driven” road miles does not provide a statistically significant number of traveled miles as compared with manned driving incidents (Kalra & Groves, 2017; Kalra & Paddock, 2016).

To quantify safety risks in ADS navigation, we leverage prior analytical work in global navigation satellite system (GNSS)-based aviation navigation, where safety is assessed in terms of integrity. Integrity is a measure of trust in sensor information (International Civil Aviation Organization, 2006). Several methods have been established to predict aircraft GNSS integrity risk (Radio Technical Commission for Aeronautics (RTCA) Special Committee 159, 1996; Working Group C, 2016). Unfortunately, these methods do not directly apply to ADS because ground vehicles operate under sky-obstructed areas, where GNSS signals can be altered or blocked by buildings and trees.

ADSs require sensors in addition to GNSS, such as IMUs, lidar, camera, and radar. This paper focuses on IMUs and lidar. The integration of lidar with an IMU improves pose prediction because the IMU can be used to coast between lidar pose updates and lidar updates can be used to calibrate IMU biases (Opromolla et al., 2016). Prior work includes tightly coupled implementations in which an IMU is used to determine the lidar tilt angle (Soloviev, 2008; Soloviev et al., 2007). In robotics, lidar-based localization is often achieved by odometry (e.g., from an IMU) and simultaneous localization and mapping (SLAM) (Bresson et al., 2015; Dryanovski et al., 2013; Guivant & Nebot, 2001; Guivant et al., 2000; He et al., 2018; Hess et al., 2016; Joerger et al., 2016; Leonard & Feder, 2000; Montemerlo & Thrun, 2003; Nerurkar & Roumeliotis, 2011; Zheng & Zhang, 2019). However, SLAM is insufficient in safety-critical ADS navigation applications because localization errors drift over distance and loop closures are trajectory-constraining.

In this paper, we assume that an *a priori* map is available. The first category of map-based lidar localization approaches includes matching and correlation methods. These methods aim to maximize data point correspondences between the lidar point cloud (PC) and the map, whether the map itself is a PC (Guo et al., 2014; Pomerleau et al., 2013; Sappa et al., 2001) or an occupancy grid map, i.e., a tessellated representation of the PC (Fan et al., 2018; Luo et al., 2020; Nuss et al., 2015). However, rigorous safety risk quantification via matching methods is an unsolved and cumbersome problem. Instead, this research uses a landmark-based localization method for which we have developed an integrity risk equation (Hassani et al., 2018; Joerger et al., 2017 2016; Joerger & Pervan, 2017). Landmark-based localization aims to identify landmarks in the lidar PC and to match them with mapped landmarks (Hunde & Ayalew, 2018; Pirovano et al., 2020; Vosselman & Dijkman, 2001).

Landmark-based localization requires two pre-estimator procedures (Hunde & Ayalew, 2018; Pirovano et al., 2020). First, feature extraction aims to identify the most consistently recognizable, viewpoint-invariant landmarks in the lidar PC. Then, data association matches the ordering of mapped landmarks to that of PC-extracted features over successive observations (Bailey, 2002; Bar-Shalom et al., 1990; Cooper, 2005; Joerger & Pervan, 2009). Incorrect association is a well-known algorithmic fault that can cause a loss of navigation integrity.

This paper builds upon the multiple-hypothesis extended Kalman filter (EKF) innovation-based data association method presented by Joerger et al. (2016). This method provides a means for evaluating the incorrect association risk of the matching process while considering all possible combinations and permutations of sensed landmarks to mapped landmarks. The incorrect association probability is then used to bound the integrity risk of lidar-based pose estimation over successive iterations. Joerger et al. (2016) and Joerger & Pervan (2017) showed that the incorrect association probability grows rapidly in cluttered environments. One approach to mitigate this problem is to select a subset of the most distinguishable features in the lidar PC (Joerger et al., 2017). However, this approach reduces the number of redundant associations and lowers the ability to detect unwanted, unmapped landmarks.

In response, in this paper, we enhance data association and integrity monitoring performance by tightly integrating lidar with an IMU and by incorporating lidar return-light intensity measurements. In addition, we design and implement an experimental testbed to evaluate the localization and data association performance of the IMU/lidar algorithm.

We first develop a tightly integrated IMU/lidar process specifically to quantify integrity risk. IMU integration can reduce integrity risk not only by improving pose prediction but also by lowering the risk of incorrect association. Then, we derive a new method to exploit return-light intensity measurements provided by lidar in addition to range and bearing angle observations. Light intensity measurements can improve the system’s ability to distinguish landmarks if their surfaces have different reflectivity properties. For example, lidar intensity can help identify an aluminum pole from a pedestrian.

Section 2 of this paper describes the tightly integrated EKF-based IMU/lidar algorithm. Nonlinear continuous-time dynamic-propagation and measurement equations are derived, linearized, and discretized. Section 3 presents a derivation of the multiple-hypothesis data association and integrity risk bounding methods, with a focus on the contributions of IMU and lidar intensity measurements. In Section 4, we perform direct simulations and a covariance analysis to evaluate the risk reduction brought about by the IMU. In Section 5, we experimentally quantify the reduction in integrity risk achieved when incorporating (a) IMU data, (b) lidar intensity measurements, and (c) both IMU data and lidar intensity.

## 2 HIGH-INTEGRITY IMU AND LIDAR MEASUREMENT MODELS

### 2.1 IMU Measurement Equations

In this implementation, we use raw IMU accelerometer and gyroscope measurements. The IMU is fixed in the ADS body frame “B,” which can be approximately oriented along the vehicle’s principal axes of inertia. The IMU accelerometers measure inertial acceleration, i.e., second time derivatives of position with respect to the inertial frame (labeled “I”), and we are interested in the ADS pose expressed in a navigation frame “N” (for example, in the local east, north, up directions). We also define an earth-centered, earth-fixed frame “E” because N may move in E. We use the Newton and Euler methods to describe the ADS translational and rotational motion. The following three equations express the time derivative of the ADS velocity with respect to N and expressed in N, the time derivative of the position with respect to E and expressed in N, and the time derivative of the rotation matrix from B to N, respectively (following equation [3.26] in Titterton et al. (2004)):

1

2

3

where:

is the 3×1 ground speed vector, i.e., the vehicle velocity vector with respect to E expressed in N, | |

^{A}d/dt | is a time derivative with respect to frame “A”, where A may stand for B, E, I, or N, |

is the 3×3 rotation matrix from B to N (Titterton et al., 2004), | |

is the 3×1 vehicle position expressed in N, | |

is the 3×1 IMU-measured and corrected specific force vector expressed in B, as derived in Appendix B, | |

^{N}ω^{IE} | is the angular velocity vector of E with respect to I expressed in N, |

^{N}ω^{EN} | is the angular velocity vector of N with respect to E expressed in N, |

is the measured and corrected angular velocity vector of B with respect to I expressed in B, as derived in Appendix B, | |

^{N}g | is the local gravity vector at the IMU axis center expressed in N (Rogers, 2007), |

[a×] | is the skew symmetric matrix of vector a. |

### 2.2 Lidar Range and Bearing Angle Measurement Equations

A raw lidar PC consists of thousands of data points, each of which individually does not carry useful navigation information. Thus, raw measurements must be processed before they can be used for localization. Feature extraction aims to consistently extract identifiable static landmark features. Figure 1 shows an example of a lidar PC collected in our laboratory testbed. Colors from red to blue designate high to low levels of return-light intensity, respectively.

The experimental testbed in Figure 1 includes six static vertical cylinders with different surface properties, which are easy-to-distinguish landmarks that facilitate feature extraction. We want successful feature extraction because this process is not the primary focus of this paper. Feature extraction aims at finding the center of quasi-circular ellipses formed by the projection of the cylinders’ edges in the lidar zero-elevation angle plane. Figure 2 illustrates our two-step feature extraction algorithm.

*Segmentation*: Within an elevation cone, range differences over the azimuthangle sequence help distinguish cylinders from the background. Point clusters corresponding to cylinders can thus be segmented.*Model fitting and feature estimation*: The segmented points are projected in the zero-elevation plane, and a two-dimensional (2D) circle-fitting algorithm is used to fit a circle to the projected points. This step assumes that the cylinders are vertical and that the elevation plane is horizontal, which is valid in our lab environment.

The center of the fitted circle, parameterized by its range and bearing angle with respect to the lidar, is the extracted point feature. Let * ^{i}d* and

*, respectively, be the range and bearing angle measurements in B for the point feature of landmark ‘*

^{i}a*i*’, where

*i*= 1…

*n*and

_{L}*n*is the number of extracted features. The horizontal position of the extracted point feature is time-invariant in the navigation frame N, which, in this paper, is fixed in E. In addition, let

_{L}*and*

^{i}p_{E}*, respectively, be the east and north position coordinates of landmark “*

^{i}p_{N}*i*” in N. The ADS position and orientation vectors in N,

**x**

_{ADS}and

**e**

_{ADS}, respectively, can be expressed as follows:

4

5

where *x _{E}*,

*x*,

_{N}*x*are the three-dimensional (3D) ADS position coordinates along the east, north, up axes, and

_{U}*ϕ*,

*θ*,

*ψ*are the ADS Euler angles. Euler angles can be extracted from the rotation matrix in Equation (3), as described in Appendix (B).

Using these notations, the nonlinear lidar range and angular measurements are respectively given as follows:

6

7

In Equations (6) and (7), *v _{d}* and

*v*are random feature measurement errors. Feature extraction error distributions are not Gaussian, but the error’s cumulative distribution function (CDF) can be overbounded by using zero-mean normal CDF models, as described in Appendix (C) (Blanch et al., 2019; DeCleene, 2000; Rife et al., 2006). Throughout the paper, the function “arctan(

_{a}*b/a*)”, for and , designates a function that equals arctan(

*b/a*) when

*a*> 0, arctan(

*b/a*) +

*π*when

*a*< 0,

*π*/ 2 when

*a*= 0 and

*b*> 0, and −

*π*/ 2 when

*a*= 0 and

*b*< 0.

We can stack the ranging and angular measurements for all extracted landmarks in a 2*n _{L}* × 1 vector and write the lidar nonlinear measurement equation as follows:

8

9

10

where:

x | is an n × 1 state vector including ADS position, velocity, orientation, and 3D IMU gyroscope and accelerometer biases; i.e., _{s}n = 15,_{s} |

v | is the 2n × 1 feature measurement error vector._{L} |

Vector ** v** is modeled as a vector of normally distributed random variables with zero mean and covariance matrix

**V**. We use the following notation:

**~ N(**

*v***0**,

**V**). Nonzero elements of the diagonal matrix

**V**are derived in Appendix (C). In Equation (8), the vector function

**h**(

**x**) consists of stacked nonlinear equations (Equations (6) and (7)) arranged as indicated in Equation (9).

In addition, the lidar provides return intensity measurements for each PC point. We evaluate the mean intensity measurement for landmark “*i*” by averaging intensity values for all points in a point cluster associated with landmark *i*. The *n _{L}* × 1 return-light intensity measurement vector is modeled as follows:

11

where we use the overbounding distribution derivation in Appendix D to model **s** as normally distributed with mean **s** and diagonal covariance matrix **V**_{s}. Vector *v*_{s} is an *n _{L}* × 1 intensity measurement error vector modeled as

*v*_{s}~ N(0,

**V**

_{s}), as described in Appendix D.

### 2.3 Linearization and Discretization of IMU and

First, we linearize the IMU measurement equations. The continuous-time model is linearized by using a first-order Taylor series expansion about reference state parameter values. We use the notation “δ” to indicate deviations of state parameters relative to the reference values. Using Equations (1)–(3) and the accelerometer and gyroscope measurement equations in Appendix B, we can write a continuous-time linearized state propagation model as follows:

12

13

14

15

where **b**_{gy}, and **b**_{ac} are bias vectors of the IMU accelerometers and gyroscopes, **I** is a 3 × 3 identity matrix, ^{N}**ω**^{IN} is the angular velocity vector of I with respect to N expressed in N (which can be defined as ^{N}**ω**^{IN} = ^{N}**ω**^{IE} + ^{N}**ω**^{EN}), and is the corrected specific force expressed in N. Matrices **F**_{H2V} and **F**_{V2T} are defined in Appendix A, and **S**, **M**, *τ*, **n**, and ** v** are defined in Appendix B for both the accelerometers and gyroscopes.

The discrete-time form of Equation (12) can be written as follows:

16

where *k* is a discrete time step and **Φ**_{k−1} is an *n _{s}* ×

*n*state transition matrix over the IMU sampling interval, i.e., between time steps

_{s}*k*– 1 and

*k*. The discrete-time IMU measurement equations and the method for computing

**Φ**

_{k−1}are found in Appendix B. Then, we linearize the lidar measurement equations. We can linearize Equation (8) about our best prediction of the vehicle pose . Considering both the lidar angular and ranging measurements, the total number of extracted feature measurements is

*n*= 2

*n*. Let be the

_{L}*n*× 1 feature measurement vector in Equation (8). We use the overbounding distribution derivation in Appendix C to model as . Using a first-order Taylor series approximation, the linear measurement equation can be written in terms of the predicted state vector under the correct-association hypothesis at time step

*k*as follows:

17

where the observation matrix **H**_{k} is a linearized measurement-to-state coefficient matrix. The linearized range and bearing angle measurement vectors and their measurement error vectors are denoted as δ**d**, δ**a** and *v*_{d}, *v*_{a}, respectively. The coefficient matrices **F**_{d,x}, **F**_{a,x}, and **F**_{a,e} are determined by using the state prediction vector and assuming a correct association, as described in Appendix A. It is worth noting that the subscript *k* is used in both Equation (16) and Equation (17). However, Equation (17) is only relevant when lidar measurements are available, typically at regular 0.1-s intervals (for a 360° azimuth scan), whereas the IMU sampling interval is 10–20 times smaller.

## 3 INTEGRATED LIDAR/IMU ESTIMATION PROCESS

In this section, we use an EKF to tightly integrate the lidar and IMU, and then, we derive an analytical upper bound on the ADS pose integrity risk that accounts for incorrect associations. The block diagram in Figure 3 outlines the three main steps of this process, which are color-coded and described in detail in Sections 3.1–3.3. The inputs to the block diagram are the IMU and lidar measurements and the map; the outputs are the pose estimation and integrity risk bound. Section 3.1 describes the IMU-based prediction process and includes the EKF initialization. Section 3.2 presents the EKF measurement update requiring data association; its output feeds into the state propagation equation. Section 3.3 describes the integrity risk bounding process, which accounts for the impact of incorrect associations.

### 3.1 EKF Initialization and IMU-Based Pose Prediction Process

Figure 4 shows EKF initialization and prediction of the state vector **x**_{k} with IMU measurements as input. This figure also shows the initialization of components of the integrity risk bound or probability of hazardous misleading information (HMI). The IMU specific force and angular velocity measurements are employed in two parallel processes. (a) *Nonlinear state propagation:* We use the discrete-time forms of the nonlinear expressions in Equations (1)–(3), the derivation of attitude using the rotation matrix from B to N, and the IMU measurements described in Appendix B to predict the state vector at each time step (Titterton et al., 2004). (b) *Linearization, discretization, and covariance propagation:* We apply Van Loan’s algorithm with the linearized form in Equation (12) to compute the discrete-time transition matrix **Φ**_{k−1} and the process noise covariance matrix **W**_{k−1}. Let be the predicted state estimate and be the state prediction covariance matrix. The other parameters in Figure 4 are defined as follows:

is the n × 1 initial predicted state estimate vector,_{s} | |

is the n × _{s}n initial EKF state prediction covariance matrix,_{s} | |

P(HMI_{0}) | is the initial value of the probability of HMI, |

B_{k} | is the 3 × 3 rotation matrix relating navigation axes at time k – 1 to navigation axes at time k, as given in Appendix B (Titterton et al., 2004), |

W_{k} | is the n × _{s}n process noise covariance at time step _{s}k, |

is the n × 1 predicted state estimate vector at time step _{s}k, | |

is the n × _{s}n prediction of the EKF covariance matrix at time step _{s}k, | |

g | is the discrete-time form of Equations (1) and (2), defined in Appendix B. |

We propagate the state vector **x**_{k} by using the nonlinear expressions in Equations (1)–(3). After each iteration in the EKF dynamic propagation update, we assess whether lidar measurements are present for the current time step *k*. If such measurements are available, we implement a measurement update, as described in Section 3.2; otherwise, we continue iterating the dynamic propagation equations (Equations (1)–(3)).

### 3.2 Data Association Criterion and EKF Measurement Update

We want to process the linearized lidar measurement equation (Equation (17)) using the EKF measurement update to obtain a correction δ**x**_{k}, an estimate of the ADS state vector , and the covariance matrix . This process requires that lidar measurements be correctly associated with mapped landmarks because their ordering is not necessarily the same.

To perform data association, we use an innovation-based approach (Joerger et al., 2016). The innovation vector under correct association **γ**_{0,k} is given by the following:

18

where **A**_{0,k} is the *n* × *n* permutation matrix that corresponds to the correct association. In practice, we do not know which is the correct permutation matrix **A**_{0,k}, but we can write an exhaustive set of permutation matrices.

The innovation vector can be interpreted as a measure of consistency between the extracted feature measurements and the measurement prediction vector . A more accurate state prediction corresponds to a higher likelihood of correct association. The state prediction is improved, for example, by using IMU data instead of a vehicle kinematic model.

#### 3.2.1 Accounting for Incorrect Data Association

Extracted landmark feature measurements are arbitrarily ordered in vector . In this paper, the number of measured landmarks in the lidar field of view (FOV) can be predicted by using a reliable map and vehicle pose prediction. In the case of occlusion, if two landmarks are in the same azimuth bin, then only the landmark nearest to the lidar is visible. Other cases such as failed extractions or extracted-but-unmapped landmarks have been addressed in prior work via combination matrices and detection (Hassani et al., 2019; Joerger et al., 2017). This paper focuses on the incorporation of IMU and intensity measurements. Let *n _{L}* be the number of extracted landmarks that are visible in the lidar FOV. There are (

*n*!) potential ways for assigning the observed landmarks to the mapped landmarks, which is the number of all possible landmark permutations.

_{L}Incorrect association occurs when the ordering of measured landmarks differs from that of mapped landmarks. There is only one correct ordering; thus, the number of incorrect associations is *h _{IA}* ≡

*n*! −1. For risk evaluation, we consider all possible orderings of measurements, and where

_{L}*i*= 0…

*h*. In an example scenario with

_{IA}*n*= 3 landmarks (both extracted and mapped), the numbers of possible landmark permutations and incorrect associations are 3! and

_{L}*h*= 5.

_{IA}The innovation vector **γ**_{i,k} has a zero mean only under correct association. Any other (incorrect) association causes the mean of the innovation vector to be non-zero. Thus, the innovation vector is a good indicator of incorrect association. The innovation vector can be expressed as follows:

19

where **A**_{i,k} are *n* × *n* permutation matrices for *i* = 0…*h _{IA}*.

Based on this criterion, we select the candidate association that satisfies the following equation:

20

where:

Figure 5 shows a detailed description of the second block in Figure 3. Lidar measurements, map data, predicted states, and the covariance matrix serve as inputs to the data association process in Equation (20). Then, we proceed to the EKF measurement update, calculate the Kalman gain **K**_{k}, and determine the *n _{s}* × 1 state estimate vector and the

*n*×

_{s}*n*estimation error covariance matrix . The state prediction vector in Equations (18) and (19) is more accurate when a tightly integrated IMU is used as compared with an ADS kinematic model, which ultimately reduces the risk of incorrect association.

_{s}#### 3.2.2 Integration of Lidar Return-Light Intensity to Improve Data Association

The association process can be further improved by using lidar return-light intensity. The difference between the lidar-extracted mean intensity measurements and that provided in the map, which is captured in the intensity-separation vector ξ* _{i,k}*, can be expressed as follows:

21

where:

s_{k} | is an n × 1 mean return-light intensity vector for all _{L}n visible landmarks,_{L} |

ŝ_{k} | is the n × 1 lidar-measured mean return-light intensity vector; we assume _{L}ŝ_{k} ~ N(s_{k}, V_{S,k}), |

is the mapped mean return-light intensity vector, | |

is an n × _{L}n covariance matrix capturing the uncertainty in the mean intensity values of the mapped landmarks,_{L} | |

are n × _{L}n permutation matrices similar to those in Equation (19) but for scalar permutations, for _{L}i = 0…h._{IA} |

Similar to the innovation vector in Equation (18), the intensity separation vector in Equation (21) has a zero mean only if the correct association is found. Landmark intensity parameters are not included in the EKF prediction and estimation processes because they do not provide direct information on ADS states. However, we can still improve the association criterion by augmenting the innovation vector with ξ* _{i,k}*. The resulting 3

*n*× 1 “separation vector” is defined as follows: . The association selection criterion for incorporating the return-light intensity is given by the following:

_{L}22

where:

### 3.3 Integrity Risk Bound

The integrity risk *P*(*HMI _{k}*), or probability of HMI at time step

*k*, is the probability of the ADS being outside of a specified alert limit box when the vehicle position is estimated to be inside this box (Joerger & Pervan, 2009; Reid et al., 2019). In ADS lane-centering applications, lateral deviations are of primary concern, and the alert limit is defined as the distance between the edge of the car and the edge of the lane when the car is at the center of the lane (Joerger et al., 2016). An analytical bound on the integrity risk that considers all possible incorrect associations has been given by Joerger & Pervan (2019) and is expressed as follows:

23

with:

24

25

where:

K | designates a range of time indices: K = {1…k}, |

J | designates a range of time indices: J = {1…j}, |

Q() | is the tail probability function of the standard normal distribution, |

ℓ | is the specified alert limit that defines a hazardous situation, |

σ_{k} | is the standard deviation of the estimation error for the vehicle state of interest, |

I_{ALLOC,k} | is a predefined integrity risk allocation for feature extraction, chosen to be a fraction of the overall integrity risk requirement I,_{REQ,k} |

is a chi-square distributed random variable with a number of degrees of freedom that is the sum of the number of measurements and of states at time step j, | |

represents the minimum value of the mean landmark feature separation, including intensity separation, at time step j. |

The probability of correct association in Equation (25) is a function of , which defines a probabilistic lower bound on the true value of *ζ _{k}* in Equation (22). This lower bound on landmark separation is set such that the risk of the true value of

*ζ*being smaller than does not exceed

_{k}*I*.

_{ALLOC,k}By integrating lidar with an IMU, we can reduce positioning errors, thereby lowering the risk *P*(*HMI _{k}* |

*CA*). In addition, IMU and return-light intensity measurements are instrumental for increasing the ability of the localization system to distinguish landmarks. In Equations (23)–(25), IMU and return-light measurements enable greater separation values, which increases the probability of correct association

_{K}*P*(

*CA*|

_{j}*CA*

_{J−1}) and ultimately reduces

*P*(

*HMI*). We will quantify this

_{k}*P*(

*HMI*) reduction using simulation and experimental data in the next two sections. Equations (23)–(25) are represented by the “Integrity Risk Evaluation” block in Figure 3, and the output is

_{k}*P*(

*HMI*).

_{k}## 4 INTEGRITY RISK EVALUATION USING SIMULATED DATA

In this section, we analyze the integrity performance of a “lidar-only” scheme compared with the IMU/lidar scheme described in Section 3. In this 2D horizontal simulation, an ADS roves between two landmarks located 10 m apart. The initial pose of the ADS is known, and it is assumed that we have a map of landmark positions in the navigation frame N. The surface reflectivity is identical for all landmarks, and intensity measurements are not used in this first evaluation. The simulation settings and lidar and IMU parameters are listed in Table 1.

### 4.1 Covariance Analysis and Integrity Risk Bound (Analytical vs. Direct Simulation)

In Figure 6, the positions of landmarks are represented by black circles, and the ADS trajectory is shown by black triangles. The 2D estimation error covariance ellipses, which represent the spread of pose estimation error, are shown in solid and dashed red lines and are inflated by a factor of 50 for better visualization. The size and shape of the covariance ellipses change as the sensor-to-landmark geometry changes because of ADS motion. The relative lengths of the semimajor and semiminor axes are also related to the standard deviations of the lidar angular and ranging measurements, as explained by Joerger (2009). This figure also shows that the integration of IMU with lidar improves the ADS trajectory estimation.

In Figure 7, we evaluate the analytical integrity risk bound *P*(*HMI _{k}*) as compared with the actual integrity risk calculated by direct simulation over 50,000 Monte Carlo (MC) trials for the lidar-only and IMU/lidar schemes. We focus on lateral deviations for integrity risk evaluation; the lateral alert limit is defined in Table 1. As captured in Equation (23),

*P*(

*HMI*) accounts not only for lateral covariance variations, but also for the probability of correct association

_{k}*P*(

*CA*).

_{k}In Figure 7, black and red circles represent the integrity risk curves obtained by direct simulation. In parallel, the black and red solid lines are the analytical bounds computed from Equation (23). Both direct simulation and analytical bounds for the IMU/lidar scheme are orders of magnitudes lower than that for the lidar-only scheme at 10–20 m of travel distance when the vehicle is close to the landmarks. Simultaneously using the IMU improves the pose estimation and data association, which results in a reduced integrity risk. The direct simulation and analytical bounds for the lidar-only scheme overlap. Discrepancies occur for low risk values: these discrepancies would disappear if we simulated more than 50,000 trials, but our computational resources are finite.

## 5 EXPERIMENTAL INTEGRITY RISK EVALUATION

In this section, we quantify navigation integrity for the multi-sensor IMU/lidar system described in Sections 2 and 3. We consider three configurations: lidar-only, lidar+ (incorporating mean intensity measurements with lidar range and bearing angle), and IMU/lidar+ (using all available sensor information). When using lidar only, state prediction is obtained by using a coarse kinematic model to replace Equation (12). This model propagates ADS states assuming a constant velocity vector between lidar measurement updates. This approach can be inaccurate for rapid ADS dynamics. When using the IMU, we apply Equation (12) to improve state prediction and also to enhance data association. We performed an experimental test to quantify the risk reductions brought about by incorporating lidar intensity and IMU measurements as compared with the lidar-only approach. An automated sensor platform was moving on a figure-eight track next to a predefined set of landmarks, some of which were occluded over segments of the trajectory. Landmark occlusions can cause an increased risk of incorrect association.

Additional testing results and performance comparisons for different sensor combinations have been reported by Hassani et al. (2019). In this paper, we focus on using lidar with intensity measurements and IMU data. Table 2 lists the parameters and settings of the test. In Figure 10, we use four landmarks, each identified by a number ranging from 1 to 4. The surface properties of the landmarks are not all the same: we use cylinders with a retroreflective surface for Landmark 1, black surfaces for Landmarks 2 and 4, and a white surface for Landmark 3.

### 5.1 Experimental Testbed

We designed and built an automated sensor safety evaluation testbed to quantify the impact of incorrect association on the integrity risk *P*(*HMI _{k}*). The testbed shown in Figure 8 is composed of a rover housing a sensor platform on a figure-eight track. The rover can operate unattended for many hours to collect large amounts of lidar and IMU data. This testbed provides a means for analyzing the performance of a navigation system over repeated trajectories, over a significant number of such trajectories, and in a controlled environment in which we can focus, for example, on the integrity impacts of landmark occlusions and of landmarks with varying surface properties. Compared with the approach using only lidar range and angle measurements, this approach helps assess the relative performance improvement brought about by additional IMU and light-intensity data. Other experiments using sensors mounted on a car’s roof rack will be performed in future work.

In this experiment, cardboard cylinders serve as landmarks to facilitate feature extraction from the lidar PC. These cylinders are covered with white and black felt and retroreflective straps (Landmark 1, first cylinder from the left) to provide different surface reflectivities. As the rover moves, the leftmost landmark (Landmark 1) is periodically occluded behind another landmark (Landmark 2), which tests the ability of the data association process to dynamically distinguish landmarks.

The sensor platform mounted on the rover includes the lidar and IMU stacked vertically to minimize lever arm calibration errors. We used Velodyne’s VLP-16 Puck LTE lidar and NovAtel’s IMU-IGM-A1 coupled with NovAtel’s ProPak6. The IMU was set to record at a 100-Hz sampling rate. Additionally, an infrared (IR) camera motion capture system (VICON) provides reference truth values for the position and orientation of the moving platform and for the static landmarks in the navigation frame. Twelve cameras, i.e., four VICON MX-T20 cameras and eight Vantage 5 cameras, record small retroreflective markers placed on the sensors and landmarks, providing sub-centimeter-level positioning. All three sensors (IR cameras, lidar, and IMU) are time-tagged using the same computer clock.

### 5.2 Using Lidar Range, Bearing, Intensity, and IMU Measurements

The integrated solution using IMU, lidar range, angle, and intensity measurements is referred to as the IMU/lidar+ configuration. In Figure 10, four landmarks are used. The lidar range limit is such that all landmarks are continuously in view of the lidar except where Landmark 2 occludes Landmark 1. The estimated trajectory is represented by a blue line and the true trajectory by a black line. The estimated and true trajectories overlap. The black arrow shows the direction of motion at the starting point. Background colors help identify segments of the rover trajectory for results presented over time: the rover follows straight line paths in the dark gray area, is in the top loop when in the white area, and is in the bottom loop when in the light gray area. The purple area represents ADS locations at which Landmark 1 is occluded by Landmark 2 in the upper loop of the figure-eight track. This arrangement makes data association challenging because Landmarks 1 and 2 can be mistaken for one another when Landmark 1 comes in and out of sight.

Figure 10 also shows red covariance ellipses representing the 2D positioning uncertainty for ADS locations taken at regular 0.8-s intervals. Covariance ellipses are inflated by a factor of five to facilitate visualization. Ellipses grow when Landmark 1 is hidden (purple area).

In addition, we derived a bound on the risk of the cross-track positioning error exceeding an example alert limit of 0.35 m (Reid et al., 2019). This integrity risk bound is predictable. The event of the risk bound exceeding the risk requirement causes a loss of availability. Thus, in this paper, we want the integrity risk bound to be as low as possible to achieve high availability performance. Both the actual integrity risk itself and our ability to analytically upper-bound this risk determine the value of the predicted risk bound. Thus, as compared with using lidar ranges, additional information from the IMU and lidar intensity not only helps reduce the actual *P*(*HMI*) but also helps tighten the predicted *P*(*HMI*) bound. Figure 11 shows *P*(*HMI*) curves for the lidar-only, lidar+, and IMU/lidar+ schemes. We find the highest risk bound values in the purple regions because the occlusion of Landmark 1 causes relatively poor landmark geometry. The lidar-only approach performs poorly, with a *P*(*HMI*) bound approaching a value of 1 as soon as the first difficult-to-identify landmark geometry is encountered. The lidar+ approach is consistently better except when Landmark 1 is occluded because fewer measurements are available and the risk of incorrect association increases. Finally, as expected, the IMU/lidar+ approach outperforms the other configurations, and our tests show that the resulting *P*(*HMI*) bound is at least four orders of magnitude lower than those of the other cases.

### 5.3 Repeated Trajectories

Figure 12 shows the ADS cross-track positioning error and covariance envelopes over 100 laps. The figure-eight track helps evaluate navigation system performance repeatability. The “1*σ*” one-dimensional envelope represents the boundary within which 68% of the error samples are expected to occur, assuming a zero-mean error.

The solid red line in Figure 12 is the analytical covariance envelope. The analytical covariance determines the contribution of *P*(*HMI _{k}* |

*CA*) to

_{K}*P*(

*HMI*) in Equation (23). The dashed red line is the sample covariance envelope, which is smaller than the analytical envelope over the entire 20-s-long trajectory; we want the analytical error bound to be larger than the sample envelope. Cross-track positioning error curves are color-coded from light blue to dark blue as the rover travels from the first to the last lap. Small discrepancies can be observed between early and late laps (e.g., accentuated at the 12- to 14-s time points), which are due to imperfections in the testbed, including a warm-up period causing variations in vehicle speed and sensor performance (Reina & Gonzales, 1997; Ye & Borenstein, 2002). Overall, we find that the pose estimation error curves are conservatively captured by the analytical covariance envelope.

_{k}## 6 CONCLUSIONS

In this paper, we derived a new IMU/lidar integration method that enables integrity risk evaluation while accounting for all possible incorrect associations between observed and mapped landmarks. The IMU improves the state prediction and reduces incorrect association risks. Our method also incorporates lidar return-light intensity measurements with lidar range and bearing data to better distinguish landmarks, which also results in a quantifiable reduction in incorrect association risk. We implemented a new analytical method to quantify the improvement in the probability of correct association. In addition, we evaluated the proposed integrity risk bound using empirical data in a structured, well-understood environment. Compared with the lidar-only approach in this specific testing environment, the performance assessment demonstrated a reduction in integrity risk of several orders of magnitude when the IMU and lidar intensity are used. Future work includes testing these methods in more realistic, unstructured environments.

## HOW TO CITE THIS ARTICLE

Hassani, A., & Joerger, M. (2023). Analytical and empirical navigation safety evaluation of a tightly integrated lidar/IMU using return-light intensity. *NAVIGATION, 70*(4). https://doi.org/10.33012/navi.623

## CONFLICT OF INTEREST

The authors declare no potential conflicts of interest.

## ACKNOWLEDGMENTS

The authors gratefully acknowledge the National Science Foundation for supporting this research (NSF award CMMI-1637899). The opinions expressed in this paper do not necessarily represent those of any other organization or person.

## APPENDIX

## A IMU AND LIDAR MEASUREMENTS AND COEFFICIENTS

The IMU measurement coefficient matrices in Equation (14) are defined as follows (Titterton et al., 2004):

A.1

A.2

where:

R | is the earth’s radius, |

h | is the vehicle’s altitude, |

λ | is the vehicle’s latitude, |

g_{0} | is the acceleration of gravity at zero altitude. |

The lidar measurement coefficient matrices in Equation (17) are as follows (Joerger, 2009):

A.3

A.4

A.5

where and .

## B DISCRETE-TIME EQUATIONS FOR THE IMU

The ADS specific force is measured with respect to the inertial frame “I” and expressed in body frame “B” as ^{B}**f**. The specific force measurement is imperfect: it can be modeled in the continuous-time domain as follows:

B.1

where:

^{B}f | is the 3×1 true specific force vector of body B with respect to I expressed in body frame B, |

is the measured specific force vector of body B with respect to I expressed in B, | |

S_{ac}, M_{ac} | are the true accelerometer calibration scale factor and misalignment matrices in B, |

b_{ac} | is the accelerometer time-varying bias vector in B, |

v_{ac} | is accelerometer measurement white noise error component expressed in B. |

In Equation (B.1), the measured specific force is expressed in terms of the scale factor and misalignment matrices for which manufacturers provide estimates **Ŝ _{ac}** and , respectively. The symbol in

**Ŝ**designates the estimate of parameter

_{ac}**S**.

The accelerometer time-varying bias is modeled as a first-order Gauss–Markov process (GMP) (Brown & Hwang, 2012). We can write the corrected specific force and the continuous-time dynamics of the time-varying bias as follows:

B.2

B.3

where:

τ_{ac} | is the GMP time constant, |

n_{ac} | is a 3×1 vector of GMP time-uncorrelated driving noise. |

The discrete-time forms of Equations (B.1)–(B.3) can be written as follows:

B.4

B.5

B.6

where:

t_{s} | is the IMU sampling interval. |

The gyroscope measures the body frame angular velocity with respect to the inertial frame and can be expressed in the body frame as (Titterton et al., 2004). We can derive equations similar to Equations (B.2)–(B.6) for gyroscope measurements. These equations have been reported by Hassani et al. (2019).

Assuming that the IMU corrected specific force and angular velocity remain constant over the short IMU sampling interval *t _{s}*, between time steps

*k*– 1 and

*k*, we can write the discrete-time form of Equations (1)–(3) and the attitude equations as follows:

B.7

B.8

B.9

B.10

where:

The notation in Equation (B.10) designates the (*i*, *j*)-th scalar component of matrix , i.e., the component in the *i*-th row and *j*-th column.

Finally, we use the Van Loan algorithm to determine the discrete-time state propagation **Φ**_{k−1} and process noise covariance matrices **W**_{k−1} based on the continuous-time matrices **F** and spectral density function of δ**w**, defined as **Q** (Brown & Hwang, 2012).

## C OVERBOUNDING OF MEASUREMENT ERROR DISTRIBUTIONS

This appendix describes a method for deriving probabilistic models of the extracted feature measurements. This method is based on the overbounding theory described by DeCleene (2000). Overbounding theory is used in aviation navigation to model non-Gaussian sample distributions, even when they are not symmetric, unimodal, or zero-mean (Blanch et al., 2019; Rife et al., 2006). We collected lidar PC data for 4,250 sensor-to-landmark geometries, processed them using our feature extractor, and stored the estimated point-feature range and bearing angle measurements. Figure C1 shows the CDF for the range and bearing angle measurement error in quantile-to-quantile plots. The x-axis scales with theoretical standard normal distribution quantiles. The y-axis scales with the sample measurement error distribution quantiles. If the empirical measurement error distribution were normal, the sample points would lie on a straight line with a slope equal to the sample standard deviation and with the y-intercept equal to the sample mean. Figure C1 shows that the core of the distribution behaves like a normal distribution within ±2*σ* on the x-axis, i.e., 95% of the time. However, the sample distributions have wide tails. The black lines in Figure C1 are overbounding Gaussian functions, which account for errors occurring 99.5% of the time, i.e., out to “3*σ*.” The bounding standard deviations are 0.12 m for the range measurement error (versus 0.03 m for the sample standard deviation) and 2° for the bearing angle measurement error (versus 1° for the sample standard deviation). Thus, Gaussian overbounds are conservative compared with sample measurement error distributions, which will impact the pose estimation error distribution.

## D LIDAR INTENSITY MEASUREMENT DISTRIBUTION

In this appendix, we follow the same methodology as in Appendix C to study the lidar intensity measurement error model. The return-light intensity is a function of the object’s surface property and the light beam’s incidence angle. The incidence angle is defined as the angle between the normal vector to the surface and the laser beam. Figure D1 shows a quantile-to-quantile plot of 17,500 intensity samples of a retroreflective object at a 70° incidence angle. This example is selected to illustrate the fact that overbounding theory can be used to define a Gaussian error model for discrete measurements of intensity.

Figure D2 shows the mean values (thick lines) and standard deviations (thin lines, solid for 1*σ* envelopes, dashed for 3*σ* envelopes) of intensity measurement overbounding functions for three different surfaces at three incidence angles. The mean values decrease with increasing incidence angle.

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.