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.
Data association aware belief space planning and perception