Initial matrix math implementation