add cpp code of orthonormalize_approx.m but the functon orth() need to be completed