Matrix functions tested and work
Merged in Gloudas' code