Sensor Fusion & Kalman Filtering: The Ultimate Guide

Every sensor on a robot lies a little, and each lies in its own way. A gyro is smooth and fast but drifts. An accelerometer feels gravity cleanly but is buried in vibration noise. A GPS receiver is dead accurate on average and jumps around by metres frame to frame. A wheel encoder is precise until the wheel slips. Point any one of them at the question "where am I, and how fast am I moving?" and you get an answer that is wrong in a predictable, characteristic way. The trick that makes modern robots work is that the ways they are wrong do not overlap. Combine them correctly and the errors partially cancel, leaving an estimate better than any single sensor could give you.

That combination is sensor fusion, and the mathematics that does it correctly (weighting each source by how much you should trust it, moment to moment, and carrying forward a running estimate of your own uncertainty) is the Kalman filter and its descendants. This guide is the long version for the engineer who has to make it work: the controls person who knows the equations but not why the covariance goes wrong on a real robot, the perception person who can run an EKF library but not tune it, and the maker who wants to fuse an IMU and a GPS without the heading spinning off into the weeds. We go from the recursive Bayes filter that underlies everything, through the Kalman filter and its predict/update equations, the EKF and UKF for nonlinear systems, particle filters for when the world is not Gaussian, the humble complementary filter that runs on a microcontroller, and factor-graph smoothing that has quietly become the default for the hardest problems. Along the way: how to tune the noise matrices, how to reject bad measurements with innovation gating, and the failure modes that will bite you.

The take: Sensor fusion is recursive Bayesian estimation. You carry a belief about the state (a mean and a covariance), push it forward through a motion model that inflates uncertainty, and pull it back with each measurement in proportion to how much you trust that measurement versus your prediction. The Kalman filter is the exact optimal solution when everything is linear and Gaussian; the EKF, UKF, and particle filter are three different ways to cope when it is not; the complementary filter is the same idea stripped to a fixed gain; and factor-graph smoothing is what you use when you can afford to look back and re-solve. The algorithm is rarely your problem. Your process and measurement noise models, your calibration, and your time synchronization are.

Companion reading: robot sensors, SLAM & localization, drone navigation: GNSS & RTK, robot perception & pose estimation, and real-time control systems.

Table of contents

1. Key takeaways
2. Why fuse sensors at all
3. The recursive Bayes filter
4. The Kalman filter
5. Reading the Kalman gain and covariance
6. The EKF and linearization
7. The unscented Kalman filter
8. Particle filters
9. Complementary filters
10. Factor-graph smoothing
11. Worked examples: IMU+GPS and visual-inertial
12. Tuning, innovation gating, and consistency
13. Failure modes
14. Frequently asked questions

Why fuse sensors at all

The case for fusion is easiest to see in the frequency domain. Take the classic problem of estimating a robot's orientation from an inexpensive inertial measurement unit. You have a gyroscope, which measures angular rate, and an accelerometer, which measures specific force. Integrate the gyro and you get orientation that is smooth and responsive over short intervals but drifts without bound, because a small rate bias integrates into an ever-growing angle error. The accelerometer, by contrast, gives you an absolute reference: when the robot is not accelerating much, the direction of the measured 9.81 m/s² tells you which way is down, so it pins roll and pitch. But it is drowning in vibration and linear-acceleration noise, so it is useless instant to instant.

One sensor is trustworthy at high frequency and garbage at low frequency. The other is exactly the reverse. Fusion is the act of taking each sensor where it is strong: the gyro for fast changes, the accelerometer for the slow absolute reference. The result tracks quick motions cleanly and never drifts. Neither sensor could do this alone.

The same pattern recurs everywhere in robotics. An IMU is fast and drifts; a GPS is slow, noisy per-sample, and globally anchored: fuse them for outdoor navigation. Wheel odometry is smooth and drifts on slip; a lidar scan-match is metric and absolute but jumpy: fuse them for a warehouse robot. A camera gives dense bearing but no scale; an IMU gives scale and gravity: fuse them for visual-inertial odometry. Every one of these is the same trade, a fast proprioceptive source that drifts married to a slower exteroceptive source that anchors.

Rule of thumb: identify which of your sensors is fast-but-drifting and which is slow-but-anchored. Fusion is almost always the marriage of those two. If all your sensors drift the same way, no filter will save you; you are missing an absolute reference.

There is a second, quieter reason to fuse: a good filter reports its own uncertainty. A raw sensor gives you a number; a filter gives you a number plus a covariance that says how much to trust it. Downstream consumers (a planner deciding whether to slow down, a controller deciding how hard to push) can use that uncertainty. This is why fusion is a first-class part of the perception and pose-estimation stack rather than a preprocessing step.

The recursive Bayes filter

Strip away the specific algorithm and every fusion method is the same recursion. You maintain a belief, bel(x_t) = p(x_t | z_{1:t}, u_{1:t}), the probability distribution over the current state given every measurement z and control u you have ever seen. Two operations update it.

Predict. Push the belief forward through the motion model, which describes how the state evolves given a control input. This step always inflates uncertainty, because the model is imperfect and you have added time without new information.

Correct. Fold in a new measurement using the observation model, which describes what you expect to sense from a given state. This step always shrinks uncertainty, because you have added information.

Recursive Bayes filter (the skeleton under every method here):

  predict:   bel-(x_t) = INTEGRAL  p(x_t | x_{t-1}, u_t) * bel(x_{t-1}) dx_{t-1}
  correct:   bel(x_t)  = eta * p(z_t | x_t) * bel-(x_t)

  eta = normalizer.  Predict spreads the belief; correct sharpens it.

This recursion rests on the first-order Markov assumption: the future depends on the past only through the present state. That is what lets you carry a fixed-size belief instead of the entire measurement history. Every filter below inherits that assumption, and most "my filter is overconfident" problems trace back to it being quietly false (a slip that persists across steps, a bias that is correlated in time).

The families differ only in how they represent bel(x):

• A Gaussian, a mean and covariance, gives you the Kalman filter and its nonlinear variants (EKF, UKF).
• A set of weighted samples gives you the particle filter.
• A fixed blending constant instead of a tracked covariance gives you the complementary filter.
• A graph of constraints solved by least squares gives you factor-graph smoothing.

The rest of this guide is a tour of those five choices and when each is right.

The Kalman filter

When the motion and observation models are linear and the noise is Gaussian, the Bayes filter has a closed-form solution, and it is the Kalman filter (Rudolf Kálmán, 1960). Under those assumptions the belief stays Gaussian forever, so you only ever need to track a mean vector x and a covariance matrix P. The filter is provably the minimum-variance unbiased estimator: no other estimator does better on those assumptions.

The setup. A linear system with process noise w ~ N(0, Q) and measurement noise v ~ N(0, R):

  state transition:   x_t = F * x_{t-1} + B * u_t + w      w ~ N(0, Q)
  measurement:        z_t = H * x_t + v                    v ~ N(0, R)

  F = state-transition matrix    B = control matrix
  H = observation matrix         Q = process noise covariance
  R = measurement noise covariance

The filter is five equations, split into the two Bayes steps.

PREDICT (project state and covariance forward):
  x- = F * x + B * u                     # predicted state (a priori)
  P- = F * P * Ftranspose + Q            # predicted covariance, grows by Q

UPDATE (fold in measurement z):
  y  = z - H * x-                        # innovation (measurement residual)
  S  = H * P- * Htranspose + R           # innovation covariance
  K  = P- * Htranspose * Sinverse        # Kalman gain
  x  = x- + K * y                        # corrected state (a posteriori)
  P  = (I - K * H) * P-                  # corrected covariance, shrinks

Read those five lines slowly, because everything else in this guide is a variation on them.

The predict step moves the mean through the dynamics F and grows the covariance. The F * P * Ftranspose term rotates and stretches your uncertainty through the dynamics; the + Q term adds fresh uncertainty for everything the model does not capture. With no measurements, P grows without bound: that is drift, expressed in the mathematics.

The update step is where fusion happens. The innovation y is the surprise: the difference between what you measured and what you predicted you would measure. The innovation covariance S is how surprised you expected to be, combining your prediction uncertainty (H * P- * Htranspose) and the sensor noise (R). The Kalman gain K is the optimal blend, and it has an intuitive form: it is prediction uncertainty divided by total uncertainty. When your prediction is uncertain and the sensor is precise, K is large and you trust the measurement. When your prediction is confident and the sensor is noisy, K is small and you barely move. The filter re-derives that blend every single step from the current covariances.

Rule of thumb: the Kalman gain is computed, every step, from P and R; you never set it by hand. If your filter trusts the wrong source, the bug lives in P (via Q) or R. Fix the covariances and the gain fixes itself.

Reading the Kalman gain and covariance

The covariance P is the part practitioners most often misunderstand, so it is worth dwelling on. P is the filter's honest self-assessment of how wrong it might be. Its diagonal entries are the variances of each state component; its off-diagonal entries are the correlations between them, and those correlations are where the quiet power lives.

Consider fusing wheel odometry and an IMU for a ground robot. Suppose the filter has learned, through the cross-terms in P, that its heading error and its lateral-position error are correlated (a heading mistake drags the position sideways as the robot drives). Now a measurement arrives that corrects heading. Because of the off-diagonal correlation, the update corrects position too, even though the measurement never directly observed position. This is the mechanism behind the whole method: a measurement of one thing improves your estimate of a correlated thing you did not measure. Get the correlations right and information flows where it is needed; get them wrong and it does not.

The single-dimensional case makes the gain concrete. Drop the matrices and let the prediction have variance p and the sensor have variance r. Then:

  K = p / (p + r)

  sensor very precise (r -> 0):   K -> 1   (trust the measurement fully)
  sensor very noisy   (r -> inf): K -> 0   (ignore it, keep the prediction)
  updated variance: p_new = (1 - K) * p = p * r / (p + r)  <= min(p, r)

That last line is the payoff in one expression: the fused variance is smaller than either input variance. Two mediocre estimates combine into one good one. This is exactly the frequency-domain intuition from the opening, written as covariance arithmetic.

The failure mode this exposes is the one to fear most. If you tell the filter your sensor is more precise than it is (R too small), or your model is better than it is (Q too small), the covariance P shrinks toward zero, the gain shrinks with it, and the filter stops listening to new measurements. It becomes serenely, catastrophically overconfident, and it will drive confidently into a wall while reporting a tight covariance. A too-large Q or R is merely sluggish; a too-small one is dangerous.

War story: a mobile robot's EKF fused wheel odometry and a lidar pose. Someone set the wheel-odometry process noise very low because "the encoders are accurate." On a patch of wet floor the wheels slipped, odometry insisted the robot had moved three metres, and the filter, trusting odometry over the lidar correction because its covariance said odometry was gospel, believed it. The lidar innovations grew large and were promptly gated out as "outliers." The robot drove into a shelf with a covariance ellipse the size of a coin. Nothing crashed. The filter did exactly what its noise model told it to. The fix was one number: raise the odometry Q so a slip could not out-vote the lidar.

The EKF and linearization

Real robots are not linear. A robot's pose lives on a rotation manifold; range and bearing measurements are trigonometric functions of position; a camera projects the world through a nonlinear pinhole. The plain Kalman filter does not apply. The Extended Kalman Filter is the oldest and still most common fix: linearize the nonlinear models around the current estimate, then run the ordinary Kalman equations on the linearized system.

Let the motion model be a nonlinear function f and the observation model a nonlinear function h. The EKF replaces F and H with their Jacobians (the matrices of partial derivatives) evaluated at the current state:

  x_t = f(x_{t-1}, u_t) + w
  z_t = h(x_t) + v

  F = df/dx  evaluated at the current estimate   # motion Jacobian
  H = dh/dx  evaluated at the predicted state     # observation Jacobian

PREDICT:
  x- = f(x, u)                          # propagate through the TRUE nonlinear f
  P- = F * P * Ftranspose + Q           # propagate covariance through the Jacobian

UPDATE:
  y  = z - h(x-)                        # innovation using the TRUE nonlinear h
  S  = H * P- * Htranspose + R
  K  = P- * Htranspose * Sinverse
  x  = x- + K * y
  P  = (I - K * H) * P-

The state and the innovation use the true nonlinear functions; only the covariance propagation uses the linear Jacobians. That is the EKF's central approximation: it assumes the function is close enough to linear over the span of your uncertainty that a first-order Taylor expansion carries the covariance correctly.

That assumption is where the EKF fails. When the nonlinearity is strong relative to your uncertainty (a sharp turn, a close-range bearing measurement, a large covariance), the Jacobian at the mean does not represent the function across the spread of the distribution. The linearization introduces error the filter does not know about, and because the EKF linearizes only once per step, at the mean, it can never undo that error on a later pass. The classic symptom is an EKF that grows overconfident, its reported covariance shrinking while its actual error grows, until it diverges. Linearizing around a wrong estimate produces a bad Jacobian, which produces a worse estimate, which produces a worse Jacobian.

Despite this, the EKF is everywhere: it is the workhorse behind GPS/INS integration, the standard robot_localization package in ROS, the attitude-and-heading reference systems in most autopilots, and countless embedded fusion nodes. It is cheap (one Jacobian evaluation per step), well understood, and good enough when the nonlinearity is mild and the update rate is high enough that the state never moves far between steps.

Rule of thumb: the EKF is fine when your uncertainty is small compared to the curvature of your models. Update often, keep the covariance tight with good measurements, and it behaves. If you see the covariance collapse while the estimate wanders, or divergence on aggressive motion, your linearization is the suspect: reach for the UKF or a smoother.

A practical note that saves hours: deriving Jacobians by hand is the most error-prone part of building an EKF. A single wrong sign in H produces a filter that looks like it runs but slowly diverges. Verify Jacobians numerically (compare the analytic Jacobian against a finite-difference approximation) before you trust them. On rotation states, use a proper manifold representation (an error-state EKF that keeps the nominal orientation as a quaternion and the error as a small rotation vector) rather than naively filtering Euler angles, which have singularities that will wreck you at 90 degrees of pitch.

The unscented Kalman filter

The Unscented Kalman Filter attacks the EKF's weakness directly. Instead of linearizing the nonlinear function, it passes the actual nonlinear function a carefully chosen set of sample points and reconstructs the resulting mean and covariance from where those points land. The insight, due to Julier and Uhlmann in the 1990s, is that it is easier to approximate a distribution than to approximate an arbitrary nonlinear function.

The mechanism is the unscented transform. Given the current mean and covariance, deterministically pick 2n+1 sigma points (one at the mean, and a symmetric pair along each of the n covariance axes) that together capture the mean and covariance exactly. Push every sigma point through the true nonlinear function. Then compute the mean and covariance of the transformed points as weighted sums.

Sigma points around mean x with covariance P (n = state dimension):
  X_0 = x
  X_i     = x + (sqrt((n + lambda) * P))_i      for i = 1..n
  X_{i+n} = x - (sqrt((n + lambda) * P))_i      for i = 1..n

Propagate each through the nonlinear f (or h), then recombine:
  x-  = SUM  W_i * f(X_i)                        # weighted mean
  P-  = SUM  W_i * (f(X_i) - x-)(f(X_i) - x-)transpose + Q

  lambda = scaling parameter;  W_i = weights (mean and covariance sets)

Because the sigma points sample the function itself, the UKF captures the true mean and covariance correctly to second order for any nonlinearity (third order for symmetric distributions), where the EKF is only first-order accurate. It needs no Jacobians at all, which removes the single most error-prone part of building an EKF: you just supply the nonlinear functions and the filter does the rest. On strongly nonlinear systems (a fast-rotating body, a bearing-only tracker, a highly maneuvering target) the UKF is meaningfully more accurate and more stable, and it rarely diverges where an EKF would.

The costs are modest. The UKF evaluates the nonlinear function 2n+1 times per step instead of once, so it is a few times more expensive, though it saves the Jacobian derivation. It still assumes the belief is Gaussian, so it cannot represent multi-modal beliefs any better than the EKF can. And the scaling parameters (alpha, beta, kappa in the usual parameterization) need sane defaults; the common alpha = 1e-3, beta = 2, kappa = 0 works for most robotics states.

Property	EKF	UKF
Nonlinearity handling	First-order (Taylor)	Second-order (unscented transform)
Jacobians required	Yes (derive by hand)	No
Function evaluations per step	1	2n+1
Accuracy on strong nonlinearity	Degrades, can diverge	Robust
Multi-modal beliefs	No	No
Typical use	Mild nonlinearity, high rate, embedded	Strong nonlinearity, when Jacobians are painful

Rule of thumb: if your models are only mildly nonlinear and you update fast, the EKF is simpler and lighter. If deriving Jacobians is painful, or your EKF diverges on aggressive motion, switch to the UKF. It is a near drop-in replacement that trades a few extra function evaluations for a large gain in robustness.

Particle filters

The Kalman family, EKF and UKF included, assumes the belief is a single Gaussian: one blob, one peak. That assumption breaks when the belief is genuinely multi-modal. A robot performing global localization in a building with three identical rooms should believe it is in one of three places, with three separate peaks. No Gaussian can represent that. The particle filter can.

A particle filter represents the belief as a cloud of weighted samples, each a complete hypothesis of the state. The recursion follows the Bayes filter directly:

Particle filter (one step):
  1. PREDICT:   push every particle through the motion model, WITH noise
                x_i <- f(x_i, u) + sampled process noise
  2. WEIGHT:    reweight each particle by how well it explains the measurement
                w_i <- w_i * p(z | x_i)          # the measurement likelihood
  3. NORMALIZE: w_i <- w_i / SUM(w_j)
  4. RESAMPLE:  when weights concentrate, draw a new particle set in
                proportion to weight (kill low-weight, duplicate high-weight)

The strengths are exactly the Kalman family's weaknesses. A particle filter represents any distribution, multi-modal or skewed, limited only by the number of particles. It handles arbitrary nonlinearity because it never linearizes; it only ever evaluates the models forward. This is why particle filters own global localization and the kidnapped-robot problem: Adaptive Monte Carlo Localization (AMCL), the ROS standard for localizing against a known map, is a particle filter precisely because it must represent "I could be in any of these places" and recover when the world contradicts its current belief. See SLAM & localization for where AMCL sits in the navigation stack.

The weakness is dimensionality. The number of particles needed to cover a belief grows roughly exponentially with the dimension of the state. A particle filter is superb for a 3-DoF planar pose (x, y, theta) and hopeless for a 15-dimensional visual-inertial state; you would need millions of particles. This is why particle filters dominate low-dimensional localization and are almost never used for high-dimensional fusion, where the Kalman family and smoothers win.

Two practical points decide whether a particle filter works. First, when to resample. Resample every step and you needlessly throw away diversity (sampling noise erodes the hypothesis set); resample never and all the weight collapses onto one particle. The standard trigger is the effective sample size, N_eff = 1 / SUM(w_i squared), which ranges from 1 (all weight on one particle, degenerate) to N (uniform weights, healthy). Resample only when N_eff drops below N/2. Second, particle depletion: after enough resampling, diversity can collapse and the true hypothesis gets resampled away. Injecting a small fraction of random particles each step, as AMCL does, guards against this and enables recovery from a lost track.

Rule of thumb: use a particle filter when the belief is multi-modal or the state is low-dimensional and hard to Gaussianize (global localization on a map). Do not use one for high-dimensional fusion (visual-inertial, full 6-DoF with biases); the particle count explodes and the Kalman family or a smoother is far more efficient.

Complementary filters

Not every robot has the compute or the need for a full Kalman filter. The complementary filter is fusion stripped to its essence: a fixed blending constant instead of a tracked covariance. It is what runs on the small microcontroller in a hobby drone's flight controller, and it is genuinely the right tool for many attitude-estimation jobs.

The idea is the frequency-domain intuition made literal. You have a fast source that drifts (integrated gyro) and a slow source that is noisy but anchored (accelerometer for gravity direction). A complementary filter high-passes the fast source (keeping its good high-frequency content, discarding the low-frequency drift) and low-passes the slow source (keeping its good low-frequency anchor, discarding the high-frequency noise), then adds them. The two filters are "complementary" because their transfer functions sum to one at every frequency, so you reconstruct the full signal with no gain distortion.

For attitude, the discrete form is a single line:

  angle = alpha * (angle + gyro_rate * dt) + (1 - alpha) * accel_angle

  alpha in [0, 1], typically 0.95 to 0.99
  high alpha  -> trust the gyro more (smoother, slower to correct drift)
  low alpha   -> trust the accelerometer more (jumpier, corrects faster)

  The crossover time constant:  tau = alpha * dt / (1 - alpha)

That tau is the knob: motions faster than tau come from the gyro, slower than tau from the accelerometer. Pick tau a few seconds long and the gyro handles all real motion while the accelerometer slowly corrects drift in the background.

The complementary filter is a Kalman filter with the gain frozen. Where the Kalman filter computes the optimal blend every step from live covariances, the complementary filter uses one constant blend you chose in advance. On a system with roughly stationary noise (an IMU with stable characteristics) the optimal Kalman gain converges to a constant anyway, so the two are nearly equivalent, and the complementary filter gets there with a fraction of the code and no matrix algebra. The Madgwick and Mahony filters are the well-known quaternion-based complementary filters that added a proper 3D orientation representation and, in Mahony's case, an integral term to estimate gyro bias; they are the default attitude estimators in a large fraction of open-source flight controllers.

Rule of thumb: for attitude on a small platform with limited compute, start with a complementary filter (Madgwick or Mahony). Reach for a full EKF/UKF only when you need to fuse more sensors (GPS, magnetometer, external pose) or you need the calibrated uncertainty output. Do not build a 15-state EKF to level a camera gimbal.

Factor-graph smoothing

Everything so far is a filter: it maintains a running estimate of the current state and throws away the past. That is the right choice when you need a low-latency answer now and cannot afford to look back. But it has costs. A filter linearizes once, at the moment a measurement arrives, and can never revisit that choice; it handles delayed or out-of-order measurements awkwardly; and marginalizing the past means information from an early measurement is baked into the covariance and cannot be re-examined when a later measurement reveals it was misleading.

Factor-graph smoothing takes the opposite stance. Keep the whole trajectory as variables, accumulate every measurement as a constraint (factor) connecting the variables it touches, and solve for the entire set of states that best satisfies all constraints at once. This is Maximum a Posteriori (MAP) estimation, and under Gaussian noise it is exactly a nonlinear least-squares problem:

  X* = argmin_X  SUM_k  r_k(X)transpose * Omega_k * r_k(X)

  r_k = residual of factor k (measured minus predicted)
  Omega_k = information matrix (inverse covariance) of factor k
          = how much to trust this measurement

  Solved by Gauss-Newton or Levenberg-Marquardt, iterating to convergence.

Two things make this practical rather than hopelessly expensive. First, the problem is sparse: each factor touches only a few variables (an IMU factor connects two consecutive poses, a GPS factor touches one), so the linear system solved at each iteration has a sparse structure that factorizes efficiently. A trajectory with occasional loop closures scales to hundreds of thousands of variables. Second, incremental solvers (iSAM2 in the GTSAM library) re-solve only the part of the graph a new measurement actually affects, so adding a measurement in real time costs almost nothing until a constraint ties distant parts of the graph together.

The advantages over filtering are concrete. Smoothing relinearizes every iteration, so it recovers from bad initial estimates where an EKF is stuck with its one-shot linearization. It handles delayed and out-of-order measurements naturally: a GPS fix that arrives 200 ms late just becomes a factor on the pose from 200 ms ago, no special handling required, which is a genuine headache in a filter. And because it keeps the trajectory, a later measurement can correct an earlier pose, which is exactly what loop closure does in SLAM.

The cost is compute and latency: solving a graph is heavier than a filter update, and you are estimating many states rather than one. The standard engineering answer is a fixed-lag smoother, which keeps only a sliding window of recent states (say the last one or two seconds) and marginalizes everything older into a prior. This captures most of smoothing's accuracy at bounded, real-time cost, and it is the architecture behind modern visual-inertial systems.

Property	Filtering (KF/EKF/UKF)	Smoothing (factor graph)
Estimates	Current state only	Trajectory (window or full)
Linearization	Once, at measurement time	Relinearized every iteration
Delayed / out-of-order measurements	Awkward	Natural
Recover from bad init	Weakly	Yes (re-optimization)
Compute per step	Light, constant	Heavier, structure-dependent
Latency	Lowest	Higher (mitigated by fixed-lag)
Typical use	High-rate control-loop estimation	VIO, LIO, SLAM back-ends

Rule of thumb: if you need a low-latency estimate for a control loop and your models are mild, filter. If you are building a navigation or SLAM system where accuracy and delayed-measurement handling matter, use a fixed-lag or full smoother. The 2026 default for serious visual-inertial and lidar-inertial estimation is a factor graph, not an EKF.

Worked examples: IMU+GPS and visual-inertial

Two examples turn the machinery concrete, because the design choices are where the difficulty lives.

IMU + GPS for outdoor navigation

This is the canonical fusion problem and the one most autopilots solve. The IMU runs fast (100 to 1000 Hz), gives you smooth high-rate motion, and drifts. The GPS runs slow (1 to 10 Hz), gives you an absolute position with metre-level noise, and never drifts on average. The IMU handles the prediction between GPS fixes; each GPS fix corrects the accumulated drift.

The state is more than pose. To integrate the IMU correctly you must estimate its biases, because an unestimated accelerometer bias integrates into position error as ½ * bias * t² and will dominate everything. A typical error-state EKF carries:

  state x = [ position (3), velocity (3), orientation (3, error-rotation),
              accel bias (3), gyro bias (3) ]        # 15 states

  PREDICT (at IMU rate, ~200 Hz):
    integrate accelerometer and gyro through the strapdown equations;
    grow P by the IMU process noise (drives the bias random walks).

  UPDATE (at GPS rate, ~5 Hz):
    innovation y = z_gps - position_estimate;
    Kalman update pulls position, and through the covariance
    cross-terms, also velocity and the biases, back toward truth.

The subtlety that trips people is the difference in rates and the role of biases. The GPS never directly measures velocity or bias, yet the filter estimates both, because the cross-covariance terms link them to position over time. Observe position often enough and the whole state, biases included, becomes observable, provided the trajectory has enough motion to excite it. Sit still and the biases drift unobserved. For centimetre accuracy you replace or augment GPS with an RTK fix, which changes only the measurement noise R (much smaller) and the update, not the filter structure. See drone navigation: GNSS & RTK for the GNSS side of this.

Visual-inertial odometry

Visual-inertial odometry (VIO) fuses a camera with an IMU, the standard for drones, headsets, and weight-constrained robots where lidar is too heavy. The camera gives rich bearing information but no absolute scale (a monocular camera cannot tell a small close object from a large far one); the IMU gives metric scale and a gravity reference. Fuse them tightly and you get metric, gravity-aligned motion that survives the brief moments the camera fails (motion blur, a passing truck).

The design choice that dominates VIO performance is coupling. Loose coupling runs the visual estimator and the inertial estimator separately and fuses their outputs; it is simpler and modular but throws away cross-information. Tight coupling puts raw IMU measurements and raw visual feature observations into one estimator and solves jointly; it is more accurate, recovers scale and biases better, and is more robust to degeneracy. Every serious VIO system is tightly coupled. The two dominant architectures are a fixed-lag factor-graph smoother (VINS-Fusion, the visual-inertial back-end of ORB-SLAM3) and a filter, the Multi-State Constraint Kalman Filter or MSCKF (OpenVINS), which cleverly keeps a sliding window of past poses in the EKF state and marginalizes features to get near-smoother accuracy at filter cost.

The catch that burns people is that scale is only observable under acceleration. A monocular-inertial system's metric scale is observable only when the accelerometer feels something beyond gravity. Fly at constant velocity and scale silently drifts. This is why a well-designed VIO does a short "initialization dance" (a brief jerky motion) before it trusts scale, and why hovering drones fight scale drift. The filter is only as good as the information the trajectory feeds it. VIO sits inside the broader perception and pose-estimation and SLAM stacks.

Tuning, innovation gating, and consistency

A working filter is 90% tuning and plumbing. The algorithm is a commodity; the noise models are a bespoke specification of how much you trust each part of your system, and they are where the effort goes.

Tuning Q and R. The process noise Q encodes how much you distrust your motion model; the measurement noise R encodes how much you distrust each sensor. Only their ratio matters for the gain. R you can often measure directly: leave a sensor stationary, record it, and compute the variance of its output. That gives you a principled starting R. Q is harder because it lumps together everything the model omits (unmodeled dynamics, discretization error, real disturbances) and is usually tuned by hand. The failure signatures are clear once you know them: a filter that lags reality and ignores good measurements has Q too small (it overtrusts its model); a filter that chases sensor noise has Q too large or R too small. Start with R measured, then adjust Q until the estimate tracks without chattering.

Innovation gating. Every measurement produces an innovation y with a known expected covariance S. That gives you a free, principled outlier detector. The normalized innovation squared (NIS), y_transpose * S_inverse * y, follows a chi-squared distribution with degrees of freedom equal to the measurement dimension if the filter is consistent. A measurement whose NIS exceeds the chi-squared threshold (for example, the 95th or 99th percentile) is statistically too surprising to be real and is probably an outlier: a GPS multipath spike, a bad feature match, a lidar return off a passing person. Reject it before it corrupts the state.

Innovation gating (Mahalanobis / chi-squared test):
  y = z - h(x-)                    # innovation
  S = H * P- * Htranspose + R      # innovation covariance
  d2 = ytranspose * Sinverse * y   # normalized innovation squared (NIS)

  if d2 > chi2_threshold(dim, confidence):   reject this measurement
  else:                                       accept and update

The gate is one of the highest-value dozen lines in a fusion stack. Without it, a single bad measurement can wrench the estimate and, worse, corrupt the covariance so the filter distrusts the good measurements that follow.

Consistency checking. The deepest tuning tool is asking whether the filter's reported uncertainty matches its actual error. Over a run, the NIS should average close to the measurement dimension; if it is consistently much larger, the filter is overconfident (its P is too small, usually because Q or R is understated) and it is heading for divergence. If NIS is consistently much smaller, the filter is underconfident (conservative, wasting information). This is the NEES/NIS consistency test, and running it on logged data is how you catch the overconfidence that leads to the silent, confident-but-wrong failures that are the worst kind.

Rule of thumb: measure R from stationary sensor data, tune Q by hand until tracking is crisp without chatter, always innovation-gate your updates, and check filter consistency (NIS) on real logs. A filter that passes a consistency check is one you can trust; one that has never been checked is a liability regardless of how good the estimate looks on a calm day.

Failure modes

Knowing how fusion breaks is more useful than knowing how it works, because the breakage is where your robot ends up in a ditch.

Overconfidence and divergence. The most dangerous failure. Understated Q or R shrinks P, which shrinks the gain, which stops the filter listening, which lets the estimate drift while the reported covariance stays tight. The filter is confidently wrong, and because it now distrusts incoming measurements, it cannot recover. Innovation gating makes it worse by rejecting the very measurements that would fix it. The defense is consistency checking and a healthy respect for larger Q.

Linearization error (EKF). Strong nonlinearity relative to your uncertainty makes the Jacobian at the mean unrepresentative, injecting error the filter cannot see. The fix is to update more often (keep uncertainty small), use an error-state formulation on manifolds, or switch to a UKF or smoother.

Time synchronization. A temporal offset between two sensors aliases directly into a state error. During a 200 deg/s turn, a 5 ms timing offset between camera and IMU injects 1 degree of orientation error into every frame: a systematic, motion-correlated bias no filter averages away, because it is not noise. Serious systems (VINS-Fusion, Kalibr) estimate the time offset online as a state variable rather than trusting driver timestamps.

Calibration errors. An uncalibrated extrinsic (the rigid transform between two sensors) or wrong intrinsics produce a consistent bias the filter interprets as real motion. Calibration is foundational, and a large fraction of "this filter is bad" reports are actually a bad extrinsic.

Correlated noise and the Markov violation. The Kalman filter assumes white (uncorrelated in time) process and measurement noise. Real errors are often correlated: a wheel that slipped last step is likely slipping this step; a GPS multipath error persists for seconds. The filter treats each correlated measurement as fresh independent information and grows overconfident. The fix is to model the correlation explicitly (augment the state with a colored-noise term) or to inflate R to account for the lost independence.

Non-Gaussian, multi-modal beliefs. A Kalman filter forced to represent a genuinely multi-modal belief (global localization ambiguity) collapses it to a single mean sitting between the modes, which is a place the robot definitely is not. When the belief is multi-modal, use a particle filter.

Unobservable states. Some states are simply not observable from your sensors under your current motion: yaw from an IMU alone, monocular scale at constant velocity, biases while stationary. The filter's covariance for those states grows without bound or, worse, the filter reports false confidence from linearization artifacts. Know your observability, and design the trajectory or add a sensor to make the critical states observable.

Rule of thumb: before blaming the algorithm, check the plumbing. Time synchronization, extrinsic calibration, and the Q/R ratio account for the large majority of fusion failures. The exotic ones (correlated noise, unobservability) are real but rarer, and you will only diagnose them once the plumbing is clean.

Frequently asked questions

What is the difference between the Kalman filter and a complementary filter? Both fuse a fast-drifting sensor with a slow-anchored one. The Kalman filter computes the optimal blend every step from live covariances and reports its own uncertainty; the complementary filter uses a single fixed blending constant you chose in advance and reports no uncertainty. On a system with stable noise the optimal Kalman gain converges to a constant, so the two are nearly equivalent, and the complementary filter gets there in a few lines of microcontroller code. Use the complementary filter for attitude on small platforms; use the Kalman family when you need to fuse more sensors or need calibrated uncertainty out.

EKF or UKF: which should I use? Start with the EKF if your models are only mildly nonlinear and you update fast; it is simpler, lighter, and standard. Switch to the UKF if deriving Jacobians is painful or error-prone, or if your EKF diverges under aggressive motion. The UKF captures nonlinearity to second order by propagating sigma points through the true function, needs no Jacobians, and is a near drop-in replacement for a few extra function evaluations per step.

When do I need a particle filter instead of a Kalman filter? When the belief is genuinely multi-modal (global localization on a map with repeated structure, the kidnapped-robot problem) or the nonlinearity is severe and the state is low-dimensional. A Kalman filter can only represent one Gaussian blob and will sit its estimate between two real modes, which is wrong. The catch is that particle filters scale badly with state dimension, so they own low-dimensional localization and are almost never used for high-dimensional fusion.

What are Q and R, and how do I set them? Q is the process-noise covariance, how much you distrust your motion model; R is the measurement-noise covariance, how much you distrust each sensor. Measure R directly from stationary sensor data, then tune Q by hand until the estimate tracks crisply without chattering. Only the ratio Q/R matters for the gain. A too-small Q makes the filter ignore measurements and lag; a too-small R or too-large Q makes it chase noise.

Why does my filter become overconfident and diverge? Almost always because Q or R is understated, so the covariance P shrinks, the gain shrinks, and the filter stops listening to new measurements while its actual error grows. For an EKF, linearization error under strong nonlinearity does the same. Diagnose it with a consistency check: if the normalized innovation squared averages well above the measurement dimension over a run, the filter is overconfident. Inflate Q, gate outliers, or move to a UKF/smoother.

What is innovation gating and why do I need it? The innovation is the difference between a measurement and its prediction, and it has a known expected covariance S. The normalized innovation squared y_transpose * S_inverse * y follows a chi-squared distribution when the filter is consistent, so a measurement whose value exceeds a chi-squared threshold is statistically too surprising to be real and is probably an outlier. Rejecting it before the update protects the estimate and the covariance from GPS spikes, bad feature matches, and spurious returns. It is a dozen lines and one of the highest-value additions to any fusion stack.

When should I use a smoother instead of a filter? Use a factor-graph smoother when accuracy matters more than the lowest possible latency, when you have delayed or out-of-order measurements, or when a later measurement should be able to correct an earlier estimate (loop closure). Smoothing relinearizes every iteration and handles late measurements naturally, at higher compute cost. A fixed-lag smoother keeps a sliding window to bound that cost, and it is the 2026 default for serious visual-inertial and lidar-inertial systems. Use a plain filter for high-rate estimation inside a control loop.

Can I fuse sensors that run at very different rates? Yes, and it is the normal case. The Kalman filter's predict and update steps are decoupled: run predict at your fastest rate (the IMU, say 200 Hz) and run update only when a slower measurement arrives (GPS at 5 Hz). Each update corrects the drift the prediction accumulated since the last one. The only requirement is accurate timestamps so each measurement is applied to the state at the correct time.

Why do the biases matter so much in IMU fusion? Because an accelerometer bias integrates into position error as ½ * bias * t² and a gyro bias integrates into a growing heading error, both unbounded. If you do not estimate the biases as part of the state, they poison the integration and the estimate drifts fast. This is why a real IMU-fusion filter carries 15 states (position, velocity, orientation, accel bias, gyro bias) rather than just pose, and why the state becomes observable only when the trajectory has enough motion to excite the biases.

My filter works in simulation but drifts on the real robot. Where do I look first? The plumbing, in this order: time synchronization between sensors (an unsynchronized timestamp is a motion-correlated bias no filter removes), extrinsic and intrinsic calibration (a wrong transform reads as real motion), then the Q/R noise models (usually too optimistic), then observability (is the state actually observable under your motion). The algorithm is almost never the first problem. Check the clocks and the calibration before you touch the math.