Clean up matrix multiplication

May need additional tests