← Back to the chapter · Previous detail · Next detail →

Technical detail · Complementary Filters: Tuning and Validity

Follow the Kalman recursion and covariance checks

The complete recursion is:

x^k−=Ax^k−1+Buk,Pk−=APk−1AT+Q,\hat{\mathbf{x}}^-_k=\mathbf{A}\hat{\mathbf{x}}_{k-1}+\mathbf{B}\mathbf{u}_k, \qquad \mathbf{P}^-_k=\mathbf{A}\mathbf{P}_{k-1}\mathbf{A}^T+\mathbf{Q}, yk=zk−Hx^k−,Sk=HPk−HT+R,\mathbf{y}_k=\mathbf{z}_k-\mathbf{H}\hat{\mathbf{x}}^-_k, \qquad \mathbf{S}_k=\mathbf{H}\mathbf{P}^-_k\mathbf{H}^T+\mathbf{R}, Kk=Pk−HTSk−1,\mathbf{K}_k=\mathbf{P}^-_k\mathbf{H}^T\mathbf{S}_k^{-1}, x^k=x^k−+Kkyk,Pk=(I−KkH)Pk−.\hat{\mathbf{x}}_k=\hat{\mathbf{x}}^-_k+\mathbf{K}_k\mathbf{y}_k, \qquad \mathbf{P}_k=(\mathbf{I}-\mathbf{K}_k\mathbf{H})\mathbf{P}^-_k.

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 RR rises during vibration, the residual variance grows and the gain normally gives that angle less influence. If process covariance QQ is too small, the filter becomes overconfident in an imperfect motion/bias model and responds sluggishly to real change.

Matrix or recordIMU meaningEvidence used to set it
A,B\mathbf{A},\mathbf{B}Sample-period motion and bias modelTimestamped update rate and chosen physical state
H\mathbf{H}Which state combination the accelerometer angle observesAxis convention and angle derivation
Q\mathbf{Q}Gyro integration/model and bias random-walk uncertaintyStationary and temperature-varying gyro data
R\mathbf{R}Accelerometer-angle measurement uncertaintyStatic orientation, vibration, and linear-acceleration tests
P0\mathbf{P}_0Initial uncertainty in angle and biasStartup alignment and calibration procedure

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

Pk=(I−KkHk)Pk−(I−KkHk)T+KkRkKkT.\boxed{\mathbf{P}_k=(\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)\mathbf{P}^-_k(\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)^T+\mathbf{K}_k\mathbf{R}_k\mathbf{K}_k^T.}

← Back to the chapter · Previous detail · Next detail →