Last changes made at MacDonald Campus on last day of NCFRN14. Integrated hysteresis for Highest likelihood point. Also included torso size for estimating the distance from robot.