So it would make sense to plot the estimated heading together with cmd_heading (e.g. psi from ROTORCRAFT_FP).
My guess is that your (estimated) heading drifted between switches....
What I don't understand is that guidance_h_heading_sp (sent as cmd_heading in HOVER_LOOP) changes even when you switch from KILL to HOVER.
But the only two instances where this variable is updated (apart from initialization to zero) is in guidance_h_run if you are in HOVER or NAV mode.
Did you make any changes that could impact this?
I can take a look at the logs if you upload them somewhere...
Cheers, Felix