2PI discontinuity fix & heading trajectory