Factor Graph-Based Continuum Robot State Estimation with Magnus Expansion Priors

Abstract

Reconstructing the shape of continuum manipulators from sparse, noisy sensor data is a challenging task, owing to the infinite-dimensional nature of such systems. Existing approaches broadly trade off between parametric methods that yield compact state representations but lack probabilistic structure, and Cosserat rod inference on factor graphs, which provides principled uncertainty quantification at the cost of a state dimension that grows with the spatial discretization. This letter combines the strengths of both these approaches by estimating the coefficients of a compact Geometric Variable Strain (GVS) parameterization within a factor graph framework. A novel kinematic factor, derived from the Magnus expansion of the strain field, encodes the closed-form rod geometry as a prior constraint linking the GVS strain coefficients to the backbone pose variables. The resulting formulation yields a compact state vector, is directly amenable to model-based control, and it retains the modularity, probabilistic treatment, and computational efficiency of factor graph inference. The proposed method is evaluated on both simulated and experimental data and compared against a Gaussian Process (GP) regression baseline, demonstrating increased accuracy in partial and sparse measurement scenarios while preserving computational efficiency.

arXiv comment8 pages, 10 figures