SQP Framework Formulation beginning
SQP Framework Formulation beginning
Active-set Method is online. Not tested with Equality constraints yet
 Checkpoint: NMPC model completed. TODO: (1)Formulate constraints.(2) Test SQP algorithm