Fix PitchHeadingRoll
Add Rotation conversion functions
Apply necessary rotations to reference frame velocities