# Integrating gyros and complimentary filters

I’m integrating the gyro angular rate into a quaternion at 500Hz with this code:

``` auto av = gyroscope.value.angular_velocity*dt; auto const& a = m_local_to_world_quat; auto hd = av*0.5f; float w = /*(hd.w * a.w)*/ - (hd.x * a.x) - (hd.y * a.y) - (hd.z * a.z); float x = (hd.x * a.w) /*+ (hd.w * a.x)*/ + (hd.z * a.y) - (hd.y * a.z); float y = (hd.y * a.w) /*+ (hd.w * a.y)*/ + (hd.x * a.z) - (hd.z * a.x); float z = (hd.z * a.w) /*+ (hd.w * a.z)*/ + (hd.y * a.x) - (hd.x * a.y); m_local_to_world_quat.x += x; m_local_to_world_quat.y += y; m_local_to_world_quat.z += z; m_local_to_world_quat.w += w; m_local_to_world_quat.normalize();```

It gives me a very accurate rotation with very little drift – if the sensor is properly calibrated.

To eliminate the drift I use a complimentary filter between the gyro quaternion and another (noisy) quaternion built from the compass and accelerometer. The gyro quat captures high frequency rotation and the accelerometer/compass quat captures low frequency rotations but is very noisy on short term. To combine them I do this:

``` m_local_to_world_quat = math::lerp(m_local_to_world_quat, noisy_quat, mu); ```

Where mu is the interpolation factor. Initially I used 0.01, big enough to compensate for gyro drift and gyro cross-axis winding and saturation. But it’s too big and the m_local_to_world quat catches a bit of accelerometer noise.
I switched to a variable mu and so far it seems much better:
``` float mu = 0.001f + math::length(av); ```

The 0.001 is big enough to compensate for idle drift – but not too big. The av is the gyro rate sample – so the bigger the rotation rate, the bigger the mu.