1. A method of substantially simultaneously planning a path and mapping an environment by a robot, comprising: determining a mean of an occupancy level for a location in a map; determining a probability distribution function (PDF) of the occupancy level; calculating a cost function based on the PDF; and simultaneously planning the path and mapping the environment based on the cost function.