Perception and Fitting for a Stair Tracker

A method for perception and fitting for a stair tracker includes receiving sensor data for a robot adjacent to a staircase. For each stair of the staircase, the method includes detecting, at a first time step, an edge of a respective stair of the staircase based on the sensor data. The method also includes determining whether the detected edge is a most likely step edge candidate by comparing the detected edge from the first time step to an alternative detected edge at a second time step, the second time step occurring after the first time step. When the detected edge is the most likely step edge candidate, the method includes defining, by the data processing hardware, a height of the respective stair based on sensor data height about the detected edge. The method also includes generating a staircase model including stairs with respective edges at the respective defined heights.