Bosch Sensortec Community

    cancel
    Showing results for 
    Search instead for 
    Did you mean: 
    SOLVED

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

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

    sMartins
    Member

    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).

    newplot(2).png

    Old plot x and z axis exchanged
    newplot(1).png

     

    4 REPLIES 4

    FAE_CA1
    Community Moderator
    Community Moderator

    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.

    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
    Community Moderator
    Community Moderator

    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.

    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.

    Icon--AD-black-48x48Icon--address-consumer-data-black-48x48Icon--appointment-black-48x48Icon--back-left-black-48x48Icon--calendar-black-48x48Icon--center-alignedIcon--Checkbox-checkIcon--clock-black-48x48Icon--close-black-48x48Icon--compare-black-48x48Icon--confirmation-black-48x48Icon--dealer-details-black-48x48Icon--delete-black-48x48Icon--delivery-black-48x48Icon--down-black-48x48Icon--download-black-48x48Ic-OverlayAlertIcon--externallink-black-48x48Icon-Filledforward-right_adjustedIcon--grid-view-black-48x48IC_gd_Check-Circle170821_Icons_Community170823_Bosch_Icons170823_Bosch_Icons170821_Icons_CommunityIC-logout170821_Icons_Community170825_Bosch_Icons170821_Icons_CommunityIC-shopping-cart2170821_Icons_CommunityIC-upIC_UserIcon--imageIcon--info-i-black-48x48Icon--left-alignedIcon--Less-minimize-black-48x48Icon-FilledIcon--List-Check-grennIcon--List-Check-blackIcon--List-Cross-blackIcon--list-view-mobile-black-48x48Icon--list-view-black-48x48Icon--More-Maximize-black-48x48Icon--my-product-black-48x48Icon--newsletter-black-48x48Icon--payment-black-48x48Icon--print-black-48x48Icon--promotion-black-48x48Icon--registration-black-48x48Icon--Reset-black-48x48Icon--right-alignedshare-circle1Icon--share-black-48x48Icon--shopping-bag-black-48x48Icon-shopping-cartIcon--start-play-black-48x48Icon--store-locator-black-48x48Ic-OverlayAlertIcon--summary-black-48x48tumblrIcon-FilledvineIc-OverlayAlertwhishlist