Added robustness to QP solve