At Heading 180 degrees, Heading-Pitch-Roll angles becoming random values in sensor fusion software.

[venkidravid]

Hi,

I am using open source NXP fusion software. Whenever i rotating module "frdmk64f_agm01_sensorfusion" such that heading pass through 180 degrees and when heading reaches exactly 180.00, then heading,pitch, roll angles sometimes becomes zero sometimes random values(heading is drfited by 10/15/20 etc.. Roll and Pitch drift by 0.3/0.5/1 degrees ).

When i go through source code, in orientation.c  file and in function "fQuaternionFromRotationMatrix"

whenever Quternion scalar component q0 is very small (<  e-4) fall back algorithm/calculation is there for q1,q2,q3 components.  In source code in comment written as "special case of near 180 deg corresponds to nearly symmetric matrix which is not numerically well conditioned for division by small q0, instead get absolute values of q1 to q3 from leading diagonal".

During debugging i found this scalar component q0 is becoming zero in my case (when heading is exactly at 180.00 ), so fallback calculation comes into picture and then pitch,roll, heading are being drifted.

Is this fallback calculation gives error or i am doing anything wrong?

I am stuck this problem, please help me to get out of this problem.

Thanks in advance,

venkat

[AmitPurohit · Accepted solution]

Hello Venkat,

Thank you for posting your question.

The Sensor Fusion Library software is numerically accurate to the limits of single precision floating point arithmetic. Conditions that lead to inaccurate results are detected and the results computed using an alternative algorithm suitable for those cases where the primary algorithm fails. An example is the calculation of the orientation quaternion from a rotation matrix where the primary algorithm differencing elements across the diagonal approaches a 0/0 calculation. The alternative algorithm used near 180 degree rotations operates on the matrix diagonal elements instead.

Try running magnetic calibration and precision accelerometer 12-point calibration, to check whether you see better results.

Regards,

Amit Purohit.