← Back to the chapter · Next detail →

Technical detail · Complementary Filters: Tuning and Validity

Build the angle-and-bias state model

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 xk\mathbf{x}_k containing the physical quantities to estimate at sample kk, a control input uk\mathbf{u}_k, and a measurement zk\mathbf{z}_k:

xk=Akxk−1+Bkuk+wk,wk∼N(0,Qk),\mathbf{x}_k=\mathbf{A}_k\mathbf{x}_{k-1} +\mathbf{B}_k\mathbf{u}_k+\mathbf{w}_k, \qquad \mathbf{w}_k\sim\mathcal{N}(\mathbf{0},\mathbf{Q}_k), zk=Hkxk+vk,vk∼N(0,Rk).\mathbf{z}_k=\mathbf{H}_k\mathbf{x}_k+\mathbf{v}_k, \qquad \mathbf{v}_k\sim\mathcal{N}(\mathbf{0},\mathbf{R}_k).

A\mathbf{A} advances the old state through the physical model, B\mathbf{B} maps a measured or commanded input into that state, and H\mathbf{H} selects the combination the sensor can observe. The process disturbance w\mathbf{w} covers model error; the measurement disturbance v\mathbf{v} covers sensor error. Their covariance matrices Q\mathbf{Q} and R\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 bb while the measured gyro rate ωm\omega_m acts as the input:

xk=[θkbk],A=[1−Δt01],B=[Δt0],uk=[ωm,k].\mathbf{x}_k= \begin{bmatrix}\theta_k\\b_k\end{bmatrix}, \quad \mathbf{A}= \begin{bmatrix}1&-\Delta t\\0&1\end{bmatrix}, \quad \mathbf{B}= \begin{bmatrix}\Delta t\\0\end{bmatrix}, \quad \mathbf{u}_k=\begin{bmatrix}\omega_{m,k}\end{bmatrix}.

The first row says θk≈θk−1+Δt(ωm,k−bk−1)\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 zk=θacc,kz_k=\theta_{acc,k},

H=[10],\mathbf{H}=\begin{bmatrix}1&0\end{bmatrix},

because the measurement directly observes angle, not gyro bias. With Δt=0.01\Delta t=0.01 s, previous state [10.0∘,0.2∘/s]T[10.0^\circ,0.2^\circ/\text{s}]^T, and measured rate 5.0∘/s5.0^\circ/\text{s}, the prediction is 10.0+0.01(5.0−0.2)=10.048∘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.

← Back to the chapter · Next detail →