It works!!!!

In order to do the tilt compensation for the compass, I need to do a series of coordinate transformations. Unfortunately, I find coordinate transformations to be one of the most confusing things I’ve learned about in my engineering education. I’ve run across a couple instances where I’ve had to use them at work and now one instance where I have to use them for the robot. Every time I do, I get confused.

The basic idea of a coordinate transformation is reasonably easy to understand. Lets say you have a point sitting somewhere in space relative to an X and Y axes. Then imagine you have another pair of axes that are in the same plane and have the same origin, but are rotated at some angle relative to the original axes. You could figure out the coordinates of that point in the new axes by using trigonometry and a few Sin and Cos terms. The problem comes in when you want to deal with stuff in 3-axes and you want to do multiple rotations. The number of terms quickly get out of control and its hard to maintain an understanding of what each term intuitively means. That’s when you start using matrix math. That takes care of keeping all the terms in the equations straight, but you still have to make sure you know what you are doing. The order of which axes you rotate first matters, as does whether your second rotation is about the already rotated coordinate frame, or whether you do it about the original, fixed coordinate system. Lastly, you also need to decide whether you are rotating the data to a coordinate system or you are rotating a coordinate system to the data. Ugh.

So when it comes to the robot and the compass, I need to do a pair of rotations. There will be a rotation for the pitch of the boat (is the bow tilted up or down?) as well as for the roll (is it tilted over on its side?). The compass sensor will take readings while the boat is pitched and rolled, and I want to know what the compass readings would be if it were not pitched and rolled(sitting flat in the water but still pointing in the same direction). Its probably useful to note that I am assuming the sensor (both accelerometer and compass) X-axis will be aligned pointing towards the bow (front) of the boat, with the Y-axis pointing to the port (left) side, and the Z-axis will be pointing upwards along the mast.

The pitch rotation will be first. If the boat is pitched downwards, that is a positive rotation about the fixed y-axis (right-hand-rule). But we want to rotate that coordinate system the opposite direction to align it with the fixed coordinate system, so its really a negative rotation about the Y-axis. After the pitch compensation, the same is done for the roll compensation. The boat tipping to the starboard (right side) would be a positive rotation about the X-axis, so we have to do a negative rotation to compensate for it.

Each of these rotations creates a rotation matrix and since there are two of these rotations, there are two matrices that need to multiplied. Doing the requisite matrix multiplication produces the following two equations. Note that there would be a Z equation as well, but I have dropped that result after the compensation since you only need the X and Y components of the horizontal magnetic field to get the heading. Also note that in the equations below p = pitch and r = roll. The -1 in each of the trig functions signifies that the angle is the opposite of what the boat is actually tilted.

X = x*cos(-1*p) + y*sin(-1*p)*sin(-1*r) – z*cos(-1*r)* sin(-1*p)
Y = y*cos(-1*r) + z*sin(-1*r)

A summary of the assumptions I’ve used are below:
- The accelerometer Z-axis reads +1.0 when the red light is facing the ceiling
- The accelerometer X-axis reads a negative value when it is tilted in a positive rotation about the y-axis (the +x-axis tilts down towards the floor)
- The accelerometer Y-axis reads a positive value when the sensor is tilted in a positive rotation about the x-axis (the +y-axis points up in the air)
- Positive rotation of the sensor about the x-axis gives a positive roll angle (roll to starboard)
- Positive rotation of the sensor about the y-axis gives a positive pitch angle (pitch downwards")
- Pitch and roll angles should be multiplied by -1.0 to make the angles that for the coordinate transforms (coordinate axes rotated the opposite way of the pitch and roll angles)
- Rotation matrix is created using the following transformations:
- Rotate the pitch angle in the fixed coordinate frame first
- Rotate the roll angle in the fixed coordinate frame second

After all that, it does actually work! It took me a lot of thinking and testing to get it to work, but it does work and it seems to work pretty well. It is a bit sensative to accelerations and nearby electronics (computer, phone, etc), but there isn't much I can do about that. Hopefully the accelerations will even out with time-averaging and hopefully we can keep the sensor far enough away from the other electronics.

I should say that I calculated the angles by an over-simplified way to get the tilt compensation working, but for large tilt angles its not accurate. I have the more accurate method figured out (I think), but I haven't tried it to confirm. I'll leave that for another time.