← 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 containing the physical quantities to estimate at sample k, a control input uk, and a measurement zk:
xk=Akxk−1+Bkuk+wk,wk∼N(0,Qk),
zk=Hkxk+vk,vk∼N(0,Rk).
A advances the old state through the physical model, B maps a measured or commanded input into that state, and H selects the combination the sensor can observe. The process disturbance w covers model error; the measurement disturbance v covers sensor error. Their covariance matrices Q and R describe expected variance and cross-correlation, not arbitrary “trust knobs.”
For one IMU tilt axis, a useful state estimates angle θ and gyroscope bias b while the measured gyro rate ωm acts as the input:
xk=[θkbk],A=[10−Δt1],B=[Δt0],uk=[ωm,k].
The first row says θk≈θk−1+Δt(ωm,k−bk−1); the second models bias as slowly varying. When the accelerometer supplies a roll or pitch observation zk=θacc,k,
H=[10],
because the measurement directly observes angle, not gyro bias. With Δt=0.01 s, previous state [10.0∘,0.2∘/s]T, and measured rate 5.0∘/s, the prediction is 10.0+0.01(5.0−0.2)=10.048∘. 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 →