- Add quadratic cost function using state costs (Q)
and input costs (R)
- Use `npredstates` and `npredinputs` instead of `npred`
when looping through states and inputs
- Update `get_state_start_idx` and `get_input_start_idx`
descriptions to include assumption of `i` starting at 0