Moved matrix utils to a separate class
-added comments
-updated test
-closes #1