sensor fusion

In a comment from Ciskje on my sensor fusion implementation, I've been asked to explain how the Processing graphical uses rotations to visualize the orientation informations coming from the sensor fusion algorithm.

The question was basically, what's the reason about using the Yaw, Pitch and Roll in a "strange" order in the following lines of code in my Processing graphical cube example:

rotateZ(-Euler[2]); // phi - roll
rotateX(-Euler[1]); // theta - pitch
rotateY(-Euler[0]); // psi - yaw

The following explanation assumes some basic knowledge of rotation matrices and 3d coordinate systems. For a good introduction on the topics, I strongly suggest reading Chapter 2 of "Robot Modeling and Control" by Mark W. Spong.

Demonstration

The first thing you have to understand is that we have 3 coordinate systems here.

• O_sX_sY_sZ_s attached to the sensor array
• O_wX_wY_wZ_w attached to the real world, with X_w pointing to the Hearth North and Z_w pointing to the sky
• O_mX_mY_mZ_m the coordinate system of the graphics program, usually it is attached on the top left corner of the monitor with X_m pointing right, Y_m pointing down and Z_m pointing forward to the monitor.

Note that when X_w and Z_m are aligned (you point the monitor to the Earth north in your location) there is a simple relationship between O_wX_wY_wZ_w and O_mX_mY_mZ_m: a point p_w defined in the coordinate system W having coordinates p_w = [p_{w_x}, p_{w_y}, p_{w_z}] can be expressed in the coordinate system M doing p_m = [-p_{w_y}, -p_{w_z}, p_{w_x}]

The sensor fusion algorithm running on Arduino computes a quaternion representing the orientation of O_sX_sY_sZ_s with respect to O_wX_wY_wZ_w, and from the quaternionToEuler function in the Processing code we can get the Euler angles expressed in the aerospace sequence, so they are the yaw (ψ - psi), pitch (θ - theta) and roll (φ - phi) angles.

We can use them in the following way: supposing that O_sX_sY_sZ_s and O_wX_wY_wZ_w are aligned in the beginning we can rotate O_sX_sY_sZ_s so that it would assume the orientation described in the quaternion or yaw, pitch, roll angles by:

1. rotate by ψ around Z_s
2. rotate by θ around Y_s
3. rotate by φ around X_s

After doing the above we will obtain that O_sX_sY_sZ_s is oriented with respect to O_wX_wY_wZ_w according to the orientation described by the output quaternion from the sensor fusion algorithm.

The same result could have been produced by rotations relative to the W coordinate system, but using the reverse order:

1. rotate by φ around X_w
2. rotate by θ around Y_w
3. rotate by ψ around Z_w

However, we are interested in rotating the graphical cube which is defined in the O_mX_mY_mZ_m coordinate system so a slightly different approach is needed. From the relationship between O_wX_wY_wZ_w and O_mX_mY_mZ_m described above we can see that there is a relationship between the rotations made in the W and M coordinate system:

• a rotation φ around X_w corresponds to a rotation of -φ around Z_m
• a rotation θ around Y_w corresponds to a rotation of -θ around X_m
• a rotation ψ around Z_w corresponds to a rotation of -ψ around Y_m

Given the above and remembering that in Processing the rotations are made in respect to the monitor frame we can conclude that, in order to align a graphical cube defined in the O_mX_mY_mZ_m to the orientation described by the quaternion and then Euler angles coming from the sensor fusion algorithm of the sensors coordinate system O_sX_sY_sZ_s, we have to do the following rotations:

1. a rotation of -φ around Z_m
2. a rotation of -θ around X_m
3. a rotation of -ψ around Y_m

as we wanted to demonstrate.

When you have created two breakout boards for the ADXL345 accelerometer and the ITG3200 gyroscope and you have those two nice sensors what you can do? Simple: create an implementation of an Attitude sensor fusion which runs with those chips!

So, I started documenting myself and reading lot of stuff on IMUs (inertial measurement units - composed by an accelerometer and a gyroscope) and MARG sensor (an IMU which also has a magnetometer to sense heading).

One really good article you should read to get started with IMUs is A Guide To using IMU (Accelerometer and Gyroscope Devices) in Embedded Applications available on http://www.starlino.com/ . Also from the same website there is a nice implementation of the theory from the IMU guide: you can find it on Arduino code for simplified Kalman filter. Using a 5DOF IMU.

So, I took the algorithm above and converted it to be used with the ADXL345 and the ITG3200. I came out with two implementations both with parts in Arduino and Processing code (see the attachments to this page below).

IMPORTANT: If you use the IMU Digital Combo Board from Sparkfun you will need to change the address of the Gyroscope from 0x69 in my code to 0x68.

The first implementation simply reads the raw accelerometer and gyroscope values on the Arduino while the Processing code (running on the PC) will compute the sensor fusion algorithm, produce the orientation vector and siplay a nicely oriented cube.

In other implementation all the sensor fusion logic is implemented in Arduino code so it will run embedded in the microcontroller. Looks like Arduino can coupe with that without any problems! So, once the orientation is computed is all sent to the PC where the Processing application will display incoming data and the oriented cube.

You can see a demonstration in the following video:

All the code is available in the attachments below. You can see the circuit in the picture below. Note that these are custom made breakout boards, your pin configurations will vary depending on the schematics of your breakout board. If you have questions just leave a comment below.

The processing programs need to be executed on the Processing IDE, available for download from http://processing.org/
Remember to adjust the serial port configuration in the Processing code to match your connection to the Arduino in your system. Here I use /dev/ttyUSB9, if you are under windows you'll probably have to use something like COM3 or something like that. See in the Arduino IDE under Tools->Serial Port to get the exact value you have to use.