remove unused function (inefficient implementation)
minor speedup in construction of RobotState