Sensor Fusion 2.0: Tighter Integration, Better Robustness
Evolving our sensor fusion architecture for V2 - tighter coupling, failure prediction, and graceful degradation.
V1's sensor fusion, the stack I sketched in 2016, was good enough to ship, which is not the same thing as robust. For V2 we went back through the tracking failures the field telemetry handed us, counted them, and rebuilt the fusion stack around what actually goes wrong in the field.
V1 Fusion Analysis
The failure analysis turned up clear patterns:
Failure modes:
- Visual deprivation (33%): Too few features, low light, motion blur
- IMU issues (22%): Saturation, vibration, bias jumps
- Synchronization (18%): Timing drift between sensors
- Initialization (15%): Bad startup state
- Environmental (12%): Dynamic scenes, reflections
A third of all failures come down to the cameras simply not seeing enough. That number shaped most of what follows.
V2 Fusion Architecture
Tighter Visual-Inertial Coupling
V1 ran a loosely coupled EKF. V2 moves to a tightly coupled factor graph, and the switch buys several things at once: relinearization handles nonlinearity better, measurement types are easy to add or remove, delayed measurements fit in naturally, and the explicit constraint graph makes debugging far less miserable.
I wrote in 2017 that we were implementing tight coupling because the accuracy gain was worth the complexity. What shipped in V1 was the loosely coupled filter. The argument was right; V1 shipped without it, and V2 is where it gets cashed.
Multi-Frame Feature Tracking
V1 matched features frame to frame. V2 maintains long-term feature tracks across 10+ frames, which means more constraints per optimization window, better handling of momentary occlusions, and implicit loop closure within the window for free.
Predictive Failure Detection
We stopped waiting for failure and started predicting it. Feature count trending downward means slow down and gather more data. IMU variance spiking means trust vision more. Lighting changing rapidly means increase the exposure adaptation rate. Underneath sits a state machine with confidence levels:
Normal → Degraded → Recovery → Normal
↓
Emergency (IMU-only)
Graceful Degradation
When fusion quality drops anyway, the fallback is staged:
- Level 0: Full 6DoF, mm accuracy
- Level 1: Full 6DoF, cm accuracy (reduced features)
- Level 2: 3DoF orientation only (visual failure)
- Level 3: IMU propagation only (temporary)
- Level 4: Lost (requires reinitialization)
The user sees a smooth experience through most degradations and usually never learns anything happened.
Implementation Challenges
Computational Cost
Factor graphs cost more than an EKF. We claw the cost back with sparse solvers (Cholesky on sparse matrices), incremental updates in the iSAM2 style, and marginalization of old states. The target is a CPU load similar to the V1 EKF despite the richer model, and early profiling says it's reachable.
Tuning Complexity
The richer model also means more knobs: measurement noise models for each sensor, prior weights, outlier thresholds, window sizes. Hand-tuning that many parameters doesn't scale, so we're building automated tuning infrastructure on top of logged data from V1 devices, ground truth from motion capture, and optimization over the tuning parameters themselves.
How Do You Test Robustness?
Four ways. Challenging datasets covering low light, fast motion, and dynamic scenes. Fault injection that simulates sensor failures. Long-duration stress tests. And an edge case library assembled from V1 failures in the wild.
Every V1 failure report becomes a regression test for V2.