Rigid Body Localization via Gaussian Belief Propagation with Quadratic Angle Approximation

Gaussian belief propagation (GaBP) is a technique that relies on linearized error and input-output models to yield low-complexity solutions to complex estimation problems, which has been recently shown to be effective in the design of range-based GaBP schemes for stationary and moving rigid body localization (RBL) in three-dimensional (3D) space, as long as an accurate prior on the orientation of the target rigid body is available. In this article we present a novel range-based RBL scheme via GaBP that removes the latter limitation. To this end, the proposed method incorporates a quadratic angle approximation to linearize the relative orientation between the prior and the target rigid body, enabling high precision estimates of corresponding rotation angles even for large deviations. Leveraging the resulting linearized model, we derive the corresponding message-passing (MP) rules to obtain estimates of the translation vector and rotation matrix of the target rigid body, relative to a prior reference frame. Numerical results corroborate the good performance of the proposed angle approximation itself, as well as the consequent RBL performance in terms of root mean square errors (RMSEs) in comparison to the state-of-the-art (SotA), while maintaining a low computational complexity

💡 Research Summary

The paper addresses the problem of estimating both the translation and orientation (pose) of a rigid body in three‑dimensional space using only range measurements between known anchor nodes and the body’s landmark points. While recent work has shown that Gaussian belief propagation (GaBP) can solve this “rigid body localization” (RBL) problem with modest computational effort, those approaches rely on a small‑angle linearization of the rotation matrix. The small‑angle approximation (sin θ≈θ, cos θ≈1) is only accurate for rotations below roughly 10–20°, and performance degrades sharply when the true orientation deviates more strongly from the prior.

To overcome this limitation, the authors propose a novel quadratic angle approximation for the rotation matrix. Instead of a first‑order Taylor expansion, each sine and cosine term is approximated by a second‑order polynomial:

• sin θ ≈ α θ_{i‑1}θ_i + β θ_i
• cos θ ≈ γ − δ θ_{i‑1}θ_i

The coefficients (α, β, γ, δ) are chosen by minimizing the mean‑square error over the interval θ∈