System and method for controlling a vehicle under sensor uncertainty

A method for controlling a vehicle, wherein the method uses a processor coupled with stored instructions implementing the method, wherein the instructions, when executed by the processor carry out at least some steps of the method, comprising:

• sensing, using at least one sensor, measurements indicative of a state of the vehicle;
• retrieving, from a memory operatively connected to the processor, a motion model of the vehicle, a measurement model of the vehicle, and a mean and a variance of a probabilistic distribution of a state of calibration of the sensor, wherein the motion model of the vehicle defines the motion of the vehicle from a previous state of the vehicle to a current state of the vehicle subject to disturbance caused by an uncertainty of the state of calibration of the sensor in the motion of the vehicle, such that the motion model includes a state of calibration sampled on the probabilistic distribution of the state of calibration of the sensor, and wherein the measurement model relates the measurements of the sensor to the state of the vehicle using the state of calibration of the sensor;
• sampling a feasible space of the state of calibration of the sensor defined by the probabilistic distribution to produce a set of sampled states of calibration of the sensor;
• estimating, for each sampled state of calibration using the motion model, an estimation of the current state of the vehicle to produce a set of estimated states of the vehicle;
• estimating, for each estimated state of the vehicle, an estimated state of calibration of the sensor by inserting the measurements and the estimated state of the vehicle into the measurement model;
• and
• updating the mean and the variance of the probabilistic distribution of the state of calibration of the sensor stored in the memory based on a function of the sampled states of calibration weighted with weights determined based on a difference between the sampled state of calibration and the corresponding estimated state of calibration;
• and
• controlling the vehicle using the measurements of the sensor adapted using the updated probabilistic distribution of the state of calibration of the sensor.

A non-transitory computer readable memory embodied thereon a program executable by a processor for performing a method for controlling a vehicle, the method comprising:

• receiving, from at least one sensor, measurements indicative of a state of the vehicle;
• retrieving, from a memory operatively connected to the processor, a motion model of the vehicle, a measurement model of the vehicle, and a mean and a variance of a probabilistic distribution of a state of calibration of the sensor, wherein the motion model of the vehicle defines the motion of the vehicle from a previous state of the vehicle to a current state of the vehicle subject to disturbance caused by an uncertainty of the state of calibration of the sensor in the motion of the vehicle, such that the motion model includes a state of calibration sampled on the probabilistic distribution of the state of calibration of the sensor, and wherein the measurement model relates the measurements of the sensor to the state of the vehicle using the state of calibration of the sensor;
• sampling a feasible space of the state of calibration of the sensor defined by the probabilistic distribution to produce a set of sampled states of calibration of the sensor;
• estimating, for each sampled state of calibration using the motion model, an estimation of the current state of the vehicle to produce a set of estimated states of the vehicle;
• estimating, for each estimated state of the vehicle, an estimated state of calibration of the sensor by inserting the measurements and the estimated state of the vehicle into the measurement model;
• and
• updating the mean and the variance of the probabilistic distribution of the state of calibration of the sensor stored in the memory based on a function of the sampled states of calibration weighted with weights determined based on a difference between the sampled state of calibration and the corresponding estimated state of calibration;
• and
• controlling the vehicle using the measurements of the sensor adapted using the updated probabilistic distribution of the state of calibration of the sensor.