compute_inputs
fmincon
- Call fmincon with cost function and constraints in `compute_inputs` - Add function `get_ref_decision_variable` to construct the full decision variable for reference trajectory from reference states and inputs