Fix coordinate transforms!

[lundeen06]

Check these transforms against MATLAB. Make sure these are passive rotations

to run tests

1. in macros.h, comment out #define FLIGHT line and put in #define TEST
2. tasks/test_task.cpp and put the testing function
3. run on pico