Looking for a quaternion from gyroscope data?

Question

I am trying to create a filter that can successfully combine compass data, geomagnetic and gyroscopic information to provide a smooth experience of augmented reality. After reading this publication, along with a lot of discussion, I finally found a good algorithm for correcting my sensor data. Most of the examples I read show how to adjust accelerometers with gyroscopes, but it’s not right to compare compass and accelerometer data with a gyroscope. This is the algorithm I stopped at, which works fine, except that I encounter a cardan lock if I try to look at the scene if I do not encounter the North. This algorithm is a balance filter , only instead of implementing only 3D

Initialization Step:

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

Update steps:

• Integrate gyro reading (delta * countdown) for each axis (x, y, z)

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

• Find the quaternion from the newly rotated matrix

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

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

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

• Convert back to a matrix and use it to paint a scene.

My problem is that when I run into the North and then try to look south, it all explodes, and it looks like a gimbal. After several locking locks, the entire filter is in an undefined state. Looking around, I hear everyone say, “Just use the quaternions,” but I’m afraid it’s not so easy (at least not for me), and I know that something is just missing me. Any help would be greatly appreciated.

Answer 1

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

Answer 2

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

but. Using a Kalman (linear or non-linear) filter, you perform the following actions:

Gyroscope for integrating the delta angle, while accelerometers tell you about the outer limit.

b. Euler frequencies are different from the rate of change of the gyroscope angle, so you need a quaternion or Euler’s representation:

Quaternion is not trivial, but two main steps: ----

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 ie 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 progress through the code, it is updated, so only quatenion are 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 describe the general steps for implementing a quaternion. There may be some minor errors, but I tried this myself and it works. Please note that when you change the Euler angles, you will get singularities, also called "Gimbal lock"

It’s important to note that this is not my job, but I found it on the Internet and wanted to thank who ever made this priceless code ... Cheers