From System-Level Sensor Fusion to the Underlying Mathematics
This studio's article on mechatronic system design introduces sensor fusion at a system level, why an accelerometer and gyroscope are combined rather than used alone, and briefly contrasts complementary and Kalman filtering as two fusion approaches. This article goes deeper into the actual mathematics behind Kalman filtering specifically: the state estimation framework it operates within, the predict/update cycle that runs at every time step, how the Kalman gain is derived and what it represents, and a closer technical look at the complementary filter as a simpler alternative, aimed at an engineer who needs to actually implement, not just conceptually understand, a fusion algorithm.
State Estimation: The Problem Kalman Filtering Solves
A state is the set of variables that fully describes a system at a given instant, for a mobile robot, this might be position and velocity; for an IMU-based orientation estimate, it might be tilt angle and angular rate. State estimation is the problem of determining that true, unobservable state as accurately as possible given two imperfect information sources: a motion (process) model that predicts how the state should evolve based on known physics and control inputs, and one or more sensor measurements that observe some function of the true state, imperfectly, with noise. Neither source alone is fully trustworthy: the motion model's predictions drift from reality over time because real systems are affected by unmodeled disturbances (wind, friction variation, wheel slip) the model doesn't capture, and sensor measurements are corrupted by noise, and sometimes only observe part of the state indirectly (an accelerometer measures acceleration, not position, directly). A Kalman filter is the mathematically optimal way (for a linear system with Gaussian-distributed noise) to combine both imperfect sources into a single best estimate, along with a rigorous, continuously updated measure of how confident that estimate actually is.
The State-Space Model
A Kalman filter operates on a state-space model of the system, expressed as two equations. The process model describes how the state evolves from one time step to the next: x_k = F·x_(k-1) + B·u_k + w_k, where x_k is the state vector at time step k, F is the state transition matrix (encoding the system's known dynamics, e.g., how position changes given velocity), B·u_k is the effect of any known control input u_k (like a commanded motor voltage), and w_k is process noise, a random variable representing everything the model doesn't capture, characterized by its covariance matrix Q. The measurement model describes how a sensor observes the state: z_k = H·x_k + v_k, where z_k is the actual sensor reading, H maps the true state into the quantity the sensor actually measures (which may not be the full state directly, for example H might extract just position from a state vector containing both position and velocity), and v_k is measurement noise, characterized by its covariance matrix R. Everything the Kalman filter does mathematically follows from these two equations and the statistical assumption that w_k and v_k are zero-mean, Gaussian-distributed, and independent of each other.
The Predict/Update Cycle
A Kalman filter runs a two-phase cycle at every time step, repeating indefinitely as new measurements arrive.
Predict (time update): using the process model alone, project the previous state estimate forward to the current time step, before incorporating any new measurement. The predicted state is x̂_k⁻ = F·x̂_(k-1) + B·u_k (apply the known dynamics and control input), and, critically, the predicted uncertainty grows: P_k⁻ = F·P_(k-1)·Fᵀ + Q (the previous estimate's covariance P propagated through the dynamics, plus the process noise Q added in, since the model itself is imperfect). This growth in uncertainty during prediction is the mathematical reason a gyroscope-only angle estimate drifts over time if never corrected: each prediction step, with no measurement to correct it, adds a little more uncertainty, and that uncertainty compounds indefinitely without a correcting update.
Update (measurement update): incorporate the new sensor measurement to correct the prediction. First compute the innovation (or residual), the difference between the actual measurement and what the model predicted it should be: y_k = z_k − H·x̂_k⁻. Then compute the Kalman gain: K_k = P_k⁻·Hᵀ / (H·P_k⁻·Hᵀ + R) — the ratio of predicted-state uncertainty to total (predicted plus measurement) uncertainty, which determines how much weight the new measurement gets relative to the prediction. Finally, correct the state estimate and shrink its uncertainty: x̂_k = x̂_k⁻ + K_k·y_k (nudge the prediction toward the measurement, weighted by K_k) and P_k = (I − K_k·H)·P_k⁻ (the corrected estimate is now more certain than the raw prediction was, since it now incorporates the new information). This corrected state and covariance become the starting point for the next cycle's predict step, and the whole predict/update cycle repeats indefinitely, once per control-loop time step.
Why the Kalman Gain Is the Heart of the Algorithm
The Kalman gain K_k is what makes the filter adaptive rather than a fixed blend. When the predicted state's uncertainty (P_k⁻) is large relative to the measurement noise (R), meaning the model prediction is currently not very trustworthy, likely because some time has passed since the last correcting measurement, K_k moves toward 1 and the filter leans heavily on the new measurement to correct itself. When the measurement noise R is large relative to the predicted uncertainty, meaning the sensor reading is comparatively noisy or otherwise untrustworthy right now, K_k moves toward 0 and the filter instead leans on its own model-based prediction, effectively filtering out the noisy measurement. This gain is recomputed at every single time step from the current covariance values, not set once in advance, which is exactly why a Kalman filter naturally handles a scenario a fixed-weight blend cannot: trusting the model heavily right after a fresh correcting measurement, then progressively trusting the next incoming measurement more as the prediction's own uncertainty grows the longer it goes uncorrected.
A Concrete Example: Fusing an Encoder and an Accelerometer for Velocity
Consider estimating a mobile robot's velocity by fusing wheel encoder data (which gives a good but not perfect velocity reading, subject to wheel slip) with an accelerometer (which measures acceleration, from which velocity can be derived by integration, but drifts due to sensor bias and noise when integrated over time). The state vector might be x = [velocity], the process model predicts the next velocity from the current velocity plus the accelerometer-derived acceleration input (B·u_k term), and the encoder provides the periodic measurement update (z_k) that corrects accumulated drift from relying on the accelerometer-driven prediction alone. During a brief period of wheel slip (encoder reading momentarily untrustworthy, effectively meaning R should be treated as large for that update), a well-tuned filter, or an adaptive implementation that detects the slip condition and temporarily inflates R, leans more on the accelerometer-based prediction; once normal traction resumes, the filter goes back to trusting the encoder's typically lower measurement noise to correct any drift the accelerometer-only prediction accumulated during the slip event. This is the same predict/update structure used for IMU orientation fusion (gyroscope-integrated prediction, accelerometer/magnetometer measurement correction), just with different physical quantities in the state and measurement vectors.
The Complementary Filter, Revisited Mathematically
The complementary filter, introduced at a conceptual level in the companion sensor-fusion article, can now be understood precisely as a simplified, fixed-gain special case of the same fusion idea. Its standard form, angle = α·(angle_previous + gyro_rate·dt) + (1 − α)·angle_accelerometer, is structurally a predict step (angle_previous + gyro_rate·dt, exactly analogous to the Kalman filter's F·x̂_(k-1) prediction) blended with a measurement correction (angle_accelerometer), using a fixed weight α instead of a covariance-derived Kalman gain that changes every step. This is precisely why a complementary filter is so much simpler to implement, no covariance matrices, no matrix inversion, just one tuned constant α, and precisely why it's less capable: it applies the same fixed trust ratio between prediction and measurement regardless of whether the prediction has actually become more or less certain since the last update, which a genuine Kalman filter accounts for automatically. For a well-behaved, roughly stationary noise environment (which describes a large share of practical mechatronic sensor pairs), a well-tuned fixed α performs nearly as well as a properly tuned Kalman filter at a small fraction of the computational and implementation cost, which is exactly why complementary filters remain a legitimate engineering choice rather than merely a "beginner" simplification to be outgrown.
Handling Nonlinear Systems: The Extended Kalman Filter
The standard Kalman filter assumes the process and measurement models are linear (F and H are constant matrices). Many real mechatronic systems, especially orientation estimation using quaternions or Euler angles, and most mobile robot localization problems, have genuinely nonlinear dynamics or measurement functions. The Extended Kalman Filter (EKF) handles this by linearizing the nonlinear process and measurement functions around the current state estimate at each time step, using their Jacobian matrices (the local, first-order-derivative linear approximation of the nonlinear function) in place of the fixed F and H matrices, then otherwise running the identical predict/update cycle described above. This is by far the most widely used Kalman filter variant in real robotics and mechatronics practice, since genuinely linear state-space models are the exception rather than the rule outside of simplified textbook examples; a further refinement, the Unscented Kalman Filter (UKF), avoids the linearization step entirely by propagating a small set of representative sample points through the true nonlinear functions, generally performing better than an EKF for strongly nonlinear systems at somewhat higher computational cost.
Practical Tuning: Choosing Q and R
Implementing the Kalman filter equations correctly is necessary but not sufficient; the filter's actual performance depends heavily on realistic process noise (Q) and measurement noise (R) values. R is usually the more straightforward of the two to determine, since it can often be measured directly by holding a sensor stationary and computing the variance of its raw output over a representative sample period, or taken from a manufacturer's datasheet noise specification. Q is inherently harder to pin down, since it represents the model's own unmodeled uncertainty rather than a directly measurable physical quantity, and is more commonly tuned empirically: start with a reasonable estimate, run the filter against logged or real sensor data, and adjust Q up if the filter responds too sluggishly to genuine changes in the true state, or down if the estimate is noisier than the true underlying signal should be. This empirical Q/R tuning step, not the predict/update math itself, which is fixed once the state-space model is defined, is typically where most of the real engineering effort in deploying a Kalman filter on a physical mechatronic system actually goes.