fixed some bugs in stochastic mpc