ILQR Implementation