System and Method for Controlling a Vehicle in an Environment

The present disclosure discloses a system and a method for controlling a vehicle in an environment. The method uses a processor coupled to a memory storing a probabilistic map of the environment relating measurements of a magnetic field and time of the measurements of the magnetic field to a probability of locations within the environment. The processor is coupled with stored instructions when executed by the processor carry out steps of the method comprising estimating a probability of the current location in the environment based on a current measurement of a magnetometer at a current timestamp by submitting the current measurement and the current timestamp to the probabilistic map, and controlling an actuator of the vehicle based on a stochastic control employing the probability of the current location.