Clean up code a bit
Added Yaw, Pitch and Roll angles