Implement some basic matrics
Update test program: test_matrix