Update Matrix.h

some optimizations + const validness