Tested and fixed matrix multiply