Fix matrix inversion typo

This fixes a typo that broke matrix inversion and adds tests
to verify it.