Add and test matrix functions