Fixed bug in yaw,pitch,roll to quaternion code.