Add checkIdentityMatrix function and test