An SE(3) Noise Model for Range-Azimuth-Elevation Sensors

TL;DR

This work addresses the misrepresentation of nonlinear $RAE$ sensor noise in scan matching by formulating a nonlinear $SE(3)$ noise model that naturally captures banana-shaped uncertainty envelopes. It extends this framework to incorporate sensor-to-vehicle extrinsics and odometry uncertainty, yielding a cohesive submap-level covariance that better reflects real measurement uncertainty. The final compound model combines measurement, extrinsic, and trajectory uncertainties via adjoint mappings on matrix Lie groups, enabling more robust data association and weighting in pose estimation. Through both simulation and field data from underwater laser scanning, the approach demonstrates more realistic and consistent uncertainty envelopes, with practical implications for robust submap formation and scan matching in challenging environments.

Abstract

Scan matching is a widely used technique in state estimation. Point-cloud alignment, one of the most popular methods for scan matching, is a weighted least-squares problem in which the weights are determined from the inverse covariance of the measured points. An inaccurate representation of the covariance will affect the weighting of the least-squares problem. For example, if ellipsoidal covariance bounds are used to approximate the curved, "banana-shaped" noise characteristics of many scanning sensors, the weighting in the least-squares problem may be overconfident. Additionally, sensor-to-vehicle extrinsic uncertainty and odometry uncertainty during submap formation are two sources of uncertainty that are often overlooked in scan matching applications, also likely contributing to overconfidence on the scan matching estimate. This paper attempts to address these issues by developing a model for range-azimuth-elevation sensors on matrix Lie groups. The model allows for the seamless incorporation of extrinsic and odometry uncertainty. Illustrative results are shown both for a simulated example and for a real point-cloud submap collected with an underwater laser scanner.

Figures (8)

• Figure 1: Reference frames and datums for the problem. From left to right, the labels indicate the world frame, the vehicle frame, and the sensor frame (and their respective datums), as well as the measured point $p$. The first basis vector ${\underrightarrow{{m}}}^1$ of the measurement-aligned frame is also indicated.
• Figure 2: A simple motivating example on $SE(2)$, showing a top-down view of the vehicle from \ref{['fig:frames_and_datums']} as it moves from left to right. At each time step the vehicle records a single range-bearing measurement of a wall. To perform scan matching, all measurements must be combined into a submap and therefore must be expressed relative to a single vehicle pose, arbitrarily chosen here to be $\mbf{T}^{z_1w}_{ab_1}$. The noise profile of the aggregated submap should reflect (a) measurement uncertainty, (b) sensor-to-vehicle extrinsic uncertainty, and (c) odometry uncertainty accumulated throughout submap formation.
• Figure 3: Parameterizing the range-bearing measurement ${\boldsymbol{y} = \left\lbrace r, \theta_{m\ell} \right\rbrace }$ as an element of $SE(2)$. The adjoint matrix then provides a straightforward mapping between the sensor uncertainty $\boldsymbol{\Sigma}^s_m$ and the measurement uncertainty $\boldsymbol{\Sigma}^p_m$. The measurement-aligned frame is also shown.
• Figure 4: Illustrating the 99.73% (3$\sigma$) confidence envelopes for simulated range-bearing measurements (left) and range-azimuth-elevation measurements (right).
• Figure 5: Uncertainty envelopes for the submap-level noise model \ref{['eqn:noise_meas_model_SE3_full_cov']}. A vehicle moves from left to right, periodically recording range-bearing measurements of a wall at ${y = 2m}$. When the individual measurements are combined into a submap about a single "central" pose, the resulting uncertainty envelope should incorporate measurement uncertainty, sensor-to-vehicle extrinsic uncertainty, and odometry uncertainty. The top row shows 99.73% uncertainty envelopes for different selections of the central submap pose (highlighted), while the bottom row shows the four sets of envelopes superimposed on selected measurements.