Added rotation matrix for ned to body frame, and transpose and additional test code.