Fixing a bug with the yaw for trajectories