all algorithms tested and work so far, TODO ROS interface