Source: https://www.robotics.stanford.edu/~ang/papers/icraoss09-ROS.pdf