Filtering

Using the detected drift error, filtering works in the same way as described in Section 9.1. The complementary filter (9.10) is upgraded to work with quaternions. It becomes slightly more complicated to represent the interpolation in terms of $\alpha$. Let $(v,\theta)$ denote the axis-angle representation of the orientation $\hat{d}[k]$, which is the estimated drift error (a quaternion value). Let $q(v,\alpha \theta)$ represent the quaternion given by axis $v$ and angle $\alpha\theta$. For a small value of $\alpha$, this can be considered as a small step ``toward'' $\hat{d}[k]$.

The complementary filter in terms of quaternions becomes

$$\hat{q}_c[k] = q(v,-\alpha \theta) * \hat{q}[k] ,$$ (9.21)

which is similar in form to (9.12). The simple subtraction from the 2D case has been replaced above by multiplying an inverse quaternion from the left. The estimated drift error $\hat{d}[k]$ is obtained by multiplying the estimated tilt and yaw errors. Alternatively, they could contribute separately to the complementary filter, with different gains for each, and even combined with drift error estimates from more sources [199].