One of the most common problems I run into during Rescue Sprints with clients building drones or mobile robots is unstable state estimation. A drone starts "swimming" in hover, a robot loses orientation after a few dozen meters, and the logs show a growing drift.
The team's most common reaction is: "Let's tweak the Kalman filter gains." Unfortunately, in 90% of cases the problem lies elsewhere: the filter is just honestly reporting that something earlier in the chain (physics, timing, input data) is broken. Here is my proven path for debugging this class of issues, together with the math worth knowing so you understand why these steps work, not just that they work.
1. Rule out hardware issues (hardware first)
Before you touch the EKF (Extended Kalman Filter) code, make sure the input data isn't corrupted at the physical level. This is the most commonly skipped step, because "the sensor is a name-brand part", yet mounting, temperature and wiring can wreck even the best chip.
- Vibration: Is the IMU properly mechanically isolated? Run the motors without propellers and log raw accelerometer data. If you see aliasing or heavy high-frequency noise, you need better vibration damping or a hardware/software low-pass filter (LPF).
- EMI: Is the magnetometer (compass) too close to high-current wires powering the ESCs? Current spikes generate a magnetic field that corrupts heading readings.
- Power: Check the IMU's power pins with an oscilloscope. Noise on the 3.3V rail feeds directly into MEMS measurement errors.
Aliasing isn't "some noise". It's a direct consequence of the Nyquist-Shannon sampling theorem. If the IMU samples at frequency fs, any signal above the Nyquist frequency gets incorrectly "folded" down into the band you're actually trying to analyse:
A real-world example: drone propellers commonly vibrate in the 800-1500 Hz range (depending on RPM and blade count). If you sample the IMU at fs = 1000 Hz without an analog front-end filter, a 1200 Hz vibration folds back and shows up as a fake ~200 Hz signal, indistinguishable from real vehicle motion.
The second spec worth checking in the datasheet is Noise Density (ND), given in µg/√Hz for an accelerometer. Total RMS noise over a given bandwidth is:
where BW is the low-pass filter bandwidth in Hz. For a typical consumer-grade MEMS IMU (ND ≈ 150 µg/√Hz) and a 100 Hz bandwidth, that works out to roughly 1.5 mg RMS noise, a random error of about 0.015 m/s² on every single measurement, before you've integrated anything.
Practical takeaway: a low-pass filter isn't cosmetic, or "the default value from the example project". It's a direct dial on how much noise you let into your estimator.
2. Check time synchronization (timestamping)
Sensor fusion only works when data from different sensors (IMU, GPS, camera) is precisely synchronized in time. This is one of those bugs that never shows up in static bench tests. It only appears in motion, usually during aggressive maneuvers.
- Are you using hardware interrupts (Data Ready) from the IMU, or polling it in a loop?
- Do the GPS and IMU timestamps use the same reference clock (e.g. the RTOS system timer)?
- A 50-100 ms delay between camera data (visual odometry) and IMU data will break any estimator.
Let's put a number on it. A drone maneuvers at an angular rate of ω = 200°/s (typical for an aggressive racing-drone flip). The orientation error from unsynchronized timestamps grows approximately linearly with the delay:
At Δt = 50 ms of latency, the orientation error is already Δθ ≈ 10°. That's enough for visual-inertial fusion (VIO) to drift apart from reality and generate a positional drift you'll later, unfairly, blame on the Kalman filter, when the real culprit was a missing timestamp.
3. Raw data sanity check
Log the raw data and inspect it in a tool such as Foxglove, PlotJuggler, or a simple Python script.
- Place the device flat on a table. Does the accelerometer read exactly 1g (9.81 m/s²) on the Z axis and 0 on X/Y?
- Does the gyroscope read 0 deg/s on all axes? How large is the baseline noise (bias)?
- Rotate the device exactly 90 degrees around the Z axis. Does the integrated gyroscope output give 90 degrees?
The "flat on a table" test tells you whether a sensor is obviously broken. It doesn't tell you how good it is over long time horizons. That's what Allan Variance analysis is for, a technique borrowed from atomic clock metrology, standardized for IMUs by IEEE (std. 952/647).
The idea, simplified (approximate notation): take a long static gyroscope log, split it into time windows of length τ, compute the variance of the differences between successive window averages, and repeat for many values of τ:
Plot the result on a log-log chart: σ(τ) versus τ. The slope of the curve in different regions tells you directly which noise type dominates:
- slope −1/2 means Angle Random Walk (ARW), the gyroscope's white noise, dominant at short τ
- slope 0 (the curve's local minimum) means Bias Instability, the best achievable stability point
- slope +1/2 means Rate Random Walk (RRW), a slow bias drift, dominant at long τ
As a rough guide: consumer-grade MEMS IMUs run around 0.1-0.3°/√h ARW and 2-10°/h bias instability. Tactical-grade navigation IMUs are already at 0.001-0.01°/√h, two to three orders of magnitude better. That number, not the filter code, is what determines how long a system can fly "blind" (dead reckoning) without a GPS or VIO correction.
If your Allan curve's minimum sits above the manufacturer's spec, you have a hardware problem (mounting, temperature, power), not a software one.
4. Only now look at the Kalman filter
If the hardware, timing and raw data all check out, it's time for the EKF. It's worth understanding not just "what to tweak", but what the filter is actually computing at each step.
- Covariance tuning: Do the measurement noise (R) and process noise (Q) covariance matrices reflect reality? Don't copy them from online tutorials. Compute the variance from logs for your specific sensor, ideally from the Allan analysis above.
- Innovations: In the EKF, "innovations" (the difference between predicted and measured state) are a key indicator. If GPS innovations keep growing and are rejected by the filter, it means your mathematical model has drifted from reality.
The full Extended Kalman Filter cycle is two steps, run every iteration.
Prediction (propagate state and uncertainty forward in time, using motion model f and IMU input u):
Update (correction when a measurement z_k arrives, e.g. from GNSS):
The most underrated part of this equation is the innovation yk and its covariance Sk. Together they give you the filter's consistency test: the Normalized Innovation Squared (NIS), also known as the squared Mahalanobis distance:
With a correctly specified model and R/Q matrices, NIS follows a χ² (chi-squared) distribution with degrees of freedom equal to the measurement dimension. For a 3D measurement (e.g. a GNSS fix) at 95% confidence, the threshold is χ² = 7.815. If your NIS consistently exceeds that threshold, the filter isn't "badly tuned". It's telling you the physical model (f, h) doesn't match reality. It's the most commonly ignored signal in sensor fusion, and one of the most reliable.
5. Why drift is mathematically unavoidable
This is worth understanding at least intuitively, even if you never compute it by hand: even a perfectly implemented EKF running on perfectly synchronized data will still drift if navigation relies on pure dead reckoning (integrating the IMU with no external correction from GPS, VIO, or another absolute reference).
This follows from random walk theory. The position error from doubly-integrated white accelerometer noise doesn't grow linearly with time. It grows approximately with an exponent of 3/2:
This is an order-of-magnitude relationship, not a precise engineering formula, but it captures the essence of the problem: 10 seconds without a GPS/VIO correction isn't "twice as bad" as 5 seconds, it's nearly three times as bad. Understanding that exponent changes how you design failsafes and estimation-validity windows in autonomous systems.
Summary
Debugging sensor fusion is a process of elimination, backed by concrete math at every stage: the Nyquist criterion and noise density at the hardware level, a latency budget for timing, Allan variance for raw-data validation, and the EKF equations plus the NIS test for the filter itself. Start at the physical layer, work through timing and raw data, and leave the filter's full mathematics for last. In that order, 90% of the time, you'll find the problem before you even get there.
If your team is stuck with a drifting estimation, navigation, or sensor integration problem, get in touch. As part of a Rescue Sprint, in 1-2 weeks I diagnose these problems, analyse the logs (including Allan analysis and NIS statistics) and deliver a working fix on your codebase.