Working point control optimization, cost on goal state
Changed mask filenames
Added timing file
Expts for Gurobi penalty SQP with same parameters as belief penalty MPC
added start and goal plotting
Merge branch 'gkahn'
different timesteps for belief penalty