// Update optimization linear cost vectors when reference is changing

// Used during runtime to update objective on reference r only steady-state trajectory mapper is not updated since it takes too long for inversion to do in real-time

// us is hovering steady state input and is constant so does not get updated

// Gurobi refers to this as 'c'. It is multiplied by 2 since Gurobi minimizes (x^T * Q * x + c^t * x)

// OSQP refers to this as 'q'. It is not multiplied by 2 since OSQP minimizes (1/2 * x^T * P * x + q^t * x)