## AHRS Angle Calculation Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.

AHRS Angle Calculation

Hello,

I am attempting to use a NXP 9-DOF w/ FXOS8700 + FXAS21002 (Product ID 3463) with the AHRS fusion code provided by Adafruit to determine the angle between my board when moved from a starting position to a new position using quaternions and the Madgwick filter option. Taking the starting position values (a1, b1, c1, d1) and the end position values (a2, b2, c2, d2), I am plugging them into the following formula to return the relative angle between the two positions: theta = 2*arccos(abs(a1*a2+b1*b2+c1*c2+d1*d2))

To test the accuracy of this method, I have sketched angles on a sheet of paper (15, 30, 45, 90) and am seeing how close I can get to the actual values by moving the board between the angles. For 45 & 90 I get values that are very close to actual, but for 15 and 30 I get approximately double the angle. I'm assuming this has something to do with the fact that I'm using quaternions and the math behind them, but I can't figure it out.

Is this a common problem with quaternions? Any and all help would be greatly appreciated. Thanks!

jzelina

Posts: 4
Joined: Sun Jul 07, 2019 3:20 pm

Re: AHRS Angle Calculation

Yes it is the dot product
You must not use abs in the formula. Also use 2.0f instead of 2 to make sure it is a float multiplication. ( I am always not sure so I use in any formula this 2.0f notification to avoid integer multiplications.
Note that quats need to be unit vector.

I am using the following code:
Code: Select all | TOGGLE FULL SIZE
`   #define radtodeg                    (180.0f / PI)//**************** dphi calculation************  float argument = (QT0 * Q0 + QT1 * Q1 + QT2 * Q2 + QT3 * Q3);     // calculate delta angle between quats  if (abs(argument) > 1) argument = 1.0f;                                               // check for N/A    if(asin(argument) > 0) dphi = 2.0f * acos(argument);                else dphi = 2.0f * (PI - acos(argument));                                           // in case rotation is >360 deg  (quat flip signs)    dphi *= radtodeg;                                                                              // conversion to deg`

This gives the minimum angle between two orientations in 3d space. It works pretty well.

jps2000

Posts: 497
Joined: Fri Jun 02, 2017 4:12 pm

Re: AHRS Angle Calculation

Thank you very much for the reply, I see where I was going wrong. Using your code, I am able to get about +/- 4 degrees but I really need +/- 1 degree for this to work for my application. Is it possible to achieve this precision? I've calibrated my magnometer, are there other steps I can take to get better?

jzelina

Posts: 4
Joined: Sun Jul 07, 2019 3:20 pm

Re: AHRS Angle Calculation

I tried many of IMU´s and fusion software.
The Madgwick algo allows tuning of some parameters but was never as good as the BNO080- my favorite.
You might be interested in this:
https://github.com/jps2000/BNO080

However, to achieve +/-1 degree is borderline. I do not know your application but it may be better to go for 6dof. (Magnetometers indoor may cause troubles). In particular if you periodically come back to a reference position (about minutes) 6dof works great.

jps2000

Posts: 497
Joined: Fri Jun 02, 2017 4:12 pm

Re: AHRS Angle Calculation

Thanks again for the reply and information. The BNO080 sounds interesting and I have purchased one to try out.

jzelina

Posts: 4
Joined: Sun Jul 07, 2019 3:20 pm

Please be positive and constructive with your questions and comments.