Put CostReferenceTrajectory in its own header