Planning for the Estimator You Will Have: Saturation-Aware Safety After Motion Capture Is Removed

Most agile drone controllers that come with safety guarantees are demonstrated with motion capture. Motion capture is a room full of external cameras that reports the robot's exact position and velocity hundreds of times per second. Once those cameras are removed, the robot has to estimate its own state from onboard cameras and inertial sensors. That estimate, called visual-inertial odometry (VIO), drifts. It lags. It degrades sharply when the camera sees a blank wall, gets blurred by fast motion, or is disturbed near a ceiling. A safety filter is a small optimizer that minimally edits commands to keep the robot inside a safe region. It has to add a margin for this estimation error. When the motors are near their thrust limits, a large enough margin leaves no feasible command at all. Our central claim is that this margin cannot be supplied reactively by the fast inner loop. The slow planning layer has to supply it ahead of time, by predicting how bad the onboard estimate will become along each candidate trajectory and slowing down before the robot gathers momentum it cannot shed.

Put in two sentences: we learn a model that forecasts VIO error from the planned motion and the scene ahead, using motion capture only as a training label. The planner then limits speed and acceleration so that the low-level safety filter, which is bounded by motor limits and inflated by that forecast, provably keeps a feasible solution. What is new is where the conservatism sits. It lives in the planner and is computed in advance from predicted perception quality. It does not live in the filter as a fixed or after-the-fact buffer.

Why the existing pieces do not compose

Four lines of work each solve part of this problem under assumptions the others break.

The first handles motor limits in safety filters but assumes accurate state. Optimized Control Invariance Conditions for Uncertain Input-Constrained Nonlinear Control Systems tunes the barrier decay rate offline so the filter never demands impossible inputs. It also uses an ensemble's mean and spread to build a probabilistic safety filter, solved as a second-order cone program, and tests it on a Crazyflie. Its uncertainty is uncertainty in the dynamics model, and its state comes from external tracking. This paper also supplies the constraint that shapes our design. Online-adaptive barrier methods recognized the need to intervene too late, and saturation then made recovery impossible. That result rules out "inflate the margin when covariance rises" as a complete answer.

The second line splits control into layers but assumes noise-free state. Multi-rate Control Design under Input Constraints via Fixed-Time Barrier Functions pairs a 5 Hz planner that bounds how fast the reference may change with a 10 kHz input-limited inner filter, and proves periodic safety. Its state feedback is perfect. A Predictive and Sampled-Data Barrier Method for Safe and Efficient Quadrotor Control does the same for quadrotors, with compensation between sample instants, in simulation only. We reuse the Garg et al. structure, in which the planner bounds the reference rate and the inner loop certifies safety. What we change is that the rate bound is set from forecast estimation error rather than fixed.

The third line makes planning aware of what the controller can actually do. Quad-LCD learns tracking error from trajectory coefficients over 200,000 simulated rollouts and penalizes trajectories that would saturate the motors. It flies with motion capture and lists vision-based navigation as future work. We borrow its methodological move of learning a feasibility quantity from trajectory parameters. We apply it to estimator error, and we feed the result into a hard certificate rather than a soft cost.

The fourth line removes motion capture but gives no guarantee. Deep Drone Acrobatics flies acrobatics zero-shot from feature tracks and inertial data, with no safety filter. Its authors note that it fails in low-texture scenes, which is exactly the regime we target. SimpleFlight identifies what makes reinforcement-learning policies transfer from simulation to hardware: collective-thrust-and-body-rate commands, action smoothness, and selective domain randomization. It still relies on motion capture. Decentralized NMPC with limited detection range is the closest work that respects both onboard sensing limits and thrust bounds. It handles limited range with worst-case relative-velocity bounds and does not model stochastic perception error.

Core Claim

Let $h(x)$ be a barrier function, which is nonnegative exactly on the safe set. Let $\hat{x}$ be the VIO estimate of the true state $x$. Let $L$ be a Lipschitz constant of $h$, meaning a bound on how fast $h$ can change as the state changes. If the estimation error satisfies $\|x-\hat{x}\|\le \bar e$ with high probability, the inner filter must enforce the tightened, input-limited condition

$$\sup_{u\in\mathcal{U}} \Big[\nabla h(\hat{x})^\top f(\hat{x},u) + \gamma\big(h(\hat{x}) - L\bar e\big)\Big] \ge L_f\,\bar e .$$

Here $\mathcal{U}$ is the set of commands the motors can produce, $f$ is the dynamics, $\gamma$ is the decay rate, and $L_f$ bounds how much the dynamics term itself changes under an error of size $\bar e$.

Two observations follow from this condition. First, the set of estimated states where it remains satisfiable shrinks as $\bar e$ grows, and it shrinks faster at high speed, because $\mathcal{U}$ is bounded. Second, the error bound $\bar e(t)$ is not stationary. It rises predictably when the trajectory points the camera at a featureless region, passes close to a reflective surface, or exceeds a blur-inducing angular rate.

The planner therefore solves its trajectory problem with an added constraint. Along the plan, each predicted estimated state must lie in the feasibility set computed for the error that is forecast at that time. If the forecast is valid, the inner filter's problem remains feasible at every step, which carries over the recursive-feasibility argument of Garg et al. to noisy, saturating flight. The probabilistic forecast can be converted into a finite-horizon failure bound in the style of Freedman-inequality DTCBFs. That approach needs per-step variance, which is exactly what our forecaster outputs, and it avoids the order-of-magnitude overestimation of risk that the same paper shows for worst-case input-to-state-safety bounds.

Research Program

The work proceeds in three stages.

The first stage is the error forecaster. We fly a VIO-equipped quadrotor in a motion-capture arena. Motion capture provides labels only. The arena contains scenery we can reconfigure: textured panels, blank panels, a low ceiling, and dim zones. We log estimator error together with the planned trajectory segment, the predicted camera view direction, the angular rate, and a cheap scene descriptor computed onboard, such as the current feature count and the feature density in the direction of travel. A small network predicts a mean and an upper quantile of error over the next planning horizon. We calibrate it with held-out split conformal prediction so that the coverage claim is checkable. We supplement the real logs with rendered-camera VIO runs in OmniDrones, and we test whether simulation pretraining reduces the amount of real data needed. Simulated VIO error is not expected to match reality exactly. Calibration on real data is what carries the guarantee.

The second stage is the feasibility map. For the quadrotor's thrust and body-rate limits, we compute offline, in the manner of Brunke et al., the largest speed at each barrier level for which the condition above remains satisfiable, as a function of $\bar e$. This produces a lookup table, $v_{\max}(h,\bar e)$, that the planner can query quickly.

The third stage is the layered controller. A 5–10 Hz trajectory optimizer receives the forecast and the feasibility table as constraints. Below it, an input-limited barrier filter tightened by $\bar e$ runs at 200+ Hz on top of a collective-thrust-and-body-rate tracking controller, following the SimpleFlight interface. If the realized error exceeds the forecast quantile, the filter falls back to a braking command. We log every fallback as a forecast failure.

Baselines

The naive reference point is the same geometric tracking controller fed by VIO, with no safety filter. Its planner uses the speed limits that work under motion capture. This shows how much removing external localization actually costs.

The strongest existing method on this question is the input-constrained probabilistic filter of Brunke et al.. We run it with a fixed estimation margin, chosen as the 95th-percentile VIO error over the entire training log. We also run a version tuned per environment, choosing the smallest fixed margin that achieves zero violations on held-out training flights. The file gives no reported numerical result on Lacuna for this method's hardware violation rate.

The second strong competitor is Quad-LCD, retrained on our logs. Its learned penalty predicts tracking error measured by motion capture during VIO-driven flight, and it receives the same scene descriptors as our forecaster, so it has every input our method has except the certificate. Under motion capture it reports a 6% crash rate in aggressive flight, against 41% and 54% for its geometric-controller baselines. Beating it means fewer true-state violations at matched completion time on VIO flights in mixed-texture scenes, and no increase in crash rate on fully textured scenes.

All learned components use the same flight logs, the same compute, and the same hyperparameter search budget.

Two ablations isolate the key ingredient. The first, a reactive ablation, keeps our filter and feasibility table but sets $\bar e$ from the estimator's current covariance instead of the forecast. The second keeps the forecaster but drops the feasibility table and simply caps speed at a forecast-scaled constant.

Why not just…?

Why not just train an end-to-end policy with domain randomization over estimator noise? Policies trained this way, as in SimpleFlight, transfer well under motion capture. That same paper warns that randomizing every parameter indiscriminately hurts transfer. Structured VIO failure, such as sudden loss of features behind a blank wall, is hard to randomize faithfully, and the resulting policy offers no checkable guarantee. We include such a policy, trained in OmniDrones with injected drift and dropout, as an extra arm. If it matches our violation rate in the mixed-texture scenes, the certificate is not paying for itself, and we will report that.

Why not just use a fixed worst-case margin? This is the real threat. If VIO error were roughly stationary, a well-tuned constant margin would capture nearly all of the benefit. Two pieces of evidence argue against it. Cosner et al. find that worst-case bounds overestimate risk by orders of magnitude. The research file also notes that worst-case tightening under input bounds frequently empties the feasible set. Neither result is on VIO data. The comparison against the per-environment-tuned fixed margin settles the question directly. We measure the completion-time cost at zero violations. If that cost is within 10% of ours, we abandon the forecasting claim.

Why not just inflate the margin in the fast filter when the estimator's covariance rises? This is the reactive ablation. Brunke et al. show that adapting late fails once momentum and saturation make recovery impossible, which predicts that this ablation will produce more filter-infeasibility events specifically when a featureless region appears mid-maneuver at speed. VIO covariance is also known to be overconfident during drift. If the reactive ablation matches the full method, the anticipation claim is false.

Proposed Experiments

We first run a controlled study in safe-control-gym using the 2D quadrotor. We inject structured estimation error with drift, latency, and dropouts that switch on in spatial zones, and we sweep thrust limits. This maps where infeasibility appears for each baseline. Next we run rendered-camera VIO experiments in OmniDrones. Hardware experiments follow. The platform is a quadrotor with an onboard compute board of the VOXL 2 class used by Goarin et al.. The tasks are the SimpleFlight figure-8 and its fast variants, Quad-LCD-style aggressive trajectories, a gap-passing task beside a blank wall, and a low-ceiling corridor in the spirit of Kocer et al.. Motion capture stays on for evaluation only.

The metrics are true-state safety violations, filter infeasibility events, crashes, completion time, and mean tracking error. For the forecaster we also report calibration as empirical coverage against the nominal level.

The decisive checks are three. First, the forecaster's coverage on held-out scenes must be within a few points of nominal, since otherwise the guarantee is void. Second, on trajectories that cross from textured to featureless regions at high speed, the full method must reduce infeasibility events relative to the reactive ablation. Third, at zero violations, it must finish meaningfully faster than the tuned fixed-margin filter. Failing the second check falsifies the claim that the planner must lead; failing the third falsifies the claim that forecasting buys anything over simple caution.