Integrating gyros #2

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.

 

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s