safe_solve function

Safe matrix inversion with conditioning check