Added condition number calculation for square matrices based on SVD.
Added condition number checking of condensed Hessian, see #49.
Fixed ocp/lsq_term example