Add angleAverage

Adds a utility function in matlab/cpp for computing the average of two angles and updates a couple instances where this was being done incorrectly. Added a unit test to test the matlab function (the cpp function is tested against matlab indirectly by runAtlasWalkingTestMex).