An Improved Formulation for Constrained Mechanical Systems