High-order regularization dealing with ill-conditioned robot localization problems

TL;DR

The paper tackles ill-conditioned robot localization by introducing a high-order regularization (HR) framework that expands the inverse of the normal matrix $A^T A$ via a finite matrix-series with a PSD regularization matrix $R$, such that the HR solution is $\hat{x}^k_{hr} = (A^T A+R)^{-1} \sum_{i=0}^k (R(A^T A+R)^{-1})^i A^T b$ and TR is recovered at $k=0$. By ensuring $\rho\left(R(A^T A+R)^{-1}\right)<1$, HR achieves better inverse-approximation than classical Tikhonov regularization and mitigates over-smoothing; an a priori criterion guides the selection of $R$ and the order $k$ to balance numerical stability against estimation bias. The paper also introduces bias-correction techniques, including a sliding-window approach, to obtain nearly unbiased HR estimates and demonstrates substantial RMSE improvements and consistent NEES in both simulations and real 3D UWB localization experiments. The results show HR outperforms LS, TR, OFTR, and TSVD across varying ill-conditioning levels, offering a principled, explainable regularization framework with practical closed-form guidance for $R$ and potential extensions to nonlinear models and ML contexts.

Abstract

In this work, we propose a high-order regularization method to solve the ill-conditioned problems in robot localization. Numerical solutions to robot localization problems are often unstable when the problems are ill-conditioned. A typical way to solve ill-conditioned problems is regularization, and a classical regularization method is the Tikhonov regularization. It is shown that the Tikhonov regularization is a low-order case of our method. We find that the proposed method is superior to the Tikhonov regularization in approximating some ill-conditioned inverse problems, such as some basic robot localization problems. The proposed method overcomes the over-smoothing problem in the Tikhonov regularization as it uses more than one term in the approximation of the matrix inverse, and an explanation for the over-smoothing of the Tikhonov regularization is given. Moreover, one a priori criterion, which improves the numerical stability of the ill-conditioned problem, is proposed to obtain an optimal regularization matrix. As most of the regularization solutions are biased, we also provide two bias-correction techniques for the proposed high-order regularization. The simulation and experimental results using an Ultra-Wideband sensor network in a 3D environment are discussed, demonstrating the performance of the proposed method.

Figures (8)

• Figure 1: Illustration of robot localization problem using anchors.
• Figure 2: The performance of the high-order method with $R$ only changing the smallest eigenvalue of $A^TA$ and $\mu^2=(\lambda_n^2+\lambda_n \lambda_1)^{1/2} (s=n-1, k=1)$, the OFTR method and the LS method. The bias of the HR solution is corrected in real-time according to the LS solution. The result of the OFTR solution is provided by the FTR method whose regularization parameter $\mu^2=(2\lambda_1 /\lambda_n)^{1/2}$ is given by the a priori criterion proposed in this paper for $s=n-1,k=0$, which only changes the smallest eigenvalue of the matrix $A^TA$. (a) Total RMSE. (b) RMSE in different dimensions.
• Figure 3: A given route to simulate the robot moves in a real 3D environment.
• Figure 4: The performance of the high-order method with $R$ only changing the smallest eigenvalue of $A^TA$ and $\mu^2=\lambda_{n-1}(s=n-1,k=1)$, the TSVD method and LS method. The bias of the HR solution is corrected in real-time based on Algorithm \ref{['alg:sliding_window_bias']} by using the latest $l$ measurements with a sliding window. The TSVD method truncates the smallest singular value with respect to the z component. In this simulation, the robot moves along a given route. (a) Total RMSE. (b) RMSE in different dimensions.
• Figure 5: The platform in our experiment. The white box on top is the UWB sensor that measures the distances between the robot and the anchors, and the LiDAR under the white box is used to provide the reference positions of the robot.

Theorems & Definitions (15)

• Lemma 1
• proof
• Lemma 2
• proof
• Proposition 1
• proof
• Lemma 3
• proof
• Theorem 1
• proof