A Sliding-Window Filter for Online Continuous-Time Continuum Robot State Estimation

AI summary

Overview

• Research area: Probabilistic state estimation for continuum robots (flexible, small-scale manipulators), specifically continuous-time factor-graph estimation and sliding-window filtering.
• Technical level: Advanced. The paper assumes familiarity with maximum a posteriori estimation, Gaussian factor graphs, Gauss-Newton optimization, the Laplace approximation, Schur-complement marginalization, and continuous-time trajectory representations.
• Scope (one sentence): The paper derives, implements, and validates on a real tendon-driven continuum robot the first stochastic sliding-window filter for continuous-time state estimation, trading a small amount of smoothing and latency for online operation at faster-than-real-time speeds.

What This Paper Is About

Estimating the shape and pose of a continuum robot is a trade-off: recursive filters run online but suffer from linearization errors and Markov approximations, while batch smoothers are accurate but must be run offline because they need the full measurement history. Prior sliding-window work on continuum robots used simplified discrete-time models and produced no stochastic (uncertainty-aware) representation. This paper fills that gap by building a sliding-window filter on top of an existing continuous-time batch estimation framework, so the robot's variable-curvature shape can be estimated online, asynchronously, and with a posterior distribution attached.

Key Contributions

1. The first stochastic sliding-window filter specifically designed for continuum robots. To the authors' knowledge, no prior probabilistic sliding-window estimator exists for CRs; earlier sliding-window attempts used a simplified constant-curvature model or a rigid-link model evaluated only in simulation, and neither provided a posterior distribution.
2. An online continuous-time formulation derived from an existing batch smoother. The window factor graph is built from prior, motion, spatial, boundary-prior, and time-interpolated measurement factors, and the state immediately preceding the window is marginalized into a single effective prior factor so the optimization depends only on in-window variables.
3. A practical filter implementation. The paper details window expansion (with a two-part initialization averaged from the spatial and temporal priors), information-form marginalization via the Schur complement, extraction of the state from the back of the window, handling of covariance interpolation across relinearized windows, and the effect of window size.
4. Real-robot validation with runtime analysis. The estimator is evaluated on five real trajectories from a 3D-printed tendon-driven continuum robot, benchmarked against a filter (window size 0 s) and the full batch method (window size 10 s), with an open-source implementation promised for the community.

Main Findings

• Accuracy improvement over a filter: On the Out-of-Bounds trajectory, the filter's normalized tip position RMSE was 2.38% and the SWF (0.1 s window) achieved 1.88%; on Fast Contact 1.75% versus 1.29%; on Impulse 1 1.80% versus 1.70%; on Impulse 2 2.11% versus 1.44%; on Slow Free Space both were 1.16%. Rotation RMSE followed the same pattern (for example, 0.075 rad versus 0.050 rad on Impulse 2).
• Accuracy comparable to full batch smoothing: The 0.1 s SWF matched the batch method on Fast Contact (1.29%, 0.038 rad), Impulse 1 (1.70%, 0.042 rad), and Slow Free Space (1.16%, 0.042 rad), and was slightly better than batch on Out-of-Bounds position (1.88% versus 2.04%) while slightly worse on rotation (0.042 versus 0.041 rad). On Impulse 2 the SWF was 1.44% versus batch 1.43%.
• Diminishing returns with window size: Sweeping window sizes from 0 s (filter) to 10 s (batch) showed that larger windows generally lower tip position and rotation RMSE, but improvements flatten beyond roughly 0.1 s. Even a 0.033 s window gave significant improvement over the filter baseline, and 0.1 s was chosen as the primary configuration.
• Runtime well within real-time budgets: For the 0.1 s window, average runtimes were 10.2 ms (Out-of-Bounds), 10.2 ms (Fast Contact), 8.7 ms (Impulse 1), 8.9 ms (Impulse 2) and 9.1 ms (Slow Free Space), against 4.4, 4.0, 3.9, 4.0 and 4.0 ms for the filter and 1359, 1379, 1377, 1380 and 1380 ms for the batch method. At a 30 Hz node rate, real-time capability requires runtimes below 33.3 ms; window sizes up to 0.3 s consistently met this.
• A sub-5 ms filter variant: Dropping the continuous-time covariance representation yields a filter algorithm that runs in under 5 ms (over 200 Hz) while still providing a continuous representation of the state mean.
• Consistency measured with NEES: Average NEES was 6.93, 3.56, 5.96, 4.99 and 5.26 for the 0.1 s SWF and 7.60, 3.80, 6.54, 5.27 and 5.64 for the batch method across the five trajectories; the optimal NEES value is 6, the number of DoF in the pose states. NEES could not be reported for the filter because it does not support continuous-time covariance interpolation.
• Out-of-Bounds anomaly: For that trajectory, position RMSE increased when the window grew beyond a certain size. The authors' best hypothesis is that pose measurements degrade severely near the workspace boundary and the batch solution overfits to these poor measurements, whereas a shorter window can lean on the gyroscopes.
• Robustness to measurement dropout: During the Out-of-Bounds trajectory, pose measurements dropped out at 21.5 s; the estimate showed a sudden increase in uncertainty and recovered once measurements resumed.
• Front-of-window extraction behaved unexpectedly: Extracting the state from the front of the window (removing latency) produced no noticeable improvement over the filter method, which the authors find surprising given the Markov property justification; their hypothesis is that they add multiple measurement time steps per new window rather than running estimation once per measurement time as in a traditional iterated EKF.
• Smoothness cost: The method keeps the continuous-time properties of the batch framework but produces less smooth estimates, and the two sources of covariance information at each time step can lead to small discontinuities in the covariance estimate over time.

Methodology in Plain English

The estimator models the robot as a continuous shape over arc length and time, where the state at any point includes pose, velocity, and strain. Relationships between states are encoded as factors in a factor graph: a motion prior derived from an approximate Cosserat rod model, spatial factors linking points along the robot's length, boundary priors, and measurement factors built from tip pose and gyroscope readings. Estimating state means solving a maximum a posteriori problem, which the authors turn into a sequence of linear least-squares problems and iterate with Gauss-Newton until convergence, then read off a covariance using the Laplace approximation.

The sliding-window twist is that only a fixed number of recent time steps are kept in the graph. Each new time step adds new states along the robot, initialized by averaging an initialization consistent with the spatial prior and one consistent with the temporal prior. To keep the window bounded, the oldest states are marginalized out using the Schur complement in information form, which folds their information into a single effective prior on the first in-window state; that locked information is carried forward without re-iterating. At each step the estimate is read from the back of the window, where it has benefited from all in-window measurements, at the cost of latency equal to the window length (at most 0.1 s for this dataset). Window size therefore interpolates between a filter (one time step) and full batch smoothing (all time steps).

Experiments used a 3D-printed tendon-driven continuum robot 46.6 cm long with a 3.6 cm outer diameter, instrumented with two 6-DoF electromagnetic pose sensors at the tip and base and two gyroscopes at the midpoint and tip, with ground-truth pose from an external motion capture system at five points. Data were collected asynchronously at 30-50 Hz. Each estimator discretized the robot into N = 5 states along its length with 30 Hz time steps. All runs were on an Intel i7-13850HX CPU at 3.80 GHz with 64 GB of RAM. Performance was measured as tip pose RMSE (position normalized by robot length), average NEES, and average runtime per time step, with all metrics evaluated through the framework's continuous-time interpolation.

Why This Matters

This work shows that uncertainty-aware continuous-time continuum robot estimation need not be an offline-only luxury. It gives the field a principled middle ground between filters and batch smoothers, keeps the factor-graph formulation that makes the estimator extensible, and demonstrates on hardware that the accuracy of batch optimization is reachable online with compute to spare for downstream planners and controllers.

Real-world applications named in the paper:

• Minimally invasive surgery with flexible small-scale manipulators.
• Industrial inspection and repair in confined spaces.
• Search-and-rescue in disaster areas.

Industry relevance: the extra compute headroom left over from running faster than real time can be directed at controllers, planners, and other processing an end user needs, and asynchronous measurement handling removes the need for pre-integration or extremely high-frequency operation. The authors also note that with lower-frequency, higher-noise sensors, larger window sizes would likely be required to reach similar performance, which is directly relevant to practical sensor selection and cost trade-offs.

Future Directions

• Sensor regime sensitivity: The paper expects window requirements to shift with different sensor configurations, noise profiles, and data rates, since the current data were collected at 30-50 Hz with low noise; this remains to be characterized.
• Explaining the Out-of-Bounds degradation: The unexpected rise in position RMSE with larger windows on that trajectory requires further investigation and validation across other sensor setups.
• The front-of-window extraction puzzle: Why extracting earlier in the window yields no improvement over the filter is unresolved, and the authors expect behavior to "return to expectations" with noisier, lower-frequency data.
• Covariance consistency and smoothness: The relinearization-induced discontinuities from maintaining two sources of variance estimates remain an open practical issue, as does recovering the smoothness that the batch method provides.

Target Audience

Researchers and graduate students in continuum and soft robotics, probabilistic state estimation, and factor-graph or SLAM-style inference who need online, uncertainty-aware shape estimation; control and planning engineers who consume state estimates at high rates; and practitioners working on flexible manipulators for surgery, inspection, or search-and-rescue who need to know what accuracy is achievable in real time and at what computational cost.

Authors’ abstract

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.