Add tests; fix sub matrix operations.