Constrained smoothing not working for models with Qez != 0.
Better solution needed.
Changed how the conditioning of the non-linear trajectory is implemented,
which fixes the bug in the constrained smoothing when Qez != 0.