Data Association Aware Belief Space Planning And Perception

Methods and systems are provided for controlling a robot to navigate a perceptually aliased environment, the robot being configured to infer a first Gaussian mixture model (GMM) of a first pose of the robot, and to plan an optimal control action by: determining a second GMM of a simulated pose of the robot by simulating applying the first control action to the first GMM; determining from the second GMM a range of possible observations; calculating scene probabilities for associating each possible observation with each aliased scene of a set of aliased scenes; calculating from the scene probabilities an objective function to determine an estimated cost of the first control action; and comparing the estimated cost of the first control action with estimated costs calculated for one or more additional control actions, to determine an optimal control action.