Fixed A_Xinv_b function to work properly and added a test code for it.