Tracking the relative orientation of two IMU and position within limited area

Mark Hughes

Joined Jun 14, 2016
Hi Andrew,
Moving things out into dedicated functions that you can individually check and test is a wonderful idea. So -- I pulled up the BNO055 datasheet and found this:
However, page 24 and 25 of the datasheet indicates that the axes can be remapped by changing register values -- so if you'd rather have the z-axis be the y-axis, the y-axis be the x-axis, and the x-axis be the z-axis, you can do that. It won't solve your problem, it will just change it.

What you need to do in your setup code is to define another rotation matrix (or two). Although this time you can use traditional trig functions because you'll be given numbers as angles and only need to create this transformation matrix one time.

So -- first step is using the gravity vector to determine the initial orientation of the sensor and creating a rotation matrix to your axis of choice. If you have this device mounted horizontally, that means z-axis. If you have it mounted vertically, that means choosing the y-axis or x-axis. Your goal is to create a rotation matrix that rotates your gravity vector to lie along your axis of choice. That way, in later code, you can simply subtract the influence of gravity from all of your readings by manipulating a single value in your array.

Okay - so you've created the necessary rotation matrix to move the gravity vector.
That gets you close -- very close -- but there's still one last thing to do -- you have to orient the axes to lie in the directions you wish -- in this case, I believe we discussed making use of a compass reading to finish the alignment. Figure out where magnetic north is and then align that with your x or y axis with a second rotation matrix.
At this point, your data should align with your inertial reference frame. The result of all of this is a new rotation matrix with 9 values.

So you will calculate this in your setup code and it will look something like this.
You'll then use this matrix in your loop code to change the starting point of all of your data. (Not this specific matrix mind you -- it will be different each time you start the device, and besides, I just made this one up.)

Essentially, you're taking the sensor, wherever it is, and you are pre-rotating it to be perfectly aligned with the environment -- straight up and down -- pointing to magnetic north.

Then in your loop code, you deal with the quaternions, subtract out the gravity vector, and start integrating stuff.

You know -- I might have mentioned when you first started this journey that this stuff is pretty difficult. For me, the difficulty is always in the visualizations -- which is why I made the graphics.

Does this all make sense?

Thread Starter


Joined Mar 26, 2019
Code to get the compass heading

imu::Vector<3> vector = bno1.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
mCompassHeading = atan2(vector.y(), vector.x());
// Correct for when signs are reversed.
if(mCompassHeading < 0)
mCompassHeading += 2*PI;
// Convert radians to degrees
mCompassHeading = mCompassHeading * 180/M_PI;


Joined Mar 22, 2019
The orientation of the BNO055 board as shown is it with the writing on top or with the soldered components on top?
Hi, good question, on my application, I just kept turning it, till the readings appeared to be correct. I think to remeber the parts were on top. The MC5338L I remember the parts on bottom.
good luck!

Mark Hughes

Joined Jun 14, 2016
Hi, good question, on my application, I just kept turning it, till the readings appeared to be correct. I think to remeber the parts were on top. The MC5338L I remember the parts on bottom.
good luck!
Hi @dachbauer, thanks for jumping in!
Just an FYI -- I spent the day yesterday writing up an article on how to calculate the rotation matrices needed to sort out the sensor from an arbitrary position. It'll be a few days before it's ready for review, and a few more before it's ready for the site -- but it'll be a step in the right direction.

Mark Hughes

Joined Jun 14, 2016
@dachbauer @AJDickinson ,
The article is through tech-review. It basically shows the rotation matrix required to put the z-axis down during initial setup. It also shows how to deal with the compass heading issue. The ultimate goal is to create a single, numerical rotation matrix at startup that can then be applied to all data coming out of the sensor -- the idea in the article is to make gravity align with the negative z-axis, and north align with positive x-axis data.
Should be on the site in a few days time.
I didn't get to do a proper code implementation since I ran up against my word limit. But I wouldn't be opposed to it in a future article if needed.