The pseudo inverse code was working all along. My test was wrong because I forgot that eigenvectors are not guaranteed to be orthogonal when your matrix is not Hermetian. I fixed the test to project out the Z vectors and so now everything works.