Added matrix trace function and test