Extended matrix classes

+ Identity matrix class methods
+ bugfixes
+ tests