On Semidefinite Relaxations for Matrix-Weighted State-Estimation Problems in Robotics

TL;DR

This paper examines certifiable state estimation for robotics when measurement noise is anisotropic and modeled with matrix weights, showing that standard SDP relaxations can lose tightness in localization and SLAM. It derives a theoretical link between posterior uncertainty (Fisher Information) and the SDP dual certificate, and demonstrates that a compact set of redundant constraints can restore tightness, enabling robust global optimization for matrix-weighted Wahba problems and related pose estimation tasks. The authors provide both simulated analyses and real-world experiments (outdoor stereo localization and stereo-SLAM) to validate that redundancy improves relaxation tightness, while acknowledging scalability challenges for large SLAM problems. The findings illuminate how noise geometry influences certification and offer practical guidance for deploying certifiable perception in stereo-based robotic systems, highlighting the trade-off between tightness, redundancy, and computational feasibility.

Abstract

In recent years, there has been remarkable progress in the development of so-called certifiable perception methods, which leverage semidefinite, convex relaxations to find global optima of perception problems in robotics. However, many of these relaxations rely on simplifying assumptions that facilitate the problem formulation, such as an isotropic measurement noise distribution. In this paper, we explore the tightness of the semidefinite relaxations of matrix-weighted (anisotropic) state-estimation problems and reveal the limitations lurking therein: matrix-weighted factors can cause convex relaxations to lose tightness. In particular, we show that the semidefinite relaxations of localization problems with matrix weights may be tight only for low noise levels. To better understand this issue, we introduce a theoretical connection between the posterior uncertainty of the state estimate and the certificate matrix obtained via convex relaxation. With this connection in mind, we empirically explore the factors that contribute to this loss of tightness and demonstrate that redundant constraints can be used to regain it. As a second technical contribution of this paper, we show that the state-of-the-art relaxation of scalar-weighted SLAM cannot be used when matrix weights are considered. We provide an alternate formulation and show that its SDP relaxation is not tight (even for very low noise levels) unless specific redundant constraints are used. We demonstrate the tightness of our formulations on both simulated and real-world data.

Figures (12)

• Figure 1: An example of a local and global minimum for stereo SLAM with 10 poses and 20 landmarks from the "Starry Night" dataset. Ground-truth landmarks are shown as grey stars, ground-truth poses as blue frames, pose/landmark estimates are shown as frames/stars coloured red and green for local and global minima, respectively. Both minima are based on the same set of measurements: stereo pixel coordinates converted to Euclidean coordinates (example shown at the top of the figure) and relative-pose measurements.
• Figure 2: Redundant constraint matrix, $\bm{A}$, found numerically via the method in dumbgenGloballyOptimalState2023a, with colours indicating the relative values of the matrix (teal: 0, yellow: 1, purple: -1). The constraint matrix corresponds to the equation $\bm{x}^T \bm{A} \bm{x} = 0$, where $\bm{x}$ is a subset of variables affected by this constraint.
• Figure 3: Comparison of numerical covariance matrix to the theoretical covariance matrix from Lemma \ref{['lem:FisherInfo']}, $\bm{\Sigma} = (\bm{L}^T\bm{H}\bm{L})^{-1}$, for a two-pose, stereo localization problem, Problem \ref{['opt:Localize']}. Numerical covariance was found by considering the sample covariance of 10,000 (globally optimal) estimates. Note that the covariance matrix is fully coupled because we have included a relative-pose measurement.
• Figure 4: Investigation of the effect of anisotropicity on tightness of semidefinite relaxations for Wahba's problem. Subplot (a) shows how varying anisotropicity affects the uncertainty ellipsoid. Subplot (b) shows the problem setup, with ellipsoids aligned to the $z$-axis of the pose. Subplot (c) shows the tightness boundary for varying numbers of landmarks. Anisotropicity decreases the tightness boundary while increasing number of observed landmarks increases the boundary.
• Figure 5: Extended study of the results in Figure \ref{['fig:ellipsoid_align']} for the 50 landmark case. Heatmaps represent the value of each metric specified in the left-most column, both without (left column) and with (right column) redundant constraints. The presence of the redundant constraints pushes the second certificate eigenvalue closer to the minimum eigenvalue of the FIM, leading to a relaxation that is tight over a larger region of the parameter space. Note that there is a significant heatmap scale difference between (a) and (b) and between (c) and (d).

Theorems & Definitions (10)

• Lemma 1
• Proposition 2
• proof
• proof
• Proposition 3
• proof
• Remark 1
• Definition 4: Noise-free Measurements
• Theorem 5: SDP-Stability of Matrix-Weighted Localization
• proof