Stochastic state estimation methods for continuum robots (CRs) often struggle to balance accuracy and computational efficiency. While several recent works have explored sliding-window formulations for CRs, these methods are limited to simplified, discrete-time approximations and do not provide stochastic representations. In contrast, current stochastic filter methods must run at the speed of measurements, limiting their full potential. Recent works in continuous-time estimation techniques for CRs show a principled approach to addressing this runtime constraint, but are currently restricted to offline operation. In this work, we present a sliding-window filter (SWF) for continuous-time state estimation of CRs that improves upon the accuracy of a filter approach while enabling continuous-time methods to operate online, all while running at faster-than-real-time speeds. This represents the first stochastic SWF specifically designed for CRs, providing a promising direction for future research in this area.
Figures & tables
Fig. 1: An example CR state estimation scenario where sensor measurements are produced asynchronously in time. The SWF in this work uses a small window of time to jointly optimize states within the window, improving accuracy over filter-based approaches while enabling online operation, something previous batch methods cannot do.
Fig. 2: Factor graph representation of the sliding-window filter: The window is initially expanded to incorporate the next discrete time step. Once the new state is included, the oldest time step is marginalized out, maintaining a fixed window size while propagating information forward.
Trial
Tip RMSE
Avg. NEES
Avg. Runtime
Pos (%)
Rot (rad)
(ms)
Out-of-Bounds
Filter (0s)
2.38
0.045
N/A
4.4
SWF (0.1s)
1.88
0.042
6.93
10.2
Batch (10s)
2.04
0.041
7.60
1359
Fast Contact
TABLE I: Proposed estimator and baseline performance summary
Fig. 3: Tip position and rotation RMSE across each of the five experimental trajectories for different window sizes. A window size of 0s corresponds to the filter baseline, while a window size of 10s corresponds to the batch optimization baseline. In general, larger window sizes lead to lower RMSE values, with diminishing returns as the window size grows past 0.1s. Notably, even small window sizes (e.g., 0.033s) provide significant improvements over the filter baseline, indicating that incorporating a short history of states can substantially enhance estimation accuracy.
Fig. 4: Side-by-side comparison of the proposed SWF with a window size of 0.1s (bottom) and the Out-of-Bounds experimental trajectory (top). The region where pose data is lost is highlighted in red on the left of the frames. The current time of each frame is provided on a visualized timeline. The SWF is able to accurately track the tip pose of the robot in real-time, even during fast motions and in the presence of occasional pose measurement dropouts. Sudden increases in uncertainty are observable when the pose measurements are lost, but the filter quickly recovers once measurements resume (see rightmost frames).
Fig. 5: Tip estimation results of a SWF with a window size of 0.1s on the Out-of-Bounds trajectory. The mean, 3 σ uncertainty bounds, and ground truth values collected from the motion capture system are shown for both position and orientation. The SWF demonstrates comparable performance to the batch optimization method, though has less smooth estimates. Notably, when the pose measurements drop out at 21.5s, the estimate contains a sudden increase in uncertainty and displays the expected discontinuity in uncertainty growth when the next measurement is recieved.
Fig. 6: Window runtime distributions for different window sizes. Larger window sizes lead to higher runtimes due to their larger state size. The long tails on each distribution corresponds to different numbers of iterations required for convergence. Each window’s state size is selected such that the estimated nodes are generated at a frequency of 30 Hz, as such, each window should ideally have a runtime below 33.3 ms to maintain real-time performance.
Continuum robots often operate in uncertain environments, where accurate state estimation is essential for safe interactions. Estimate uncertainty is inherently spatially non-uniform: confidence varies depending on where measurements are available. Global estimation accuracy is not always the top priority, but rather achieving sufficient confidence at task-relevant locations along the robot. This extended abstract introduces mechanically reconfigurable sensing enabling uncertainty-shaping in state estimation for continuum robots. We present a concept hardware design demonstrating the feasibility of longitudinal translation of a sensor within a continuum robot. We demonstrate that state estimation confidence can be reconfigured by varying the sensor location, and show a reduction of full-body shape estimation errors when sliding the sensor back and forth over time, compared to a single fixed tip sensor.
Ella Walsh, Spencer Teetaert, Eric Diller +2
University of Toronto Robotics Institute, Toronto, ON, Canada
Recent factor graph approaches to continuum robot state estimation have been successful for quasi-static applications and spatiotemporal estimation using white-noise kinematic motion priors. However, when inertial effects are significant, these approximations may fail to capture the underlying physics, limiting accuracy during dynamic motions. In contrast, our approach approximates the Cosserat rod dynamics of continuum robots. We write inertia and damping as equivalent applied loads, so that the dynamic balance retains the algebraic form of the static one from prior work with quasi-static robots. Without backbone observations, the framework reduces to a stochastic forward simulation of the robot's motion. Given observations, it jointly refines kinematic and dynamic states and infers external loads, among other states. We validate the approach through simulation and experiments, demonstrating stochastic forward simulation as well as state estimation on tendon-driven continuum robots.
James M. Ferguson, Tucker Hermans, Alan Kuntz
Department of Electrical and Computer Engineering and Department of Computer Science, Vanderbilt University, TN, USA. · Kahlert School of Computing and Robotics Center, University of Utah, UT, USA.
In this paper, we present a fully discrete approach for the accurate and numerically efficient dynamical modeling and state estimation of continuum robots. The model is based on geometrically exact beams in a minimal, strain-based formulation and derived in the framework of Lie group variational integrators, allowing to preserve important geometric properties that we exploit to achieve high accuracy and numerical efficiency. We then propose a disturbance observer based on an extended Kalman filter formulation that reliably estimates system states as well as model uncertainties and external disturbances. Experiments on a real system validate the accuracy and efficiency of the proposed model and observer.
Maximilian Herrmann, Leander Pfeiffer, Paul Kotyczka
Technical University of Munich, TUM School of Engineering and Design, Munich Institute of Robotics and Machine Intelligence (MIRMI), Chair of Automatic Control.