Update QP controller

no, there's only a mex version of this function currently