title: Finding a Quaternion from Gyroscope Data?
body: <p>I've been trying to build a filter that can successfully combine compass, geomagnetic, and gyroscopic data to produce a smooth augmented reality experience.  After reading <a href="https://stackoverflow.com/questions/1586658/combine-gyroscope-and-accelerometer-data">this</a> post along with lots of discussions, I finally found out a good algorithm to correct my sensor data.  Most examples I've read show how to correct accelerometers with gyroscopes, but not correct compass + accelerometer data with gyroscope.  This is the algorithm I've settled upon, which works great except that I run into gimbal lock if I try to look at the scene if I'm not facing North.  This algorithm is <a href="http://web.mit.edu/scolton/www/filter.pdf" rel="nofollow noreferrer">Balance Filter</a>, only instead of only implemented in 3D</p>
  
  <p>Initialization Step:</p>
  
  <ul>
  <li>Initialize a world rotation matrix using the (noisy) accelerometer and compass sensor data (this is provided by the Android already)</li>
  </ul>
  
  <p>Update Steps:</p>
  
  <ul>
  <li><p>Integrate the gyroscope reading (time_delta * reading) for each axis (x, y, z)</p></li>
  <li><p>Rotate the world rotation matrix using the Euler angles supplied by the integration</p></li>
  <li><p>Find the Quaternion from the newly rotated matrix</p></li>
  <li><p>Find the rotation matrix from the unfiltered accelerometer + compass data (using the OS provided function, I think it uses angle/axis calculation)</p></li>
  <li><p>Get the quaternion from the matrix generated in the previous step.</p></li>
  <li><p>Slerp between quaternion generated in step 2 (from the gyroscope), and the accelerometer data using a coefficient based on some experimental magic</p></li>
  <li><p>Convert back to a matrix and use that to draw the scene.</p></li>
  </ul>
  
  <p>My problem is that when I'm facing North and then try to look south, the whole thing blows up and it appears to be gimbal lock.  After a few gimbal locks, the whole filter is in an undefined state.  Searching around I hear everybody saying "Just use Quaternions" but I'm afraid it's not that simple (at least not to me) and I know there's something I'm just missing.  Any help would be greatly appreciated.</p>