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

TL;DR

This work addressing online continuum-robot state estimation introduces a probabilistic sliding-window filter (SWF) that performs continuous-time state estimation within a fixed temporal window. The method builds a factor-graph MAP formulation with Cosserat-rod priors and measurement factors, solving via Gauss-Newton and using the Laplace approximation for covariance, enabling online updates while preserving temporal continuity. The key contributions are the first probabilistic SWF tailored for CRs, a practical window-expansion/marginalization/state-extraction pipeline, and empirical results showing SWF achieving near-batch accuracy with real-time performance on a tendon-driven CR. The approach has practical impact for real-time planning and control of CRs by delivering accurate, uncertainty-quantified state estimates with modest computational requirements, and an open-source implementation is provided.

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.

Figures (6)

• Figure 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.
• Figure 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.
• Figure 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.
• Figure 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).
• Figure 5: Tip estimation results of a SWF with a window size of 0.1s on the Out-of-Bounds trajectory. The mean, 3$\sigma$ 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.