3

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 this 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 Balance Filter, only instead of only implemented in 3D

Initialization Step:

  • Initialize a world rotation matrix using the (noisy) accelerometer and compass sensor data (this is provided by the Android already)

Update Steps:

  • Integrate the gyroscope reading (time_delta * reading) for each axis (x, y, z)

  • Rotate the world rotation matrix using the Euler angles supplied by the integration

  • Find the Quaternion from the newly rotated matrix

  • Find the rotation matrix from the unfiltered accelerometer + compass data (using the OS provided function, I think it uses angle/axis calculation)

  • Get the quaternion from the matrix generated in the previous step.

  • Slerp between quaternion generated in step 2 (from the gyroscope), and the accelerometer data using a coefficient based on some experimental magic

  • Convert back to a matrix and use that to draw the scene.

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.

Community
  • 1
  • 1
svm_invictvs
  • 31
  • 1
  • 3
  • Your question is really too general and vague to answer well in this format. If you want a survey of the pros and cons of different approaches, then use Goggle, Wikipedia or a good game developers forum for pointers to the many essays available. Otherwise, try to ask a more specific question. – RBarryYoung Oct 17 '11 at 00:16
  • 1
    Yeah, agreed. But I was hesitant to post the code :( However I did find a solution :D – svm_invictvs Oct 20 '11 at 01:32
  • How did u do in the end? Do u simply use euler angles? Do u suffer from the gimbal lock? – Sibbs Gambling Sep 03 '13 at 07:50

2 Answers2

4

The biggest reason to use quaternions is to avoid the singularity problem with Euler angles. You can directly rotate a quaternion with gyro data.

rawr
  • 41
  • 2
2

Many appologies if information is delayed or not useful specifically but may be useful to others as I found it after some research:::

a. Using a kalman (linear or non linear) filter you do following ::

Gyro to integrate the delta angle while accelerometers tell you the outer limit.

b. Euler rates are different from Gyro rate of angle change so you ll need quaternion or Euler representation::

Quaternion is non trivial but two main steps are ----

1. For Roll, pitch,yaw   you get three quaternions as cos(w) +sin(v) where w is scalar part and v is vector part (or when coding just another variable) 
 Then simply multiply all 3 quat. to get a delta quaternion
 i.e   quatDelta[0] =c1c2*c3 - s1s2*s3;
  quatDelta[1] =c1c2*s3 + s1s2*c3;
  quatDelta[2] =s1*c2*c3 + c1*s2*s3;
  quatDelta[3] =c1*s2*c3 - s1*c2*s3;
where c1,c2,c3 are cos of roll,pitch,yaw and s stands for sin of the same actually half of those gyro pre integrated angles.

2. Then just multiply by old quaternion you had

  newQuat[0]=(quaternion[0]*quatDelta[0] - quaternion[1]*quatDelta[1] - quaternion[2]*quatDelta[2] - quaternion[3]*quatDelta[3]);
  newQuat[1]=(quaternion[0]*quatDelta[1] + quaternion[1]*quatDelta[0] + quaternion[2]*quatDelta[3] - quaternion[3]*quatDelta[2]);
  newQuat[2]=(quaternion[0]*quatDelta[2] - quaternion[1]*quatDelta[3] + quaternion[2]*quatDelta[0] + quaternion[3]*quatDelta[1]);
  newQuat[3]=(quaternion[0]*quatDelta[3] + quaternion[1]*quatDelta[2] - quaternion[2]*quatDelta[1] + quaternion[3]*quatDelta[0]);

As you loop through the code it gets updated so only quatenion is a global variables not the rest

3. Lastly if you want Euler angles from them then do the following:


`euler[2]=atan2(2.0*(quaternion[0]*quaternion[1]+quaternion[2]*quaternion[3]), 1-2.0*(quaternion[1]*quaternion[1]+quaternion[2]*quaternion[2]))euler[1]=safe_asin(2.0*(quaternion[0]*quaternion[2] - quaternion[3]*quaternion[1]))euler[0]=atan2(2.0*(quaternion[0]*quaternion[3]+quaternion[1]*quaternion[2]), 1-2.0*(quaternion[2] *quaternion[2]+quaternion[3]*quaternion[3]))`

euler[1] is pitch and so on..

I just wanted to outline general steps of quaternion implementation. There may be some minor errors but I tried this myself and it works. Please note that when changing to euler angles you will get singularities also called as "Gimbal lock"

An important note here is that this is not my work but I found it over the internet and wanted to thank who ever did this priceless code...Cheers

tej
  • 21
  • 2
  • What about the order of roll,pitch,yaw quaternion multiplication because roll x pitch is not equal to pitch x roll – user818117 Jul 10 '17 at 19:08