Update QP Algorithm

Redesign QP Algorithm to enhance robustness.