Added Phi_hold_0 to compensate the offset when heading hold is activated.