Checkpoint: Untested SQP Completed. TODO: (1)Formulate constraints.(2) Test SQP algorithm
 Checkpoint: Untested SQP Completed. TODO: (1)Formulate constraints.(2) Test SQP algorithm