Spherical Grid-Based IMU/Lidar Localization and Uncertainty Evaluation Using Signal Quantization

2.1 Quantization Theory

Quantization is a process in which a large number of values is represented by a much smaller set of values (Sayood, 2017). We focus on scalar quantization, where the values to be quantized are scalars. The quantizer design aims at minimizing information loss. Figure 1 shows the quantization of range measurements in a 3D lidar PC. The parameters of a scalar quantizer are shown in Figure 1 and are conceptually represented in Figure 2 on a one-dimensional input axis. These parameters are defined as follows:

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 1

Spherical gridding of a 3D lidar PC

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 2

Scalar quantizer parameters

Input: The quantizer input is a set of values r with an associated probability density function f(r).

Interval: Intervals partition the input into separate ranges of values J = {$J_i$, for i = 1, …, m}. Intervals are also known as Voronoi regions or encoders.

Representation value: The representation value is a single value selected for each interval, corresponding to the output of the quantizer. Representation values are also known as decoders.

Decision boundary: The limit value of an interval is a decision boundary. The number of decision boundaries is m + 1, where m is the number of intervals.

The quantization process Q(r) for an input r can be described as follows:

$d_{i-1}<r<d_i\iff Q\left(r\right)=a_i$1

In this research, the performance measure we use to reduce information loss between quantizer input and output is the quantization distortion D, which is defined as follows:

$D=\frac{1}{M}\sum _{i=1}^m{\sum _{r\in J_i}\left(r-a_i\right)}^2$2

The right-hand side of Equation (2) is the sum of the squared errors in each interval $J_i$ over all m intervals, divided by the total number of range measurements M. The number M can be expressed as $M={\sum }_{i=1}^m{M}_i$, where M_i is the number of range measurements per interval $J_i$, for i = 1, …, m.

The quantization process can be described as follows. Given an input r and a number of intervals m, the quantizer finds the values of d_i and a_i in each interval $J_i$, for i = 1, …, m, that minimize the distortion D. There are two main categories of quantizers: uniform quantizers use fixed-size intervals, whereas non-uniform quantizers use intervals of varying size.

In this paper, we implement a Lloyd–Max quantizer, which is a practical non-uniform scalar quantizer. Lidar range measurements serve as inputs r. The Lloyd–Max quantization algorithm finds the values of a_i and d_i that minimize the distortion by taking the derivatives of D with respect to these parameters and setting them equal to zero. The values of a_i and d_i for a set of inputs r can be found using the following equations (Madisetti, 2018):

$a_i=\frac{1}{M_i}\sum _{r\in J_i}r$3

$d_i=\frac{a_{i+1}+a_i}{2}$4

For a Lloyd–Max quantizer, the representation values a_i are equal to the mean of the input values in interval $J_i$ for i = 1, …, m as expressed in Equation (3), and d_i in Equation (4) is the mid-point of two neighboring representation values. Considering initial values for d_i and a_i, the minimization of D can be iteratively achieved (Madisetti, 2018; Sayood, 2017).

2.2 Spherical Grid Design

In this section, we develop a novel approach that aims to apply quantization techniques borrowed from the signal processing field to lidar PCs. Data points are processed one elevation cone at a time, instead of following the traditional Cartesian-frame representation of a lidar PC. Therefore, the number of azimuth bins can differ from one lidar-frame elevation angle to the next. This gives flexibility for the resulting spherical grid to adjust to the surrounding environment. Figure 3 shows the range versus bearing angle for a single elevation angle (1° elevation in this example) and for a 360° azimuth angle. The raw lidar scan is represented by a thin blue line.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 3

Lidar scanned and quantized range measurements

Then, we use quantization theory to find the optimal decision boundaries d_i and representation values a_i for i = 1, …, m, that minimize the distortion D. These values are represented as horizontal gray dashed lines and solid red lines, respectively, in Figure 3. The vertical solid gray lines show the boundaries of the azimuth bins, which define the varying bearing angle intervals. Original and quantized signal discontinuities (interruptions in the curves) occur in the absence of lidar return measurements. Each interval has a constant a_i value. A change in a_i defines a new azimuth bin, enabling the spherical grid algorithm to (a) dynamically adapt to the environment and (b) increase the number of azimuth cells when variations in range measurements are more frequent. Conversely, the algorithm reduces the number of azimuth bins when fewer variations occur. Multiple non-adjacent bins can be assigned the same a_i value. Figure 4 shows the quantizer parameters and the azimuth bins in a zoomed-in region of the lidar range-versus-bearing-angle curve.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 4

Quantizer parameter definition for lidar range versus bearing angle measurements

Figure 5 shows the output measurements in a Cartesian coordinate system. This output is obtained after minor data trimming to exclude azimuth bins containing fewer than a minimum number of data points, which only increase the computational cost. The solid gray lines intersecting at the origin correspond to the vertical gray lines in Figure 3. In Figure 5, the lidar scan is represented by a blue curve, and the selected points after quantization are shown by red crosses. This representation is consistent with that used for the 3D results shown in Figure 1. The zoomed-in window shows raw azimuth–range measurements in each bin and their corresponding quantized values. In addition, a few elements of the testing environment, including segments of walls, doors, cardboard cylinders, and furniture, are labeled and shown as colored boxes and circles. A photograph of the testbed is displayed later in Figure 9. In Figures 1 and 5, the quantization azimuth bins are large for surfaces on which range measurement variations are small (e.g., walls with no irregularities); these bins are smaller, for example, near the edges of cylindrical objects, where ranging variations are larger compared with their center.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 5

Lidar scan and quantized signals

2.3 Quantization Error

The quantization error is defined as the difference between quantizer input r and output a_i. The quantization error bound b_i for i = 1, …, m is defined as follows (Sayood, 2017):

$b_i=d_{i-1}-d_i$5

The decision boundaries d_i are given in Equation (4). Quantization theory ensures that the following inequality is always satisfied:

$\left(r-a_i\right)<b_i$6

Figure 6 shows quantization errors (blue) and error bounds (red) for all intervals in a frame. The red curve bounds the blue curve and is known.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 6

Quantization error and error bound for 360° measurements

The range measurement quantization error bounding bias vector is defined as follows:

$b_k={\left[\begin{array}{cc}{b}_1\dots b_m& 0_{1\times m}\end{array}\right]}^T$7

where k is the time epoch, b_i is defined in Equation (5), and 0_1×m is an m × 1 vector of zeros. The zeros correspond to bearing angle measurements, which are not quantized but will be included in a lidar measurement vector.

2.4 Applying a Spherical Grid to the Map

We apply the same spherical grid to the mapped PC. We first convert the PC map from the navigation frame to the sensor frame using the predicted state vector $\overline{x}$ provided by the pose estimation algorithm. Then, in each azimuth–elevation bin, we select the mapped point closest to the sensor. This approach automatically addresses occlusions of objects that may not be visible from the lidar’s current viewpoint. Figure 7 shows the lidar and map PCs after quantization, with the azimuth bins shown in gray.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 7

Applying a spherical grid to the map and selecting points

3.1 Vehicle Linearized State Propagation Model

The localization algorithm uses the IMU measurements and error model to propagate vehicle pose between two lidar updates. The IMU states consist of vehicle position, velocity, orientation, and IMU biases. This section describes the continuous-time linearized state propagation model. The complete nonlinear continuous and discrete-time equations and sensor error models can be found in our prior work (Hassani et al., 2019) based on the work by Titterton et al. (2004). In the following equations, the notation “δ” indicates deviations of the state parameters relative to reference values about which linearization is performed. We consider a vehicle body frame “B,” an east–north–up (ENU) navigation frame “N,” and an inertial frame “I.” The state propagation model is expressed as follows:

$\delta \dot{x}=F\delta x+\delta w$8

$\delta x=\left[\begin{array}{ccccc}\delta x_V^T& \delta v_V^T& \delta e_V^T& \delta b_{gy}^T& \delta b_{ac}^T\end{array}\right]$9

$\begin{array}{c}F=\left[\begin{array}{ccccc}0& I& 0& 0& 0\\ F_{\text{H2V}}& 0& \left[{}_{}{}^N\overline{f}\times \right]& 0& C_B^N\\ 0& F_{\text{V2T}}& -\left[{}_{}{}^N\omega {}^{\text{IN}}\times \right]& -C_B^N& 0\\ 0& 0& 0& -{\tau }_{gy}^{-1}I& 0\\ 0& 0& 0& 0& -{\tau }_{ac}^{-1}I\end{array}\right]\\ \delta w=\left[\begin{array}{c}0\\ -C_B^N\left(\left(\delta S_{ac}+\delta M_{ac}\right){}_{}{}^B\overline{f}+C_B^N{v}_{ac}\right)\\ C_B^N\left(\left(\delta S_{gy}+\delta M_{gy}\right){}_{}{}^B\overline{\omega }{}^{\text{IB}}+v_{gy}\right)\\ n_{gy}\\ n_{ac}\end{array}\right]\end{array}$10

where:

δx_V

is the vehicle position expressed in the navigation frame N,

δv_V

is the vehicle velocity with respect to earth expressed in frame N,

δe_V

is the attitude of the vehicle with respect to earth expressed in the body frame B,

δb_gy, δb_ac

are the gyroscope’s and accelerometer’s time-varying bias vectors in frame B, respectively,

F_V2T, F_H2V

are 3×3 matrices defined in Appendix A,

^Nω^IN

is the angular velocity vector of the inertial frame I with respect to frame N expressed in frame N (Hassani et al., 2018),

${}_{}{}^N\overline{f}$

is the estimated specific force expressed in frame N,

$C_B^N$

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

${}_{}{}^B\overline{f}$

is the measured specific force vector at the IMU axis center with respect to frame I, expressed in frame B (Titterton et al., 2004),

^B$\overline{\omega }$^IB

is the measured angular velocity vector of frame B with respect to frame I, expressed in frame B,

τ_gy, τ_ac

are the gyroscope’s and accelerometer’s Gauss–Markov process (GMP) time constants,

S_gy, S_ac

are the estimated gyroscope and accelerometer scale factors in frame B,

M_gy, M_ac

are the estimated gyroscope and accelerometer misalignment matrices in frame B,

v_gy, v_ac

are the gyroscope and accelerometer measurement white noise error components expressed in frame B,

n_gy, n_ac

are the gyroscope and accelerometer GMP time-uncorrelated driving noise vectors.

Considering the discrete-time expressions of the terms in Equation (10) given in Appendix B, the discrete-time realization of Equation (8) can be written as follows:

$\delta x_{k+1}={\Phi }_k\delta x_k+\delta w_k$11

where Φ_k is the state transition matrix between time steps k and k + 1 (Brown & Hwang, 1992).

3.2 Measurement Models

In this section, we derive nonlinear equations for the sensed and mapped PCs designed in Sections 2.2 and 2.4. Let N_m be the total number of mapped points. Their coordinate vector in navigation frame N is expressed as ${}_{}{}^{N}p{}_j={\left[\begin{array}{ccc}{p}_{E,j}& p_{N,j}& p_{U,j}\end{array}\right]}^T$ for j = 1 … N_m. These point coordinates are converted in the sensor frame using the following equations:

$\begin{array}{ccc}{}_{}{}^{S}p{}_j=C_N^S\left({}_{}{}^{N}p{}_j-\overline{x_V}\right)& for& j=1\dots N_m\end{array}$12

where deviations in vehicle position x_v and orientation e_v appear in the state error equation (Equation (9)), ${}_{}{}^{s}p{}_j={\left[\begin{array}{ccc}{p}_{1,j}& p_{2,j}& p_{3,j}\end{array}\right]}^T$ is the mapped data point in sensor frame “S,” and $C_N^S$ is the rotation matrix from frame N to frame S derived using the predicted vector $\overline{e}$_v. We use the notations ${\overline{e}}_V={\left(\begin{array}{ccc}\varphi & \gamma & \psi \end{array}\right)}^T$ and ${\overline{x}}_V={\left(\begin{array}{ccc}{x}_E& x_N& x_U\end{array}\right)}^T$.

After applying the spherical grid to ${}^{S}p{}_{j=1\dots N_m}$, for each elevation range, we define the ranges $\overline{r}$_i and bearing angles ${\overline{\theta }}_i$ of the gridded points, for i ranging from 1 to m, as given below:

${\overline{r}}_i=\sqrt{p_{1,j}^2+p_{2,j}^2}$13

${\overline{\theta }}_i=\arctan \left(\frac{p_{2,j}}{p_{1,j}}\right)$14

We then stack the computed range and bearing angle measurements and define the mapped measurements at time step k as follows:

$\begin{array}{c}{h}_k{\left(\overline{x}\right)}_k={\left(\begin{array}{cc}\overline{r}& \overline{\theta }\end{array}\right)}^T\\ {\begin{array}{cc}\overline{r}={\left(\overline{r_1}\dots \overline{r_m}\right)}^T,& \overline{\theta }=\left(\overline{{\theta }_1}\dots \overline{{\theta }_m}\right)\end{array}}^T\end{array}$15

The lidar range r_i and bearing angle θ_i are provided in the sensor frame. The 2m × 1 sensed measurement vector can be written as shown below:

$z_k=h_k{\left(x\right)}_k+v_k$16

$z_k={\left(\begin{array}{cc}{r}_1\dots r_m& {\theta }_1\dots {\theta }_m\end{array}\right)}^T$17

$v_k={\left(\begin{array}{cc}{v}_{r_1}\dots v_{r_m}& v_{{\theta }_1}\dots v_{{\theta }_m}\end{array}\right)}^T$18

where:

x_k

is the state vector whose elements are defined in Equation (9) for the error states,

v_k

is the 2m × 1 measurement error vector modeled as a vector of zero-mean normally distributed random variables with covariance matrix V_k. We use the notation v_k ~ N(0,V_k). v_r and v_θ are random range and bearing angle measurement errors, respectively.

We linearize Equation (16) about our best pose prediction of the vehicle. The linearized range and angular measurement and measurement error vectors are designated by δr, δθ and v_r, v_θ, respectively. The linearized lidar measurement equation can be written as follows:

${\left[\begin{array}{c}\delta r\\ \delta \theta \end{array}\right]}_k={\left[\begin{array}{ccccc}{F}_{r,x}& 0& 0& 0& 0\\ F_{\theta ,x}& 0& -F_{\theta ,e}& 0& 0\end{array}\right]}_k{\left[\begin{array}{c}\delta x_V\\ \delta v_V\\ \delta e_V\\ \delta b_{gy}\\ \delta b_{ac}\end{array}\right]}_k+{\left[\begin{array}{c}{v}_r\\ v_{\theta }\end{array}\right]}_k$19

where the coefficient matrices F_r,x, F_θ,x, and F_θ,e are determined via the state prediction vector as described in Appendix A.

3.3 Model-Based Estimator Design

We develop an IEKF to tightly integrate lidar and IMU measurements and estimate vehicle pose. A diagram of the IEKF SGL is presented in Figure 8, which includes the spherical gridding and pose estimation processes. In the IEKF pose estimation block in Figure 8, the last term in the state vector estimation equation, together with the integration of high-update-rate IMU data, aims at improving the convergence of the iterative solution. The following variables are used in Figure 8:

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 8

Diagram of the IEKF SGL

${\overline{x}}_k$

is the predicted state vector at time step k,

${\overline{P}}_k$

is the predicted covariance matrix at time step k,

${\hat{x}}_k$

is the estimated state vector at time step k,

P_k

is the estimated covariance matrix at time step k,

H_k

is the observation matrix defined in Equation (19) at time step k,

K_k

is the Kalman gain at time step k,

${\hat{\Phi }}_k$

is the discrete-time state transition matrix at time step k,

c_k

is the expected error due to range quantization at time step k (explained in Section 3.4),

L_q,k

is the estimation error bound for quantile q at time step k (explained in Section 3.4).

3.4 Pose Error Due to Quantization

In this section, we determine the impact of the quantization bias vector on the state estimation error. The expected value of the Kalman filter error in the presence of a bias b_k can be written as follows (Garcia Crespillo, 2022):

$E\left[x_{k|k}\right]=\left(I-K_k{H}_k\right){\hat{\Phi }}_{k}E\left[x_{k-1|k-1}\right]+K_k{b}_k$20

The impact of the quantization bias on the a posteriori estimation of the state vector at time step k is the following (Tanil et al., 2018):

$E\left(x_k\right)=\left(A_{1k}\hspace{0.17em}\hspace{0.17em}\dots \hspace{0.17em}\hspace{0.17em}{A}_{kk}\right)\left(\begin{array}{c}{b}_1\\ ⋮\\ b_k\end{array}\right)=A_k{b}_k$21

where:

$A_{mk}=\left\{\begin{array}{cc}\left({\Pi }_{t=n}^{m+1}\left(I+K_n{H}_n^T\right){\hat{\Phi }}_{k,m}\right)K_m,& \text{if}\hspace{0.17em}m<n\\ K_m& \text{if}\hspace{0.17em}m=k\end{array}\right\}$22

In Equation (21), bias vectors b_i, for i = 1, …, k, are stacked from time step 1 to k in b_K, where K designates time steps 1, …, k. We can express a bound on the impact of the range quantization bias on a state of interest, or a linear combination of states such as the cross-track positioning coordinate for ground vehicle applications, as follows:

$c_k^2=b_K^T\left(A_K^T{\alpha }_k\right)\left({\alpha }_k^T{A}_K\right)b_K$23

where α_k is a vector used to evaluate errors on the state combination of interest. For example, the cross-track error is obtained by projecting the errors in state vector x_k on a unit vector defining the vehicle’s horizontal cross-track direction at time epoch k. Calculating c_k requires that we stack biases and estimator coefficients from time steps 1 to k. Appendix C shows the individual and total impacts of $\left(A_{\mathrm{mk}}\right)b_k$ for m = 1,…, k on c_k which demonstrates that current and recent bias contributions are significantly larger than those in a more distant past. This feature can be exploited in future work to design a sliding time window or a recursive approach to derive c_k in a computationally efficient manner. In addition, the state estimation error variance for the state of interest is given by the following:

${\sigma }_k^2={\alpha }_k^T{P}_k{\alpha }_k$24

The two terms in Equations (23) and (24) contribute to defining estimation error bounds for a desired quantile “q.” The estimation error bound on a state of interest is defined follows:

$L_{q,k}=\lambda \left(c_k+{\kappa }_q{\sigma }_k\right),\hspace{0.17em}\text{with}\hspace{0.17em}\lambda =\{\begin{array}{cc}1& q\ge 50\%\\ -1& q\le 50\%\\ undef.& q=50\%\end{array}$25

where κ_q is a confidence multiplier for a given quantile q: κ_q = Φ⁻¹(q), where Φ⁻¹() is the inverse cumulative distribution function for a standard normal distribution. For example, we can determine the 84% and 16% quantiles of the estimation error bounds as follows:

$L_{84\%,k}=c_k+{\sigma }_k$26

$L_{16\%,k}=-\left(c_k+{\sigma }_k\right)$27

where the impact of the quantization error c_k is accounted for in a worst-contributing manner to guarantee a bound on the actual error quantiles. If c_k = 0, then the range L_16%,k to L_84%,k defines a 1σ (68%) error envelope on the state of interest.

4.1 Indoor Testing for Experimental Validation in a Structured Environment

We first process the experimental data collected using the testbed shown in Figure 9 (Hassani et al., 2019). This setup includes a sensor platform moving on a figure-eight track and equipped with a Velodyne VLP-16 Puck LTE lidar and a NovAtel IMU-IGM-A1. Sixteen Optitrack Prime 13W infrared (IR) motion capture cameras provide sub-centimeter-level positioning by tracking retro-reflective markers fixed on the rover. Vertical cardboard cylinders surround the figure-eight track and occlude each other as the rover passes by. The features of these cylinders are extracted to serve as landmarks for the LL method. In contrast, the IEKF SGL algorithm leverages most of the lidar PC, excluding returns from the lab floor but including returns from walls and furniture as well as cylinders. Testbed settings and parameter values are listed in Table 1.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 9

Overview of the indoor testbed

Figure 10 shows a top view of the true-versus-estimated rover trajectories. The SGL trajectory is shown by a red line, and the LL trajectory is shown by a dashed green line. The trajectories match most of the time. The map used for LL comprises the center-point coordinates of the six cylindrical landmarks shown by black circles. In contrast, the map employed for SGL uses the PC represented by blue crosses. This map is built via lidar measurements transformed from the lidar frame to the navigation frame using “ground truth” pose estimates provided by the IR camera-based motion capture system. A color-coded background is used to facilitate the interpretation of subsequent figures over time: the upper loop’s background is shown in white, the lower loop in light gray, and the straight segments in dark gray. The initial position of the rover is shown by a black cross.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 10

Performance of the IEKF SGL for a single-lap estimation of rover position

Figure 11(a) shows cross-track SGL errors over time for experimental data collected over 80 laps. The sample cross-track positioning error is color-coded in shades of gray, from white to black as the rover travels from the first to the last lap. Slight differences in performance are observed in the early laps as compared with later laps because of the sensor system’s warm-up period. The blue envelope represents the error bounds in Equations (26) and (27). These bounds are represented by an area whose boundaries are the minimum and maximum L_84%,k and L_16%,k values over the 80 laps at trajectory time k. The solid red line indicates the empirical 68% error envelope (i.e., the 84% and 16% sample quantiles), which is bounded by the analytical blue envelope for most of the 22-s-long trajectory. The looseness of the blue bound with respect to the red bound indicates our limited knowledge of the spherical grid quantization error, which we are only able to bound.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 11

IEKF SGL errors and error bounds for the rover’s (a) cross-track and (b) heading angle estimation performance

Figure 11(b) shows error bounds for the rover’s heading angle estimation. In this case, again, the analytical envelope bounds the sample error envelope for most of the trajectory. In addition, the difference between the blue area and red line for the heading error is smaller than that of the cross-track error because the range quantization primarily impacts the rover’s position states whereas the impact on the heading error is due to coupling of the rover’s states. The lidar’s bearing angle resolution error and the rover’s heading prediction error provide the main contributions to the analytical error envelope of the heading angle.

In Table 2, we compare the performance of LL, BF SGL (Hassani & Joerger, 2021), and IEKF SGL. For each approach, the table presents the cross-track error bounds derived from the maximum-over-the-trajectory of the 16%–84% quantile deviation. LL and SGL involve different sources of uncertainty and therefore have different error bounds. For LL, which is not affected by gridding errors, the error bound L_LAT accounts for localization errors and for the risk of incorrect associations, as derived in Appendix D. For BF SGL, the error bound is optimistic because we have no means to account for gridding errors. For IEKF SGL, the error bounds L_84%,k and L_16%,k are given in Equations (26) and (27).

The maximum error bound for LL is 5 cm: LL requires FE and DA, which introduce additional risks (Hassani & Joerger, 2021; Hassani et al., 2019). These risks grow if landmarks are not well separated or are not easily recognizable (LL is challenging outdoors). In this indoor test, the experimental setup favors LL over SGL. The BF SGL error bound is also 5 cm. BF SGL does not require FE or DA, but it only provides a 1-sigma bound, with no means to account for gridding errors, resulting in an underestimated localization error. Finally, the IEKF SGL provides a maximum 84% quantile bound of 26 cm: in this case, spherical gridding errors are accounted for.

Table 2 also presents the single-lap computation time, which was evaluated using a 4-GHz processor with 32-GB RAM on a 64-bit Windows operating system. The longest computation time was observed for BF SGL. The computation time is determined by the size and resolution of the vehicle pose candidate search space and of the fixed-size azimuth–elevation grid. The automated grid size and resolution selection of IEKF SGL reduces the computation load to half that of BF SGL. The LL has the lowest computation time because it only processes a small subset of data points corresponding to extracted landmarks.

In the last column of Table 2, the root mean square error (RMSE) at the final time step of a single lap is shown for each algorithm. The RMSE values for BF SGL and IEKF SGL are similar, whereas LL yields a smaller error. Note that the RMSE may vary for different laps. In this work, we have modeled the spherical gridding error analytically and considered its contribution to the localization error bound. However, it is important to recognize that the RMSE is not an ideal metric for comparison in this context, as our experimental process is not ergodic. For an ergodic process, time-domain (such as RMSE) and ensemble characteristics are equivalent, typically requiring constant or periodic dynamics. This condition does not apply to our experiments, where changes in the rover’s linear velocity and sensor behavior, such as the sensor warm-up phase discussed earlier, result in non-ergodic characteristics.

4.2 Outdoor Testing in a Realistic Automotive Environment

Figure 12 displays an outdoor sensor platform that includes a NovAtel IMU-IGM-A1, a NovAtel ProPack6 global navigation satellite system (GNSS) receiver and antenna, and an Ouster OS1-64 lidar. The sensor platform is mounted on the roof rack of a car. A differential code and carrier phase GNSS reference station is installed at a nearby pre-surveyed location. Sensor data are collected on a laptop computer using the Robotic Operating System (ROS). We use ROS to time-tag and record sensor data for post-processing. The “truth” vehicle position and orientation are determined from NovAtel’s synchronous position, attitude, and navigation solution, which tightly integrates IMUs with a carrier phase differential GNSS. This section evaluates the performance of the IEKF SGL algorithm using lidar/IMU; the test settings and lidar/IMU parameter values are listed in Table 3.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 12

Overview of outdoor test equipment

Figure 13(a) displays a 3D PC map of the test site, where the height is color-coded: measurements between 2-m and 18-m heights were included. Figure 13(b) shows the test site, which corresponds to a parking lot on Virginia Tech’s Blacksburg campus. The a priori PC map is rough, which will accentuate localization errors and the need for realistic error bounds while achieving submeter-level accuracy. The map uses a limited subset of lidar measurements converted from the moving sensor frame to the ENU navigation frame using the truth solution.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 13

3D PC map used in the outdoor experiment

Mapping errors include deviations in the reference “truth” solution, imperfect GNSS/IMU/lidar synchronization, errors caused by non-stationary objects (including vegetation), and poor, noisy, and inconsistent returns from surfaces with varying retro-reflectivity properties, particularly those we observed through glass windows. In addition, in this first outdoor experiment, we did not implement motion compensation: we did not account for vehicle motion over the 0.1-s-long 360° lidar scans. We will consider improving the map in future work; such improvements are not essential in this paper because the emphasis is placed on error bounding rather than error reduction. Further information on PC motion compensation can be found in the work by McDermott and Rife (2024a). The standard deviations of the range and bearing angle measurements in Table 3 were selected to incorporate mapping errors.

Figure 14 shows a top-view of the rover trajectory for a single lap in the parking lot. Vehicle pose is estimated using lidar and IMU data processed via the IEKF SGL algorithm. The true and estimated trajectories overlap. The initial position of the vehicle is indicated by a black cross. Figure 14 also shows magenta covariance ellipses that roughly represent the 2D positioning uncertainty for vehicle locations taken at regular 0.5-s intervals. The covariance ellipses are inflated by a factor of thirty to facilitate visualization. These ellipses underestimate localization errors because they do not account for gridding errors.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 14

Performance of lidar/IMU-based IEKF SGL for a vehicle during testing in an outdoor environment

Figure 15(a) shows the cross-track error over time. The sample cross-track positioning error is shown by black cross markers. The blue curves represent the upper and lower IEKF SGL error bounds L_84%,k and L_16%,k in Equations (26) and (27), respectively. The black sample error curve is bounded by the blue sub-meter analytical bounds over the 41-s-long trajectory. The magenta line represents the 1σ (68%) error covariance envelope, which grossly underestimates localization errors by not considering the uncertainty caused by gridding. Figure 15(b) shows the error, error covariance envelope, and error bounds for the vehicle heading angle. In this case, again, the blue analytical IEKF SGL error bounds better represent the black sample errors than the magenta covariance envelope.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 15

IEKF SGL error bounds for the vehicle’s (a) cross-track positioning error and (b) heading angle estimation error

In Figure 15, the increase in cross-track position and heading angle estimation errors occurs as the vehicle turns, as highlighted by a dashed white ellipse in Figure 14. During this time period, the errors due to a lack of motion compensation are accentuated by the vehicle’s rotation and are significantly higher compared with other segments of the trajectory. This effect will be mitigated in future work by implementing motion compensation (Li et al., 2016; McDermott & Rife, 2024a; Meng et al., 2021; Ollero et al., 2004). Furthermore, it is important to note that the error curves represent a single sample of a random process. A larger data set, similar to what was used in the indoor experiment, would enable a more statistically meaningful comparison. However, collecting such statistically significant data outdoors is often impractical. Therefore, we performed this comparison using our indoor testbed.

In addition, a smaller difference between the blue and magenta error covariance envelopes is observed for the heading angle as compared with the cross-track deviation. This result occurs because range quantization primarily impacts the vehicle’s position states. Its impact on heading estimation is indirect, through the coupling of the vehicle and heading angle states in matrix $A_K^T$ in Equation (23). Additionally, the RMSE values for cross-track and heading errors are 0.28 m and 1.6°, respectively. However, this experiment is also non-ergodic, as explained for the indoor test, owing to variations in the vehicle’s linear and angular velocities (driven manually) and changes in sensor behavior.