solve constrained optimization problem in matlab