Updated. working on converting matrix.h to ROS