The result of going 1000Hz is… total disaster. Rotating the quad 90 degrees results in a 105 degree rotation in the AHRS so I either integrate too much rate or I’m doing it wrong.

Digging through the internets I found these:

http://physicsforgames.blogspot.ro/2010/02/quaternions.html

http://www.euclideanspace.com/maths/differential/other/quaternioncalculus/

Long story short, I was doing it wrong. My current code is as follows:

//integrate gyros

if (gyroscope.sample_idx != m_last_gyroscope_sample_idx)

{

m_last_gyroscope_sample_idx = gyroscope.sample_idx;

auto av = gyroscope.value.angular_velocity*dt*0.5f;

auto hd = av; //the rate quat

auto const& a = m_local_to_world_quat;

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<math::safe>();

}

I’m basically treating the angular velocity as the axis of a quaternion and adding it to the current rotation. The correct way – apparently – is to build the hd quaternion like this:

float l = math::length(av);

float s = sin(l) / l;

hd.x = av.x * s;

hd.y = av.y * s;

hd.z = av.z * s;

hd.w = cos(l);

I kind of get why – the rates can be expressed as an angle axis rotation – with the angle == length(av) and axis == av. But not sure why the sin is divided by the length.

Well, I’ll see tomorrow.