The nasty thing about angle measurements is the fact that they roll over. That is to say if you add 10 degrees to 355 degrees you get 365, but what you really want is 5. So after every addition or subtraction you need to normalize the angle. By this I mean if the new angle is above 360.0, subtract 360 and if it less than 0.0 add 360.0.
If you are adding or subtracting one angle from another angle, then this pseudo code works:
if (ang > 360) {
ang -= 360
} else if (ang < 0.0) {
ang += 360
}
Of course if you want to use radians (and you should) then replace the 360.0 with 2*PI.
Now if you add up a bunch of angles, then the above code does not work. Assume you want 350+350+350. This gives you 1050. Normalize this with the above and you only subtract 360 from 1050 to get 690. This is still not a ‘legal angle’. You may be tempted to put the above pseudo code in a loop till such time as the angle is between 0 and 360, but from experience I would suggest against this. Years ago I did just this, but an error in the code caused the angle to approach infinity. What to guess how long it takes to normalize an absurdly large angle by iteratively subtracting 360 degrees at a time?
You may also be tempted to do some fancy modulo arithmetic to normalize the angle, but I also suggest against that. In the spirit of finding bugs as fast as possible, if you add 4 angles together, you should call the normalize routine 4 times (or pass in an argument for a fixed number of iterations). One the normalization is done, add an ASSERT to the code to check for 0-360.
Now the above assumed you were interested in angles in the range of 0 to 360. This is great for bearings and azimuths. But there are many other angles that are best measured in the range of -180 to +180. For these I have a different normalization routine. I have taken to calling the 0-360 angles Full Circle Angles (FCA) and the -180-+180 angles Half Circle Angles (HCA). Therefore I have an angles library that allows me to add subtract, and perform other operations on FCA, HCA, and a mixture of these angles.
In a previous post I mentioned the issue with averaging angles. Assuming the use of FCA angles, averaging an azimuth of 350 and 10 degrees does not yield the anticipated answer of 0 degrees but rather 180 (350+10)/2 = 180.
After a fair amount of Googling, I have found the answer to averaging. I should mention that there are in fact a gazillion ways to average based on your definition of what an average is for numbers that wrap around like angles. But using the definition of an average direction of motion, then the following works.
y = sin(a1) + sin(a2) + ... + sin(an)
x = cos(a1) + cos(a2) + ... + cos(an)
avg = atan2(y,x)
So a couple of caveats. Make sure you pass the right type of angles (deg/radians) into your trig functions. Second, look at the arguments to your atan2 function. They may be ordered (y,x) or (x,y).
So how does this work? Taking the sin of an angle and multiplying it by 1 gives the y component of a unit vector along your angle. The cosine gives you the x component. If you add up all the x and y values these give you the x and y magnitude of the sum of all the vectors. The resulting vector will be in the direction of the average. Now take the arctan of the x and y values and you get the resulting average heading.
Note that in some cases (such as in a complimentary filter) you may want to give unique weights to each angle. To do this, you can simply multiply the sines and cosines by the weight like this:
y = w1*sin(a1) + w2*sin(a2) + ... + wn*sin(an)
x = w1*cos(a1) + w2*cos(a2) + ... + wn*cos(an)
avg = atan2(y,x)
And that is how I deal with pesky angles!
