Added pivoting to matrix inversion, making it work for all invertible matrices.

Modified the test procedure test_matrix to work with the new argument list.