# 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.