Anchor-Relative Online Control: Keeping Loop Closures Out of the Disturbance Signal

A robot that builds its map while flying through it has two sources of error that look the same to its controller. The first is physical: a gust or a slip pushes the robot off course. The second is bookkeeping: the mapping system recognizes a place it has seen before (a loop closure) and retroactively corrects where it thinks it has been, so the estimated position jumps. Modern online controllers built for arbitrary, even adversarial, disturbances learn to cancel whatever pushes the state off its predicted path. That makes them vulnerable to this confusion. A loop-closure jump looks to them like a huge, instantaneous gust, and they update their disturbance-cancelling feedback to fit it. Separately, maps built from many small neural fields, each attached to a keyframe, already have a useful property: a loop closure moves the keyframes relative to each other but leaves the geometry inside each local field untouched.

We propose to exploit that property on the control side. The controller's state, goal, and obstacle constraints are expressed relative to the robot's current anchor keyframe instead of a global world frame. Under this choice, a loop closure changes nothing the controller measures locally. It shows up only as a shift in far-away goals and in cross-submap constraints, and that shift can be charged to the "moving safe set" term that time-varying safe online control already accounts for. The physical disturbance estimate stays clean. The short version: express control in the same local coordinates the neural map already uses, and map corrections stop masquerading as wind.

Background and the gap

The mapping side supplies the mechanism. Neural Graph Map (Neural Graph Map, arXiv version) attaches small spherical neural distance fields to keyframes of a pose graph produced by ORB-SLAM2. On loop closure, only the keyframe poses change, and the reconstruction deforms with no neural retraining. Loopy-SLAM (Loopy-SLAM) does the same with keyframe-anchored neural point clouds. NF-Atlas (NF-Atlas), MIPS-Fusion (MIPS-Fusion) and NEWTON (NEWTON) are variants of "locally rigid, globally deformable" maps. All of these are pure perception systems, and Neural Graph Map states plainly that it "uses the SLAM result passively." None of them says what a downstream controller should do when the graph deforms. That silence is the gap we address.

The control side supplies the learning rule. Agarwal et al. (Online Control with Adversarial Disturbances) consider a linear system $x_{t+1}=Ax_t+Bu_t+w_t$, where $x_t$ is the state, $u_t$ the control input, and $w_t$ an arbitrary bounded disturbance. They parameterize control as $u_t=-Kx_t+\sum_{i=1}^{H}M_iw_{t-i}$, where $K$ is a fixed stabilizing gain and the matrices $M_i$ are learned online. Because the state is linear in the $M_i$, convex costs stay convex, and online gradient descent achieves $O(\sqrt{T}\log T)$ regret, meaning cumulative cost within that margin of the best fixed policy in hindsight. Related work improves this to logarithmic regret (Logarithmic Regret for Adversarial Online Control) and removes the need for known dynamics (The Nonstochastic Control Problem).

Zhou and Tzoumas (Safe Non-Stochastic Control) add time-varying safety constraints with Safe-OGD, which projects each gradient step onto a robustly computed safe set. It achieves zero violations and dynamic regret that grows with the total movement $S_T$ of the safe sets. Two assumptions in this literature are the ones we challenge. First, the disturbance is recovered as $\hat w_t = x_{t+1}-Ax_t-Bu_t$ from a state estimate taken at face value. Second, safe sets are known analytically. With a SLAM state estimate, both fail at every loop closure.

Core Claim

In world coordinates, a loop closure at time $\tau$ that corrects the pose by $\Delta_\tau$ injects a spurious term $\Delta_\tau$ into $\hat w_\tau$. The effect lasts for $H$ steps of disturbance memory, and the $M_i$ update chases it. It can also move the world-frame obstacle set by an amount no bounded-disturbance assumption allows.

Suppose instead the state is written as $z_t = T_{a(t)}^{-1}x_t$, the robot's pose relative to its current anchor keyframe $a(t)$ (with $T_a$ that anchor's pose). Suppose also that the local safe set comes from the distance fields attached to anchors near $a(t)$. Then a loop closure leaves $z_t$ and the local constraints unchanged, because it only rewrites the anchor poses $T_a$, and those do not appear in the local quantities. Anchor switches are deterministic coordinate changes computed from the current graph, not disturbances.

What remains is three things. The physical disturbance becomes $\hat w_t$ rotated into the anchor frame. Estimated gravity is fixed by inertial sensing, so corrections are mostly yaw and translation, and we rotate the stored disturbance history by the yaw correction. The goal shifts. So do constraints that span submaps whose relative pose was corrected. Our conjecture, to be proven for the linearized hover model, is that Safe-OGD run in anchor coordinates keeps zero violations with respect to the estimated map. Its dynamic regret should grow with a path length that counts only the corrections to relations between anchors, rather than the full world-frame jumps plus their spurious disturbance echo. The form of this bound comes directly from Safe-OGD's $S_T$ term, restricted to the constraints that actually move.

Research Program

We will build three things. The first is the anchor-relative controller: disturbance-action control with Safe-OGD projection whose state, disturbance buffer, and constraints live in the current anchor frame, plus an explicit transform rule for anchor switches. The second is a constraint extractor that turns the k-nearest local distance fields of a Neural Graph Map–style map into a convex (polytopic) local safe region around the robot. This is done by linearizing the distance-field zero level at sampled directions, with a robustness margin set from the fields' reported reconstruction error. The third is a closed-loop simulator: a linearized quadrotor flying repeated loops through Replica scenes (Replica) rendered in Habitat. The disturbances are drawn from the same six families Zhou and Tzoumas used (Gaussian, Uniform, Gamma, Beta, Exponential, Weibull), plus a temporally correlated gust process. The mapping runs on rendered RGB-D with an ORB-SLAM2 front-end, so drift and loop closures are real rather than injected.

The theory target is modest. We aim for the path-length bound stated above for linear dynamics with a correct anchor transform, and a characterization of when anchor switching itself costs regret: frequent switches near submap boundaries are the obvious failure. We do not claim guarantees with respect to the true world, only with respect to the estimated map plus a margin. Recursive feasibility, which Safe Non-Stochastic Control leaves open, stays open here. We will report how often the projected safe set is empty.

Baselines

The naive reference is a fixed LQR gain in world coordinates with the same SLAM estimate and the same local obstacle constraints enforced by a simple safety filter. It shows how much any online adaptation buys.

The strongest existing method is Safe-OGD (Safe Non-Stochastic Control) run in world coordinates on the SLAM estimate, with safe sets from the same map extractor. It is exactly our controller minus the coordinate choice, and it is the only method in the file with safety guarantees under non-Gaussian disturbances. Its reported numbers are zero safety violations and 9× and 114× speedups over safe $H_2$ and safe $H_\infty$ controllers on quadrotor hover. Those numbers come from full-state hover without perception. There is no reported result found on Lacuna for any nonstochastic controller operating on a neural-SLAM state estimate, so the relevant numbers must be produced here.

Beating Safe-OGD means three things. We need fewer collisions with the ground-truth Replica mesh in the window after loop closures. We need lower cumulative tracking cost. And we need a disturbance estimate whose error, measured against the simulator's true $w_t$, does not spike at closures. All of this must hold at matched memory $H$, step size tuned by the same grid search per method, identical disturbance seeds and trajectories, and identical SLAM runs replayed where possible.

We also include a safe $H_\infty$ controller for the conservatism and runtime comparison. Two ablations isolate the key ingredient. One uses an anchor-relative state with world-frame constraints. The other uses world-frame state with anchor-relative constraints. These separate the disturbance-cleaning effect from the constraint-invariance effect. A monolithic-map variant, using NICE-SLAM (code), tests whether the control benefit depends on the map being decoupled.

Why not just…?

Why not just reset the disturbance buffer when a loop closure fires? This is the most serious threat, because it removes the spurious spike cheaply. It has two costs. It discards $H$ steps of real disturbance memory exactly when gusts may be correlated. It also does nothing about obstacle constraints jumping in world coordinates. We include reset-Safe-OGD as a baseline. If it matches our method on collisions and cost, the honest conclusion is that the contribution reduces to the constraint side, and the second ablation will show whether that alone matters.

Why not just smooth the loop-closure correction over many steps, as many SLAM systems do? Smoothing turns one large false gust into a persistent, slowly varying false disturbance. Disturbance-action control is designed to learn and cancel exactly that kind of signal, so the controller would physically push the robot to "cancel" a bookkeeping correction. We test smoothed-correction Safe-OGD directly and measure the disturbance-estimate error against ground truth over the smoothing window.

Why not just treat the jump as an outlier and clip $\hat w_t$? Small loop closures have the same magnitude as real gusts under heavy-tailed noise, especially Weibull and Exponential. Clipping either misses them or discards real gusts. The anchor-relative formulation needs no threshold because it knows from the pose graph when and by how much a correction happened. Clipped Safe-OGD is run with the threshold tuned on held-out trajectories.

Why not just use $H_\infty$ control, which already assumes worst-case disturbances? It is conservative, and it was 114× slower than Safe-OGD in the only reported comparison (Safe Non-Stochastic Control). It also still consumes the corrupted world-frame state. We include it to confirm this in the perceptual setting.

Decisive Experiments

The first check is open-loop and cheap. We run Neural Graph Map and Loopy-SLAM style pipelines on TUM-RGBD (benchmark), where Loopy-SLAM reports 3.85 cm average absolute trajectory error against 8.92 cm for Point-SLAM (Loopy-SLAM). We measure the distribution of loop-closure correction sizes and compare it with realistic per-step gust displacements. If corrections are negligible relative to disturbances, the problem is not worth solving and we stop.

The second check is the main closed-loop study in Replica, using Replica-Big–style trajectories with repeated loops (Neural Graph Map) across all seven disturbance families. We report ground-truth-mesh collisions, cumulative cost, disturbance-estimate error, and controller runtime, and we plot each metric against the number of loop closures. The proposal predicts that the world-frame baselines' collision rate and cost gap grow with closure count while ours stays flat. If reset or smoothing matches ours on those curves, we will say so.

The third check tests whether the result depends on the decoupled map. We repeat the study with the monolithic map. The prediction is that the constraint-invariance benefit disappears, because a monolithic map must be re-optimized and its local geometry changes at closures.