Bounded-Error Attitude Smoothing on $\mathbb {SO}(3)$ for UAV Navigation via Structured ADMM

Abstract

Batch attitude smoothing is important for reconstructing uncrewed aerial vehicle (UAV) trajectories from high-rate sensor measurements, especially when uncertainty information is available in the form of predefined bounds rather than Gaussian covariance models. This article formulates attitude smoothing on $\mathbb {SO}(3)$ as a bounded-error optimization problem with geodesic tubes defined around measured attitudes. To solve the resulting nonconvex formulation, we develop a sequential convexification framework with a structured alternating direction method of multipliers solver that exploits the block-banded temporal smoothness model. Unlike conventional smoothers based on soft residual penalties, the proposed approach explicitly evaluates nonlinear tube compliance after manifold retraction. A local analysis shows that the mismatch introduced by linearization is bounded by $O(\Delta ^{2})$ under bounded trust-region steps. Therefore, the method provides a local bound on tube excess rather than an unconditional strict-feasibility guarantee. The proposed solver is evaluated on synthetic benchmarks and EuRoC MAV datasets. Compared with a generic second-order cone programming formulation, it reduces the computational cost by $6\times$ to $15\times$ for batch sizes from $M=100$ to 500 and remains applicable to larger scale problems with $M=1000$. On five EuRoC Machine Hall sequences, it achieves attitude errors of 0.012–0.017 rad rms while maintaining nonlinear tube feasibility rates of 94%-96%. These results demonstrate the effectiveness of the proposed bounded-error smoothing framework for UAV attitude reconstruction under explicit uncertainty constraints.