Voting resources, early voting, and poll worker information - VOTE. ... Adafruit is open and shipping.
0

Calculate Earth Referenced Acceletation with BNO055
Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.

Calculate Earth Referenced Acceletation with BNO055

by bugratufan on Thu Mar 23, 2017 8:00 am

Hello,
I bought an IMU ( BNO055) and I need to calculate earth referenced acceleration. How can I do that?

Thanks in advance.

bugratufan
 
Posts: 8
Joined: Fri Mar 17, 2017 9:31 am

Re: Calculate Earth Referenced Acceletation with BNO055

by adafruit_support_bill on Thu Mar 23, 2017 8:07 am

We have detailed guides for using this sensor with Arduino, Raspberry Pi and Beagle Bones:
https://learn.adafruit.com/adafruit-bno ... n?view=all
https://learn.adafruit.com/bno055-absol ... e?view=all

adafruit_support_bill
 
Posts: 78759
Joined: Sat Feb 07, 2009 10:11 am

Re: Calculate Earth Referenced Acceletation with BNO055

by bugratufan on Thu Mar 23, 2017 8:24 am

adafruit_support_bill wrote:We have detailed guides for using this sensor with Arduino, Raspberry Pi and Beagle Bones:
https://learn.adafruit.com/adafruit-bno ... n?view=all
https://learn.adafruit.com/bno055-absol ... e?view=all


I used this formula but I think it doesn't work. I compared raw acceleration data with quaternion calculated acceleration data. But there are not similar. Actually I need to transform acceleration data to earth cooordinate system. I need to fetch my robots acceleration in 3 dimension.

Code: Select all | TOGGLE FULL SIZE
imu::Quaternion quat = bno.getQuat();
  quat.x() = -quat.x();
  quat.y() = -quat.y();
  quat.z() = -quat.z();
  // fetch linear acceleration (excludes gravity)
  imu::Vector<3> linearaccel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);

  // compute earth-referenced acceleration (excludes sensor tilt)

  acc[0] = (1-2*(quat.y()*quat.y() + quat.z()*quat.z()))*linearaccel[0] +   (2*(quat.x()*quat.y() + quat.w()*quat.z()))*linearaccel[1] +   (2*(quat.x()*quat.z() - quat.w()*quat.y()))*linearaccel[2];  // rotate linearaccel by quaternion
  acc[1] =   (2*(quat.x()*quat.y() - quat.w()*quat.z()))*linearaccel[0] + (1-2*(quat.x()*quat.x() + quat.z()*quat.z()))*linearaccel[1] +   (2*(quat.y()*quat.z() + quat.w()*quat.x()))*linearaccel[2];
  acc[2] =   (2*(quat.x()*quat.z() + quat.w()*quat.y()))*linearaccel[0] +   (2*(quat.y()*quat.z() - quat.w()*quat.x()))*linearaccel[1] + (1-2*(quat.x()*quat.x() + quat.y()*quat.y()))*linearaccel[2];

bugratufan
 
Posts: 8
Joined: Fri Mar 17, 2017 9:31 am

Re: Calculate Earth Referenced Acceletation with BNO055

by adafruit_support_bill on Thu Mar 23, 2017 8:29 am

I compared raw acceleration data with quaternion calculated acceleration data. But there are not similar.

Please post an example of the data you are referring to.

adafruit_support_bill
 
Posts: 78759
Joined: Sat Feb 07, 2009 10:11 am

Re: Calculate Earth Referenced Acceletation with BNO055

by bugratufan on Thu Mar 23, 2017 8:43 am

adafruit_support_bill wrote:
I compared raw acceleration data with quaternion calculated acceleration data. But there are not similar.

Please post an example of the data you are referring to.



I want to transform bno055's acceleration data to my coordianate system whichever rotations when my sensors turns. Because ı calculate position with this acceleration data.
Attachments
WhatsApp Image 2017-03-23 at 15.36.41.jpeg
WhatsApp Image 2017-03-23 at 15.36.41.jpeg (54.01 KiB) Viewed 1543 times

bugratufan
 
Posts: 8
Joined: Fri Mar 17, 2017 9:31 am

Re: Calculate Earth Referenced Acceletation with BNO055

by adafruit_support_bill on Thu Mar 23, 2017 8:52 am

I want to transform bno055's acceleration data to my coordianate system whichever rotations when my sensors turns. Because ı calculate position with this acceleration data.

Are you trying to calculate position or orientation? The BNO055 can give you orientation directly in Euler or Quaternion form.

If you want to calculate position, you need to do a double integration on the raw data. Due to the double integration, this is subject to cumulative errors and not likely to be accurate over the long-term.

adafruit_support_bill
 
Posts: 78759
Joined: Sat Feb 07, 2009 10:11 am

Re: Calculate Earth Referenced Acceletation with BNO055

by bugratufan on Thu Mar 23, 2017 9:01 am

adafruit_support_bill wrote:
I want to transform bno055's acceleration data to my coordianate system whichever rotations when my sensors turns. Because ı calculate position with this acceleration data.

Are you trying to calculate position or orientation? The BNO055 can give you orientation directly in Euler or Quaternion form.

If you want to calculate position, you need to do a double integration on the raw data. Due to the double integration, this is subject to cumulative errors and not likely to be accurate over the long-term.



But x,y and z directions of position may depend on bno055's physical positon. First I will put bno055 on first object then I will reset it. Later, I will put this on the second object then, I will calculate the distance between two objects.

bugratufan
 
Posts: 8
Joined: Fri Mar 17, 2017 9:31 am

Re: Calculate Earth Referenced Acceletation with BNO055

by adafruit_support_bill on Thu Mar 23, 2017 9:10 am

To estimate the distance between the two objects, you would need to do a double integration of the raw data through the duration of the move from the first location to the second location. You would need to integrate both the gyro data to account for any rotation along the way, as well as the acceleration data to capture changes in velocity.

adafruit_support_bill
 
Posts: 78759
Joined: Sat Feb 07, 2009 10:11 am

Re: Calculate Earth Referenced Acceletation with BNO055

by gammaburst on Thu Mar 23, 2017 10:16 pm

Hi bugratufan,

That looks like a code snippet from one of my examples.

To transform the BNO's sensor-referenced linear acceleration to an earth-referenced linear acceleration, rotate the BNO's linear acceleration by the conjugate of the BNO's normalized quaternion. That's what my code snippet should do.

Here's a working example that outputs earth-referenced linear acceleration:
- With sensor at rest, see zero acceleration.
- Shake sensor east-west, see only X acceleration.
- Shake sensor north-south, see only Y acceleration.
- Shake sensor up-down, see only Z acceleration.
- The output acceleration vector is now unaffected by sensor orientation.
- Tested with Arduino Uno + BNO055.
- Remember to calibrate the sensor.
- My axes names may be arranged differently than yours.

Code: Select all | TOGGLE FULL SIZE
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>

#define BNO055_SAMPLERATE_PERIOD_MS 10

Adafruit_BNO055 bno = Adafruit_BNO055(55);

unsigned long tnext, tnow;

void setup(void)
{
  Serial.begin(115200);
  Serial.println("Orientation Sensor Test"); Serial.println("");

  /* Initialise the sensor */
  if(!bno.begin())
  {
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }
  tnext = millis() + 100;
}

void loop(void)
{
  // fetch quaternion, negative
  imu::Quaternion quat = bno.getQuat();
  quat.x() = -quat.x();
  quat.y() = -quat.y();
  quat.z() = -quat.z();

  // fetch linear acceleration (excludes gravity)
  imu::Vector<3> linearaccel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);

  // compute earth-referenced acceleration (excludes sensor tilt)
  imu::Vector<3> acc;
  acc[0] = (1-2*(quat.y()*quat.y() + quat.z()*quat.z()))*linearaccel[0] +   (2*(quat.x()*quat.y() + quat.w()*quat.z()))*linearaccel[1] +   (2*(quat.x()*quat.z() - quat.w()*quat.y()))*linearaccel[2];  // rotate linearaccel by quaternion
  acc[1] =   (2*(quat.x()*quat.y() - quat.w()*quat.z()))*linearaccel[0] + (1-2*(quat.x()*quat.x() + quat.z()*quat.z()))*linearaccel[1] +   (2*(quat.y()*quat.z() + quat.w()*quat.x()))*linearaccel[2];
  acc[2] =   (2*(quat.x()*quat.z() + quat.w()*quat.y()))*linearaccel[0] +   (2*(quat.y()*quat.z() - quat.w()*quat.x()))*linearaccel[1] + (1-2*(quat.x()*quat.x() + quat.y()*quat.y()))*linearaccel[2];

  Serial.print("EarthRefAccel:");
  for (int n=0; n<3; n++)
  {
    String s = String(acc[n]);
    for (int m=s.length(); m<7; m++)
      Serial.print(" ");
    Serial.print(s);
  }
  Serial.println("");

  tnow = millis();
  delay(tnext - tnow);
  tnext += BNO055_SAMPLERATE_PERIOD_MS;
}
If that's not the transformation you want, please explain more clearly.

Using the BNO055's fusion outputs (such as linear acceleration) to compute velocity or displacement generally gives unsatisfactory results. The BNO is not a precision instrument. If you somehow achieve good results, please share your technique.

gammaburst
 
Posts: 486
Joined: Thu Dec 31, 2015 12:06 pm

Re: Calculate Earth Referenced Acceletation with BNO055

by bugratufan on Fri Mar 24, 2017 5:44 am

gammaburst wrote:Hi bugratufan,

That looks like a code snippet from one of my examples.

To transform the BNO's sensor-referenced linear acceleration to an earth-referenced linear acceleration, rotate the BNO's linear acceleration by the conjugate of the BNO's normalized quaternion. That's what my code snippet should do.

Here's a working example that outputs earth-referenced linear acceleration:
- With sensor at rest, see zero acceleration.
- Shake sensor east-west, see only X acceleration.
- Shake sensor north-south, see only Y acceleration.
- Shake sensor up-down, see only Z acceleration.
- The output acceleration vector is now unaffected by sensor orientation.
- Tested with Arduino Uno + BNO055.
- Remember to calibrate the sensor.
- My axes names may be arranged differently than yours.

Code: Select all | TOGGLE FULL SIZE
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>

#define BNO055_SAMPLERATE_PERIOD_MS 10

Adafruit_BNO055 bno = Adafruit_BNO055(55);

unsigned long tnext, tnow;

void setup(void)
{
  Serial.begin(115200);
  Serial.println("Orientation Sensor Test"); Serial.println("");

  /* Initialise the sensor */
  if(!bno.begin())
  {
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }
  tnext = millis() + 100;
}

void loop(void)
{
  // fetch quaternion, negative
  imu::Quaternion quat = bno.getQuat();
  quat.x() = -quat.x();
  quat.y() = -quat.y();
  quat.z() = -quat.z();

  // fetch linear acceleration (excludes gravity)
  imu::Vector<3> linearaccel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);

  // compute earth-referenced acceleration (excludes sensor tilt)
  imu::Vector<3> acc;
  acc[0] = (1-2*(quat.y()*quat.y() + quat.z()*quat.z()))*linearaccel[0] +   (2*(quat.x()*quat.y() + quat.w()*quat.z()))*linearaccel[1] +   (2*(quat.x()*quat.z() - quat.w()*quat.y()))*linearaccel[2];  // rotate linearaccel by quaternion
  acc[1] =   (2*(quat.x()*quat.y() - quat.w()*quat.z()))*linearaccel[0] + (1-2*(quat.x()*quat.x() + quat.z()*quat.z()))*linearaccel[1] +   (2*(quat.y()*quat.z() + quat.w()*quat.x()))*linearaccel[2];
  acc[2] =   (2*(quat.x()*quat.z() + quat.w()*quat.y()))*linearaccel[0] +   (2*(quat.y()*quat.z() - quat.w()*quat.x()))*linearaccel[1] + (1-2*(quat.x()*quat.x() + quat.y()*quat.y()))*linearaccel[2];

  Serial.print("EarthRefAccel:");
  for (int n=0; n<3; n++)
  {
    String s = String(acc[n]);
    for (int m=s.length(); m<7; m++)
      Serial.print(" ");
    Serial.print(s);
  }
  Serial.println("");

  tnow = millis();
  delay(tnext - tnow);
  tnext += BNO055_SAMPLERATE_PERIOD_MS;
}
If that's not the transformation you want, please explain more clearly.

Using the BNO055's fusion outputs (such as linear acceleration) to compute velocity or displacement generally gives unsatisfactory results. The BNO is not a precision instrument. If you somehow achieve good results, please share your technique.


Hello gammaburst,
First of all, thanks for your reply. This code snipped is from you. Your quartenion formula is works so good. But I need to calculate distance between two objects. First, I calculate earth referenced acceleration with your quartenion formula. Then I calculate double integration of each direction. But, this integration didn't give me a close position. Actually, it's so wrong result.
I don't need to calculate absolute position but it must be consistent. Gammaburst, you look like wise. I am waiting your reply impatiently.

bugratufan
 
Posts: 8
Joined: Fri Mar 17, 2017 9:31 am

Re: Calculate Earth Referenced Acceletation with BNO055

by gammaburst on Fri Mar 24, 2017 7:12 am

Oh, when you said "I used this formula but I think it doesn't work", I tried to clarify its purpose. Now I see your problem is something else.

When I place the BNO055 on a table and display its Linear Acceleration data, I see the values are slightly offset from zero. I pick up the BNO, wiggle it slightly, and put it back down. The offsets are now different. The BNO keeps changing its mind! Such erratic behavior spoils any hope of calculating velocity or displacement.

Instead of using the BNO's erratic fusion outputs (linear acceleration, gravity, quaternion, euler), you might get better results by reading its raw sensors (accelerometer gyroscope and possibly magnetometer) and doing your own calibration and AHRS math. I have not tried doing that, and I don't know if the results will be better. One possible starting place would be the Mahony library: https://learn.adafruit.com/ahrs-for-ada ... algorithms
Beware, this approach probably has a steeper learning curve!
Last edited by gammaburst on Fri Mar 24, 2017 7:19 am, edited 1 time in total.

gammaburst
 
Posts: 486
Joined: Thu Dec 31, 2015 12:06 pm

Re: Calculate Earth Referenced Acceletation with BNO055

by bugratufan on Fri Mar 24, 2017 7:18 am

Thanks for your help. I will try it and I will tell the result.

bugratufan
 
Posts: 8
Joined: Fri Mar 17, 2017 9:31 am

Re: Calculate Earth Referenced Acceletation with BNO055

by slinger87 on Wed Jun 28, 2017 4:03 pm

Hi Gammaburst,

I have one small question.

When browsing the forum for information about the quaternion derived rotation matrix, I found your help very useful.

But I also found multiple "+/-" variations of the rotation matrix. There is a difference how the function assign the +/- signs. Not just the inverse, but also the order. Take the 2 examples for the rotation matrix you provided.

I'm not really a mathematic so I can't make any sense of it.

Thanks!!

slinger87
 
Posts: 2
Joined: Wed Jun 28, 2017 3:29 pm

Re: Calculate Earth Referenced Acceletation with BNO055

by gammaburst on Wed Jun 28, 2017 9:52 pm

Seems like everyone uses different axes conventions. Confusing

Sometimes I need to convert from unclear or undocumented axes into familiar aerospace axes conventions, or sometimes into another project's unusual axes. Many times I've done trial-and-error axes flipping and swapping (with occasional educated guesses) until everything rotates and moves correctly.

Can you clarify which of my examples you're looking at, and your goals?

gammaburst
 
Posts: 486
Joined: Thu Dec 31, 2015 12:06 pm

Re: Calculate Earth Referenced Acceletation with BNO055

by slinger87 on Thu Jun 29, 2017 11:45 am

Hi Gammaburst,

Thanks for your response. My goal is to rotate linear acceleration from body to earth reference frame. I am testing with the BNO055 now, but as you stated earlier, the Sensor Fusion is a bit erratic. So I want to use my good old mpu9250. I also like to understand a bit of the underlaying theory. I want to use a quaternion derived rotation matrix, which I like to calculate myself.

For example, this I an other version of the rotation matrix you provided:

Code: Select all | TOGGLE FULL SIZE
void rotate_vector_by_conjugate_quaternion(float *vx, float *vy, float *vz, float qw, float qx, float qy, float qz)
{
  float tx = (1-2 * (qy * qy + qz * qz)) * *vx +   (2 * (qx * qy - qw * qz)) * *vy +   (2 * (qx * qz + qw * qy)) * *vz;
  float ty =   (2 * (qx * qy + qw * qz)) * *vx + (1-2 * (qx * qx + qz * qz)) * *vy +   (2 * (qy * qz - qw * qx)) * *vz;
  float tz =   (2 * (qx * qz - qw * qy)) * *vx +   (2 * (qy * qz + qw * qx)) * *vy + (1-2 * (qx * qx + qy * qy)) * *vz;
  *vx = tx;
  *vy = ty;
  *vz = tz;
}


But as I expected it was probably axis swapping. (not that the logic makes sense for me now ) The code I pasted in this reply looks a lot like the one implemented in most Madgwick AHRS Arduino libs, which does have a different axis alignment I believe.

slinger87
 
Posts: 2
Joined: Wed Jun 28, 2017 3:29 pm

Please be positive and constructive with your questions and comments.