Fix pitch and roll angle and add heading