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.

Trying To Tilt Compensate

I haven't been spending my 5 hours a week on this project, but I did spend a couple hours this week trying to put the compass and accelerometer readings together to get the tilt compensation working. I figured that I had the math down and the difficult part would be getting the microcontroller to talk to both the compass and the accelerometer at the same time. It turns out I was wrong on both accounts. It was surprisingly easy to merge my accelerometer and compass codes together, modify some variable names, and get both sensor readings to output to the computer screen. Unfortunately, the tilt compensation math didn't seem to work out, because the compensated outputs didn't make much sense.

Several weeks ago I had spent quite a bit of time trying to figure out the math for doing the tilt compensation. I believe its just a couple coordinate transformations, but it gets tricky when you start figuring out which axis to rotate about first and whether you use fixed axes or the transformed axes for the second rotation. I thought I figured all that out, reduced it to a couple simple equations, and verified it with a spreadsheet. Something isn't right, though.

It may be because I had mistakenly thought that when an axis is tilted downwards in the direction that gravity is pulling, it would read a positive value. It actually reads a negative value. I guess this makes sense if you think about accelerating the sensor along the postive x-direction. It would feel a force pulling it towards negative x, but it reads a positive value since it is accelerating in the positive x-direction. Just to try, I multiplied the inputs by -1 to see if that fixed it, but that didn't work. I don't think its a simple change like that. I might have to multiply the matrices in a different order or something. Unfortunately, I didn't really write out how I figured out the math last time, so I'll have to go through it again. When I have it figured out, I'll be sure to write a blog post on it.

Averaging

I haven't posted in a little while, but I have done some robot work. The main thing I did was to create some code to do averaging of sensor readings. That forced me to learn how to use arrays (groupings of variables), and it also made me realize that I need to think a bit more about how local or global my variables are.

On the Arduino (the brand of micro-controller I am using) website, there is actually an example program that does what they call "smoothing". It basically calculates the running average every time a new reading is put in the array. It does so by keeping a running sum of all the sensor values in the array and divides by the number of values in the array. I don't think that is ideal for our application. I'd much prefer to just periodically poll the sensors and store the data in arrays and then calculate the average only when I'm interested in the value. It seems like that would save calculation time.

I set it up to average the compass heading over a certain number of readings. I arranged it so that the number of readings could be changed just by changing one number at the top of the program. I played around with it a little bit, and I decided that rather than the readings being random, they seem to kind of drift around. The averaging happens so fast that unless I set it to average several thousand readings over a few seconds, it still moves around a bit (+/- 3 degrees or so). I think its just taking readings way faster than the readings are drifting, so its not averaging terribly well. As the programming gets more complex, there will be quite a bit more time between readings, so I think that will improve the averaging somewhat. There will have to be some testing/playing around to determine the optimal delay between readings and the total number of readings to average.