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

The gist: This work presents the first stochastic sliding-window filter specifically designed for continuum robots, which improves accuracy over filtering methods while enabling continuous-time methods to operate online and at faster-than-realtime speeds.

Introduction and Motivation

Continuum robots (CRs) are flexible, small-scale manipulators capable of bending into highly nonlinear shapes and adhering to complex trajectories in confined spaces<ref:2510.26623#pg2>. Accurate localization within their environment is crucial for controlling CRs in applications like minimally invasive surgery, industrial inspection, and search-and-rescue<ref:2510.26623#pg3>. Significant progress has been made in physical modeling of CRs, but open-loop methods suffer from inaccuracies due to unmodeled effects and unknown disturbances<ref:2510.26623#pg4>. While filtering methods are computationally efficient and recursive, approximations introduced during their derivation can lead to biased, suboptimal estimates for nonlinear systems<ref:2510.26623#pg6>. Batch estimation methods offer superior accuracy by jointly optimizing all states retrospectively but are typically not applicable in online settings because only past measurements are available at any given time step<ref:2510.26623#pg7>. Sliding-window filters provide a compromise between filtering and full batch optimization by performing smoothing over a fixed-length time window, improving accuracy while remaining computationally efficient<ref:2510.26623#pg8>.

Estimation Framework

The estimation framework constructed is a factor-graph estimator for estimating the state x of a continuum robot, which includes the pose T (s, t) ∈ SE(3), velocity ϖ(s, t) ∈ R 6, and strain ϵ(s, t) ∈ R 6 along its entire arc length continuously in time<ref:2510.26623#pg2>. The problem is structured as a maximum a posteriori (MAP) estimation problem that can be solved using nonlinear optimization techniques<ref:2510.26623#pg3>. The cost function is formulated such that x∗ = arg max x p(xy, v) ≡ arg min x − X i log(ϕi(x))<ref:2510.26623#pg4>. Each factor ϕi(x) is defined as phi i (x) ∝ exp − 1/2 ei(x) T Σ-1 i ei(x)<ref:2510.26623#pg5>. The prior factors are formulated using an approximate Cosserat rod model, derived from a ‘white-noise-on-acceleration’ motion prior<ref:2510.26623#pg6>. Measurement factors include tip pose measurements and gyroscope measurements<ref:2510.26623#pg6>. This optimization problem is solved by linearizing each cost term into a linear least-squares problem and iterating using the Gauss-Newton method until convergence, specifically HTW−1Hδx = −HTW−1 e(x)<ref:2510.26623#pg7>. Upon convergence, the state covariance is extracted using the Laplace approximation as Σ = HTW−1H −1<ref:2510.26623#pg8>.

Sliding-Window Filter Formulation

The sliding-window filter formulation defines a window with w time steps that includes states xa:b, with b = a + w<ref:2510.26623#pg3>. The joint factorization over the sliding-window is given by p(xa:ky1:k, xa−1) ∝ ϕp(xa−1)ϕb(x0a−1)ϕs(xa−1) × Y k i=a ϕb(x0i)ϕm(xi−1, xi)ϕs(xi)ϕy(xi-1, xi)<ref:2510.26623#pg4>. A marginalized prior factor ψa (xa) is defined to encapsulate all information from before the active window<ref:2510.26623#pg8>. This marginalized term acts as a single prior factor that encapsulates all information from before the active window, allowing the optimization to depend only on variables within xa:k. The formulation highlights three categories of factors: the marginalized prior factor ψa (xa), factors that are only a function of states that vary down the arc length of the robot, and motion factors connecting consecutive time steps within the window.

Filter Implementation Details

The implementation involves several key steps to manage the sliding window.

1. Window Expansion: At each new time step, N new states are initialized in our factor graph, connected to each other via ‘space binary factors’ and to the previous time step via ‘time binary factors’. A principled way to initialize the mean of these states is by averaging two different initializations consistent with the prior factor in space and time.

2. Marginalization: To keep the size of the window bounded, old states are marginalized out using information form via the Schur complement. As the window slides, information associated with only locked states is accumulated and carried forward to future time steps without iteration.

3. State Extraction: The best estimate is extracted from the state at the back of the window, which introduces latency by the period of the window.

4. Window Size: The choice of window size is critical, as setting it to include all time steps results in a batch method, while a single time step results in an iterated filter algorithm.

Experimental Results and Discussion

The results indicate that increasing the window size generally leads to improved tip position and rotation RMSE, with diminishing returns beyond a window size of approximately 0.1s<ref:2510.26623#pg4>. A window size of 0.1s is selected as the primary result because it provides significant improvements over the filter baseline<ref:2510.26623#pg4>. Qualitative results show that the SWF is able to accurately track the tip pose in real-time, even during fast motions and in the presence of occasional pose measurement dropouts<ref:2510.26623#pg5>. The runtime performance shows that window sizes up to 0.3s consistently achieve real-time performance throughout operation<ref:2510.26623#pg8>. The method maintains continuous-time properties at the expense of smoothness in the estimate, and a version running in under 5ms provides access to a continuous representation of the state mean<ref:2510.26623#pg6>. The unexpected increase in position RMSE observed on the Out-of-Bounds trajectory when increasing window size beyond a certain point is hypothesized to be due to measurements near boundaries severely degrading, causing the batch solution to overfit poor measurements<ref:2510.26623#pg8>.

Conclusion

The proposed SWF strikes a balance between estimation accuracy, computational efficiency, and online operation capacity at the expense of introducing a small latency and reduced smoothing compared to the batch approach<ref:2510.26623#pg2>. The method maintains a factor-graph formulation of the estimation problem, which is hoped will lead to further adoption<ref:2510.26623#pg2>. An opensource implementation for this estimator is provided for the community<ref:2510.26623#pg2>.

References

[1] J. Burgner-Kahrs, D. C. Rucker, and H. Choset, “Continuum robots for medical applications: A survey,” IEEE Transactions on Robotics, vol. 31, no. 6, pp. 1261–1280, 2015<ref:2510.26623#pg8>.

[2] X. Dong, D. Axinte, D. Palmer, S. Cobos, M. Raffles, A. Rabani, and J. Kell, “Development of a slender continuum robotic system for onwing inspection/repair of gas turbine engines,” Robotics and ComputerIntegrated Manufacturing, vol. 44, pp. 218–229, 2017<ref:2510.26623#pg3>.

[3] E. W. Hawkes, L. H. Blumenschein, J. D. Greer, and A. M. Okamura, “A soft robot that navigates its environment through growth,” Science Robotics, vol. 2, no. 8, p. eaan3028, 2017<ref:2510.26623#pg4>.

[4] C. Armanini, F. Boyer, A. T. Mathew, C. Duriez, and F. Renda, “Soft robots modeling: A structured overview,” IEEE Transactions on Robotics, vol. 39, no. 3, pp. 1728–1748<ref:2510.26623#pg5>.

[5] T. D. Barfoot, State estimation for robotics. Cambridge University Press, 2024<ref:2510.26623#pg6>.

[6] S. Teetaert, S. Lilge, J. Burgner-Kahrs, and T. D. Barfoot, “A stochastic framework for continuous-time state estimation of continuum robots,” 2025, arXiv:2510.01381<ref:2510.26623#pg7>.

[7] R. J. Roesthuis, M. Kemp, J. J. van den Dobbelsteen, and S. Misra, “Three-dimensional needle shape reconstruction using an array of fiber bragg grating sensors,” IEEE/ASME Transactions on Mechatronics, vol. 19, no. 4, pp. 1115–1126, 2014<ref:2510.26623#pg8>.

[8] F. Stella, C. Della Santina, and J. Hughes, “Soft robot shape estimation with imus leveraging pcc kinematics for drift filtering,” IEEE Robotics and Automation Letters, vol. 9, no. 2, pp.

1. Improved state estimation accuracy for continuum robots by enabling continuous-time methods to operate online all while running at faster-than-realtime speeds. This allows downstream applications like controllers and planners to receive real-time, probabilistic state information even when sensor measurements are asynchronous.

2. Enables real-time, probabilistic shape estimation by utilizing 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. This provides better accuracy than current filtering methods while maintaining computational efficiency necessary for high-speed control.

3. Provides uncertainty quantification through a probabilistic sliding-window filtering approach for CRs which allows the system to extract a mean and covariance estimate, offering continuous representation of the state mean, which is not dependent on the joint covariance between times. This enables systems to make informed decisions based on quantifiable risk during operation.

4. Improves robustness during sensor dropouts by demonstrating that a shorter window filter could better handle the pose sensor dropout by relying on the gyroscopes fully, resulting in a quick recovery after measurements resume, which is crucial for reliable autonomous operation in environments with intermittent sensing.

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.