Yaw control unit tests.
Committing code for calculating yaw control and unit tests.