Add pseudoinverse function and its test