49 Complementary Filters: Tuning and Validity

49.5.1 Kalman state-space bridge

The complementary filter uses a fixed blend. A Kalman filter makes the same predict-and-correct story explicit as a state model with uncertainty. Define a state vector $\mathbf{x}_k$ containing the physical quantities to estimate at sample $k$, a control input $\mathbf{u}_k$, and a measurement $\mathbf{z}_k$:

$\mathbf{A}$ advances the old state through the physical model, $\mathbf{B}$ maps a measured or commanded input into that state, and $\mathbf{H}$ selects the combination the sensor can observe. The process disturbance $\mathbf{w}$ covers model error; the measurement disturbance $\mathbf{v}$ covers sensor error. Their covariance matrices $\mathbf{Q}$ and $\mathbf{R}$ describe expected variance and cross-correlation, not arbitrary “trust knobs.”

For one IMU tilt axis, a useful state estimates angle $\theta$ and gyroscope bias $b$ while the measured gyro rate $\omega_m$ acts as the input:

The first row says $\theta_k\approx\theta_{k-1}+\Delta t(\omega_{m,k}-b_{k-1})$; the second models bias as slowly varying. When the accelerometer supplies a roll or pitch observation $z_k=\theta_{acc,k}$,

because the measurement directly observes angle, not gyro bias. With $\Delta t=0.01$ s, previous state $[10.0^\circ,0.2^\circ/\text{s}]^T$, and measured rate $5.0^\circ/\text{s}$, the prediction is $10.0+0.01(5.0-0.2)=10.048^\circ$. The measurement update decides how far to move from that prediction only after comparing the predicted covariance with accelerometer noise.

The complete recursion is:

Read it step by step: predict the state, predict its uncertainty, form the measurement residual, predict the residual variance, calculate the Kalman gain, correct the state, then reduce the covariance in the observed direction. If accelerometer variance $R$ rises during vibration, the residual variance grows and the gain normally gives that angle less influence. If process covariance $Q$ is too small, the filter becomes overconfident in an imperfect motion/bias model and responds sluggishly to real change.

49.5.2 IMU Kalman matrices and tilt-compensated yaw

The complete estimator is easier to review when matrices and axis observability remain on the data path. Figure 49.1 keeps gyro and accelerometer roll/pitch correction separate from magnetometer tilt compensation for yaw.

In Figure 49.1, PREDICT records A and Q, while CORRECT records H and R instead of hiding uncertainty inside a generic filter box. TILT COMPENSATE rotates m using roll and pitch before the YAW atan2 step, so magnetic disturbance can widen yaw health without falsely invalidating roll and pitch.

Calculate the matrix dimensions and units before writing the loop. In the two-state example, $\mathbf{x}$ is $2\times1$, $\mathbf{A}$ and $\mathbf{Q}$ are $2\times2$, $\mathbf{B}$ is $2\times1$, $\mathbf{H}$ is $1\times2$, and scalar angle measurement noise may be represented by a $1\times1$ $\mathbf{R}$. The angle-bias off-diagonal entries in $\mathbf{P}$ matter: a measurement that corrects angle can also improve the inferred bias through that covariance. For numerical robustness, keep $\mathbf{P}$ symmetric, verify that $\mathbf{Q}$ and $\mathbf{R}$ are positive semidefinite, and use the Joseph covariance update when finite-precision rounding is visible:

Roll and pitch provide the missing geometry for magnetometer heading. First calibrate hard-iron offset and soft-iron scale, then rotate the body-frame magnetic vector $(m_x,m_y,m_z)$ onto the horizontal plane. For one common right-handed roll-$\phi$, pitch-$\theta$ convention,

The signs and axis order change with ENU/NED coordinates and sensor mounting, so prove the implementation with known north/east headings and positive roll/pitch rotations rather than copying the equations without a frame definition. Tilt compensation removes the geometric projection error; it does not remove magnetic distortion, gyro drift during magnetic rejection, or the need to apply local magnetic declination when the product requires true rather than magnetic north.