Fixed testing

Fixed testing with identity matrix