PaperlayerView exact revision on arXiv

Source-linked AI summary

A Stochastic Framework for Continuous-Time State Estimation of Continuum Robots

Spencer Teetaert, Sven Lilge, Jessica Burgner-Kahrs, Timothy D. Barfoot

arXiv:2510.01381v1cs.RO

TL;DR

Continuum-robot estimation has lacked probabilistic methods that represent smooth state continuously across both time and arclength while accounting for uncertainty. This paper introduces a stochastic factor-graph framework that fuses asynchronous sensor measurements; experiments report accurate estimates across motions and dropout, with approximately real-time mean-only solves at 30Hz. The method’s measurement-interpolation independence assumption can make estimates overconfident, especially at lower estimation-node rates.

  • Problem

    Probabilistic continuum-robot methods are often discrete-time or quasi-static, leaving a gap in smooth estimation across time and arclength with explicit uncertainty.

    Source: 2) Probabilistic Methods · p. 2

  • Method

    A stochastic factor-graph framework combines a physically motivated prior over space and time with asynchronous sensor measurements and interpolation.

    Source: B. Contributions · p. 3, 3) Interpolation of Measurement Factors · p. 7

  • Results

    The estimator maintains tip errors below 2% across trajectories with pose measurements, and mean-only solves reach approximately real-time rates of 30Hz.

    Source: C. Dynamic Trajectories · p. 11, §V · p. 14

  • Takeaways & Limitations

    The framework provides a flexible foundation for adding sensors and factor-graph methods to continuum-robot state estimation.

    Source: §VI · p. 14

    Assuming interpolated measurement states are independent can cause overconfidence, an effect that worsens at lower estimation-node rates; real-time covariance computation remains future work.

    Source: §V · p. 14

Abstract

from arXiv · show

State estimation techniques for continuum robots (CRs) typically involve using computationally complex dynamic models, simplistic shape approximations, or are limited to quasi-static methods. These limitations can be sensitive to unmodelled disturbances acting on the robot. Inspired by a factor-graph optimization paradigm, this work introduces a continuous-time stochastic state estimation framework for continuum robots. We introduce factors based on continuous-time kinematics that are corrupted by a white noise Gaussian process (GP). By using a simple robot model paired with high-rate sensing, we show adaptability to unmodelled external forces and data dropout. The result contains an estimate of the mean and covariance for the robot's pose, velocity, and strain, each of which can be interpolated continuously in time or space. This same interpolation scheme can be used during estimation, allowing for inclusion of measurements on states that are not explicitly estimated. Our method's inherent sparsity leads to a linear solve complexity with respect to time and interpolation queries in constant time. We demonstrate our method on a CR with gyroscope and pose sensors, highlighting its versatility in real-world systems.

I. INTRODUCTION

Continuum-robot state estimation must address model inaccuracies and noisy sensing while representing robot shape over time and arclength. The paper combines probabilistic continuous estimation across both dimensions with a framework intended to fuse sensor data and handle dynamic motion and dropout.

I. INTRODUCTION

  • Existing continuum-robot models can be inaccurate under unmodelled effects and unknown contact forces, motivating state estimation that combines sensor feedback with model prediction.Real-world measurements are also noisy.Source: §I · p. 1

1) Non-probabilistic Methods:

  • Non-probabilistic approaches fit kinematic, static, dynamic, or learned representations to measurements, including curve models, mechanics-based models, and neural networks.Examples include constant-curvature fitting, Cosserat-rod dynamics, and physics-informed networks.Source: 1) Non-probabilistic Methods · pp. 1–2

2) Probabilistic Methods:

  • Probabilistic methods include discrete-time filters over simplified shape representations and arclength-based methods that estimate a continuous shape at a single time.The latter include smoothing and Gaussian-process approaches.Source: 2) Probabilistic Methods · p. 2
  • A gap remains in probabilistic estimation of smooth continuum-robot state continuously over both time and arclength while accounting for noise and uncertainty.Existing variable-curvature methods are largely quasi-static, while dynamic methods rely on detailed modeling and are non-probabilistic.Source: 2) Probabilistic Methods · p. 2

B. Contributions

  • The proposed physically motivated Gaussian-process prior unifies continuous-time and continuous-arclength estimation in one framework.The method is evaluated with multiple sensor modalities and is reported to handle dynamic effects and data dropout.Source: B. Contributions · pp. 2–3

A. Notation

  • The paper uses pose transformations expressed in the robot body frame and omits frame subscripts when they are not needed.The notation follows Chapter 8 of.Source: A. Notation · p. 3

B. Setup

The estimator represents pose, strain, and velocity as a stochastic state over arclength and time. Its prior approximates simplified rod dynamics by injecting white noise into derivative terms, with measurements intended to compensate for modeling errors.

B. Setup

  • The state x(s, t) comprises pose T, body-centric strain ϵ, and body-centric velocity ϖ, and the framework estimates its posterior distribution from a prior and measurements.The state depends on both arclength and time.Source: B. Setup · p. 3

C. Approximate Dynamics

  • The prior is an approximate factor-graph representation rather than a closed-form Gaussian process, and the paper reports reasonable estimates in practice.The model simplifies dynamic Cosserat-rod dynamics under the assumption that data can compensate for modeling errors; white noise is injected on second-order derivative terms and strain rate.Source: B. Setup · p. 3, C. Approximate Dynamics · pp. 3–4

D. Prior Factors

The prior is assembled from factors linking estimated states across time and arclength, with Euler approximations and weighted errors. The spatial factor uses an approximated covariance, and its noise treatment simplifies the problem but relaxes a consistency relationship.

D. Prior Factors

  • The framework explicitly estimates states on discrete arclength and time grids, then interpolates between them to represent the state continuously.The prior is a sum of factors designed to approximate the system dynamics.Source: D. Prior Factors · p. 4

1) Time Factors:

  • Time factors use an Euler step in which pose advances according to velocity while strain and velocity remain constant, and penalize deviation from this approximate progression.The error is zero when the state follows the approximate dynamics; the weighted cost uses an analogous covariance approximation.Source: 1) Time Factors · p. 4

2) Arclength Factors:

  • Arclength factors use an Euler step in which pose advances according to strain and strain remains constant; their covariance is approximated by linearizing the nonlinear consistency term.The spatial covariance depends on velocity. The method treats strain-change-over-time and velocity-change-over-arclength noise as independent samples, although the consistency constraint relates them; the authors say this simplifies sparsity and improves numerical stability without greatly affecting performance.Source: 2) Arclength Factors · pp. 4–5

3) Unary Factors:

Unary factors encode prior knowledge or measurements at selected states, while a grid of neighboring space-time states forms the sparse factor graph. Measurement factors can incorporate sensor readings, including pose and gyroscope data.

3) Unary Factors:

  • Unary factors encode prior knowledge or measurements at a specific arclength and time, including initial states, boundary conditions, and partial state information.The base receives a full unary factor at each timestep rather than being locked, allowing small mounting disturbances.Source: 3) Unary Factors · p. 5

4) Factor Graph:

  • The factor graph uses an N-by-K grid with NK estimated states and N(2K − 1) factors, each connecting only immediate neighbors.A base boundary condition is applied along s = 0; the local connections introduce sparsity.Source: Figure 2 · p. 5, 4) Factor Graph · p. 6

E. Measurement Factors

  • Measurement factors relate sensor readings to the state and allow pose-tracker and gyroscope measurements to be included.The paper presents models for both modalities.Source: E. Measurement Factors · p. 6

1) Pose Measurements:

Pose and gyroscope measurements are modeled with sensor noise, while interpolation accommodates asynchronous measurements between the states explicitly estimated. The framework also interpolates state covariances.

1) Pose Measurements:

  • Pose measurements are modeled as noisy observations of the robot’s true pose, with pose-sensor covariance used to form the measurement error and cost.The pose error is expressed in the Lie algebra.Source: 1) Pose Measurements · p. 6

2) Gyro Measurements:

  • Gyroscope measurements are modeled as noisy observations of angular velocity, with a sensor-noise covariance used in their measurement model.The framework also supports other modalities when their measurement model and Jacobian are known.Source: 2) Gyro Measurements · p. 6

F. Interpolation

  • Asynchronous timestamps require interpolation because estimation is performed only at a discrete set of arclength-time states.Sensor arclengths can generally align with estimation points, but measurement times need not.Source: F. Interpolation · p. 6

1) Interpolation in Time:

  • The method interpolates states in time and computes interpolated covariances after the main solve using the Laplace approximation.The time interpolation scheme follows the approach in.Source: 1) Interpolation in Time · pp. 6–7

2) Interpolation in Space:

The framework interpolates states and measurements between estimation nodes, then solves a sparse factor-graph optimization problem. Its batch solver uses a banded system to keep solve complexity linear in the number of time steps.

2) Interpolation in Space:

  • Spatial interpolation uses a consistency-constraint approximation, producing linear interpolation of velocity along arclength and spline interpolation of pose.Arclength interpolation follows the time-only interpolation formulation.Source: 2) Interpolation in Space · p. 7

3) Interpolation of Measurement Factors:

  • Interpolation Jacobians and the chain rule connect continuous-time measurement models to the states at the surrounding estimation nodes.This permits measurements within the estimation window to enter the factor graph at times not explicitly estimated.Source: 3) Interpolation of Measurement Factors · p. 7
  • The measurement interpolation assumes intermediate states are independent, which can make multiple measurements contribute to overconfidence; smaller Δt minimizes this effect.The authors report reasonable estimates for most practical choices of Δt.Source: 3) Interpolation of Measurement Factors · p. 7

G. Solver

  • A Gauss–Newton solver iteratively minimizes the prior and measurement costs, and the final iteration uses a Laplace approximation for posterior covariance.Relevant state variances and correlations are extracted using the information matrix sparsity in O(N^3K) time; the estimator is a batch solver.Source: G. Solver · pp. 7–8
  • The Cholesky factor’s banded sparsity yields solve complexity O(N^3K), and total posterior computation including matrix construction is O(N^3K + M).The band width scales with the number of spatial nodes N, so solve complexity is linear in time steps K.Source: Figure 4 · p. 8, G. Solver · p. 8

III. APPARATUS

The simulation evaluates the estimator on a fully extensible, three-segment tendon-driven robot using noisy pose measurements at segment tips.

A. Simulation

  • The simulated robot is a fully extensible three-segment tendon-driven continuum robot of unitless length 1, with three tendons per segment.Each segment can extend from zero to one third of the robot’s full length.Source: A. Simulation · p. 9
  • Noisy pose measurements are simulated at the tip of each segment, with Gaussian white noise on SE(3).Source: A. Simulation · p. 9
  • The extensible trajectory bends while fully extended, contracts to zero length, then extends into a new fully extended state.Source: A. Simulation · p. 9

B. Prototype

The prototype is a sensor-equipped two-segment continuum robot designed for ground-truth comparison. Its sensor streams are processed and calibrated before estimation errors are assessed.

B. Prototype

  • The two-segment prototype was designed as a state-estimation testbed, prioritizing sensor capacity and ground-truth comparison over size and weight.Source: B. Prototype · p. 9

1) Robot Design:

  • The inverted robot is 466 mm long and 36 mm in diameter, with a hollow nitinol backbone and two tendon-actuated segments.Each segment uses two opposing tendons.Source: 1) Robot Design · p. 9

2) Sensors:

  • Nine sensors comprise five 15 Hz Vicon frames, two 50 Hz electromagnetic pose sensors, and two 30 Hz gyroscopes.The pose sensors are at the tip and base; gyroscopes are at the tip and midpoint.Source: 2) Sensors · p. 9

3) Post-Processing:

  • Sensor data undergoes outlier rejection and time synchronization, and hand-eye calibration has about 5 mm average reprojection error that contributes significantly to reported estimator errors.The calibration uses data from eight static robot configurations.Source: 3) Post-Processing · p. 9

C. Estimator Parameters

Estimator behavior is tuned through process and measurement noise, then evaluated for consistency and pose accuracy. In simulation, the estimator smooths abrupt motion, while comparison with a quasi-static GP shows temporal smoothing and reasonable estimates during sensor-data dropout.

C. Estimator Parameters

  • Process-noise matrices determine expected uncertainty across state dimensions, so parameters should be tuned individually for each robot; measurement noise may be increased to counter overconfidence.The reported estimator settings use 17 spatial nodes (19 in simulation) and 30 Hz time nodes; mean estimates solve a 10-second batch in under 10 seconds, while explicit covariance estimation adds overhead.Source: C. Estimator Parameters · pp. 9–10

D. Benchmarks

  • The benchmark restricts the method to K = 1 and removes velocity to approximate the quasi-static GP, using only pose measurements because that method cannot incorporate gyro-rates.Both methods use matching noise parameters in the spatial dimension.Source: D. Benchmarks · p. 10

E. Error Metrics

  • Evaluation reports position and orientation errors against asynchronous Vicon ground truth, querying the estimator at matching timestamps to avoid time-alignment errors.Ground truth provides only 6DoF pose, so velocity and strain errors are not reported.Source: E. Error Metrics · p. 10

1) Estimator Consistency:

  • Average NEES assesses covariance consistency: for 6DoF pose, 6 is consistent, values above 6 indicate overconfidence, and values below 6 indicate underconfidence.Source: 1) Estimator Consistency · p. 10

2) Pose Accuracy:

  • Tip-pose accuracy uses mean distance, with position and orientation MAE; body-pose errors use the same metrics averaged over four motion-capture frames, alongside tip and body RMSE.Source: 2) Pose Accuracy · p. 10

A. (Simulation) Robot Extension

  • The ‘Extensible’ simulation yields 0.195% average tip error and 0.400% average body error, with an average NEES of 7.605 indicating slightly overconfident covariance.The estimate smooths sudden velocity changes, briefly failing to contract fully to zero length; similar spatial smoothing is reported in and.Source: A. (Simulation) Robot Extension · pp. 10–11, Figure 6 · p. 11

B. Comparison to 1D GP Prior

  • On ‘Out-of-Bounds’, the proposed method smooths sensor-noise jitter and produces a reasonable estimate with increased uncertainty when the tip leaves the sensor workspace.It also produces a more confident estimate when data are available by leveraging information at other times; accuracy cannot be compared directly because the quasi-static method lacks time interpolation.Source: B. Comparison to 1D GP Prior · p. 11

C. Dynamic Trajectories

Across dynamic trajectories, the estimator maintained reasonable state estimates under external contact and pose-data dropout. The interpolation and discussion sections describe discretization effects and limitations in covariance computation and uncertainty under strong contact.

C. Dynamic Trajectories

  • During ‘Fast Contact’, the estimator maintained reasonable state estimates under unmodelled forces, with 1.162% average tip error and 3.851% average body error.Strong forces may cause overconfidence because GP smoothing can make estimates less responsive to sudden strain or velocity changes.Source: C. Dynamic Trajectories · p. 11
  • Across trajectories, tip errors stayed below 2% with pose measurements, and adding gyroscope measurements improved or preserved tip estimates, especially during pose-data gaps.On ‘Out-of-Bounds’, the proposed and quasi-static methods had similar mean estimates when measurements were available, but the quasi-static method could not produce a reasonable estimate when data were absent.Source: C. Dynamic Trajectories · p. 11, Figure 7 · p. 12

D. Interpolation Validation

  • Spatial interpolation showed slight changes in mean and covariance as node count increased; coarser discretization produced greater deviation from a more finely discretized solution.For N = 5, average tip and body errors were 0.11% and 0.38%; for N = 9, they were 0.06% and 0.21%.Source: D. Interpolation Validation · p. 14, Figure 11 · p. 14

V. DISCUSSION

  • The framework provides uncertainty-aware estimates that can inform sensor placement and collection rates, but its tuning was overconfident in some higher-contact scenarios.The authors suggest users may wish to err on the side of overconfidence when high-impact scenarios are expected.Source: §V · p. 14

C. Dynamic Trajectories

  • The implementation scaled linearly with time nodes and cubically with spatial nodes, while mean-only solves ran at approximately 30 Hz with tip error below 2%.The runtime analysis estimates only the mean; the linear time scaling is attributed to problem sparsity.Source: C. Dynamic Trajectories · p. 13, Figure 10 · p. 13, §V · p. 14

V. DISCUSSION

  • Interpolated-state independence assumptions have greater impact at lower estimation-node rates; approximately real-time solves at 30 Hz apply when computing only the mean.The authors leave computationally efficient real-time covariance computation to future work and note that the WNOA Cosserat-rod model may reduce accuracy relative to more sophisticated models.Source: §V · p. 14

VI. CONCLUSION

  • The framework extends continuous-time estimation to continuum-robot trajectories in space and time, fusing asynchronous measurements from multiple sensor modalities with uncertainty awareness.The authors provide an open-source implementation.Source: §VI · p. 14
  • The factor-graph formulation is presented as a foundation for adapting mobile-robotics methods and adding sensors or factor-graph algorithms such as localization, mapping, and sliding-window filtering.The authors describe the framework as, to their knowledge, the first stochastic continuous-time state-estimation framework for continuum robots.Source: §VI · p. 14

APPENDIX A

Appendix A sets notation for values and objects and specifies the paper’s pose-transform convention. Appendix B outlines linearizations for time, space, unary, pose-measurement, and gyro-measurement errors.

APPENDIX A

  • Mean, posterior, and prior values use bars, hats, and checks; scalars, vectors, and matrices use lower-case, bold lower-case, and bold upper-case notation, respectively.These conventions follow.Source: Appendix A · pp. 14–15
  • Pose transforms denote the inertial-frame pose relative to the body frame, and body-frame kinematics use the left Jacobian of SE(3) and act on the transform’s left side.The appendix notes that this convention differs from many works in continuum robotics.Source: Appendix A · p. 15
  • When subscripts are omitted, the text uses a parenthesized superscript number to denote an object’s index when needed to avoid conflicting indices.Source: Appendix A · p. 15

APPENDIX B JACOBIANS

Interpolation Jacobians

The appendix describes how perturbations of interpolated states relate to estimation-state perturbations and specifies a block-selection matrix. It also lists experiment parameters, including a gyro covariance setting.

Interpolation Jacobians

  • The time-interpolation Jacobians represent perturbations of an interpolated state with respect to perturbations of the estimation states.Spatial interpolation Jacobians are said to take a similar form.Source: Interpolation Jacobians · p. 15
  • The selection matrix P_i is a 3 × 1 block matrix whose ith column is identity, with each block sized 6×6.P_*1 selects the first block of a block vector containing six blocks of size 6.Source: Interpolation Jacobians · p. 16

APPENDIX C PARAMETERS

  • The cited material identifies parameters used in the real-world experiments and states the gyro covariance as Rgyro = diag{1 × 10^-6, 1 × 10^-6, 1 × 10^-6}.Source: Experiment Parameters · p. 16
Loading 2510.01381v1…