📣📪 🚚 Adafruit will not be shipping USPS orders Monday October 14, 2019 🗓. Expedited USPS orders placed after 11am ET 🕚 Friday October 11 will go out Tuesday October 15 📣📪🚚
0

AHRS Angle Calculation
Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.

AHRS Angle Calculation

by jzelina on Sun Jul 07, 2019 3:38 pm

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

by jps2000 on Mon Jul 08, 2019 12:58 am

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: 456
Joined: Fri Jun 02, 2017 4:12 pm

Re: AHRS Angle Calculation

by jzelina on Tue Jul 09, 2019 9:50 pm

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

by jps2000 on Wed Jul 10, 2019 12:49 am

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: 456
Joined: Fri Jun 02, 2017 4:12 pm

Re: AHRS Angle Calculation

by jzelina on Wed Aug 07, 2019 8:31 am

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.