BNO055 (w/ Adafruit Lib) Default Orientation for Transformation into abs Reference Frame (ENU/NED)

[sMartins]

Hello Bosch-Community,

We use Adafruit_BNO055.h (and Adafruit_Sensor.h) to gather IMU-Data. We set no registers and just use:

bno = Adafruit_BNO055(55, 0x28, imuConfig.theWire); // and after successful initialisation
//get all data from BNO 055
sensors_event_t angVelocityData, linearAccelData;
bno.getEvent(&angVelocityData, Adafruit_BNO055::VECTOR_GYROSCOPE);
bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);
imu::Quaternion quat = bno.getQuat();

to gather data as json. It works! Now I want to generate a rotation matrix from your quaternion data to transform your linear_x, _y, _z acceleration from sensor coordinate system into ENU coordinate system. I'm not really interested into the orientation of the sensor. I just want the linear accelerations in ENU (or NED) reference frame. For this I analysed the attached data, where I wiggle the IMU across a table in sensor X (almost North), wait, turn 90° cw, wiggle again in X, wait, 90° cw, wiggle in X.

from scipy.spatial.transform import Rotation as R
import numpy as np

for measurement in data:
    raw_quat = np.array([measurement['quat_w'], measurement['quat_x'], measurement['quat_y'], measurement['quat_z']])
    correct_quat = np.array([measurement['quat_w'], measurement['quat_z'], measurement['quat_y'], measurement['quat_x']])
    raw_rotation = R.from_quat(raw_quat)
    corrected_rotation = R.from_quat(correct_quat)
    # Quaternion to rotation matrix
    corrected_rotation_matrix = corrected_rotation.as_matrix()
    # IMU measurements
    accel = np.array([measurement['linear_x'], measurement['linear_y'], measurement['linear_z']])
    # Transform IMU measurements to NED (ENU??) frame
    accel_ned = np.dot(corrected_rotation_matrix, accel)

This only works as expected in the "correct_quat" case, where I swapped z and x quaternion (PS: not true if I use the quaternion_rotation_matrix instead of R.from_quat). Please answer me, if this is correct or if I am doing something horribly wrong. If I use raw_quat (PS: with R.from_quat), it does not calculate correctly and linear_x stays untouched.

Kind regards,
Martin

PS/Edit: I think the scipy Rotation implementation is a bit opaque. Therefore let's use this implementation of quaternion to rotation matrix implementation as basis for our discussion (from https://automaticaddison.com/how-to-convert-a-quaternion-to-a-rotation-matrix/). My main question is, if this is the correct approach for linear acceleration transformation to ned/enu, or if I am missing something besides calibration, filtering and smoothing?

import numpy as np
 
def quaternion_rotation_matrix(Q):
    """
    Covert a quaternion into a full three-dimensional rotation matrix.
 
    Input
    :param Q: A 4 element array representing the quaternion (q0,q1,q2,q3) 
 
    Output
    :return: A 3x3 element matrix representing the full 3D rotation matrix. 
             This rotation matrix converts a point in the local reference 
             frame to a point in the global reference frame.
    """
    # Extract the values from Q
    q0 = Q[0]
    q1 = Q[1]
    q2 = Q[2]
    q3 = Q[3]
     
    # First row of the rotation matrix
    r00 = 2 * (q0 * q0 + q1 * q1) - 1
    r01 = 2 * (q1 * q2 - q0 * q3)
    r02 = 2 * (q1 * q3 + q0 * q2)
     
    # Second row of the rotation matrix
    r10 = 2 * (q1 * q2 + q0 * q3)
    r11 = 2 * (q0 * q0 + q2 * q2) - 1
    r12 = 2 * (q2 * q3 - q0 * q1)
     
    # Third row of the rotation matrix
    r20 = 2 * (q1 * q3 - q0 * q2)
    r21 = 2 * (q2 * q3 + q0 * q1)
    r22 = 2 * (q0 * q0 + q3 * q3) - 1
     
    # 3x3 rotation matrix
    rot_matrix = np.array([[r00, r01, r02],
                           [r10, r11, r12],
                           [r20, r21, r22]])
                            
    return rot_matrix

For your convenience, graphs of the raw and processed data. Euler(Euler) and Euler(Quats) was generated by R.from_euler/from_quat and R.as_euler('xyz', degrees=True). Attached you also find an Animation of the Rotation and "correct" Rotation. As of PS I added the a plot of the original quat(w, x, y, z) using the quaternion_rotation_matrix function above the old image with quat(w, z, y, x).

Old plot x and z axis exchanged

[FAE_CA1]

Hi,

Thanks for your inquiry.

BNO055 chip itself is ENU which means BNO055 y axis is pointing to North, x to East and z to Up. Please refer to BNO055 quick start guide at https://www.bosch-sensortec.com/media/boschsensortec/downloads/application_notes_1/bst-bno055-an007.... for more info.

If for some reason your device body X/Y/Z axes are not aligned with BNO055 x/y/z axes, then you can use BNO055 axis remapping. Please see the attached "BNO055 axis remapping v2.pdf".

This means that you don't need to develop your own rotation matrix to transfer BNO055 linear acceleration results to your ENU coordinate system because BNO055 linear acceleration is already based on ENU.

Thanks.

[sMartins]

Thank you for your reply FAE_CA1!

I've read the datasheet and other community posts regarding this topic before posting here. Still thank you.

As I said. I'm not interested in the rotation of the device and thus less interested into axis remapping because the orientation of my device body does not matter at all. ALL I want is the linear accelerations in the device coordinate frame transformed to the ENU coordinate system (no matter the orientation).

Sincerely,
Martin

[FAE_CA1]

Hi,

I see. Thanks for the explanation.

BNO055 has 9DoF sensor fusion outputs including quaternions, Euler angles, gravity vectors and linear accelerations. It is up to you how to transfer BNO055 linear acceleration outputs to ENU system.

Thanks.

[sMartins · Accepted solution]

Thank you FAE_CA1,

As with github issues the beauty lies in formulating out the problem and answering it yourself. If you would have read the original post and tried to understand it, you would have known, that I solved it myself, before you could even copy paste your first"helpful" "answer". Good day. Thank you for emphasizing, what bosch stands for. I will make sure to remember this, next time I need some hardware.

Fellow Implementers, please use quaterion to rotation matrix solution to convert into ENU reference frame as described in the edit to the original question.

So long and thanks for all the fish.