Add 2d tests and make the package pass them
Add identity matrix tests for 2d and 3d