System and method for controlling a vehicle under sensor uncertainty

Abstract

A system for controlling a vehicle a sensor to sense measurements indicative of a state of the vehicle and a memory to store 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. The motion model of the vehicle defines the motion of the vehicle from a previous state to a current state subject to disturbance caused by an uncertainty of the state of calibration of the sensor in the motion of the vehicle. The measurement model relates the measurements of the sensor to the state of the vehicle using the state of calibration of the sensor. The system includes a processor to update the probabilistic distribution of the state of calibration based on a function of the sampled states of calibration weighted with weights determined based on a difference between the state of calibration sampled on a feasible space defined by the probabilistic distribution and the corresponding state of calibration estimated based on the measurements using the motion and the measurements models. The system includes a controller to control the vehicle using the measurements of the sensor adapted using the updated probabilistic distribution of the state of calibration of the sensor.

We claim: 
     
       1. A system for controlling a vehicle, comprising:
 at least one sensor to sense measurements indicative of a state of the vehicle; 
 a memory to store 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; 
 a processor configured to
 sample 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; 
 estimate, 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; 
 estimate, 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 
 update 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 
 
 a controller to control the vehicle using the measurements of the sensor adapted using the updated probabilistic distribution of the state of calibration of the sensor. 
 
     
     
       2. The system of  claim 1 , wherein the set of sampled states of calibration of the sensor represents the state of calibration of the sensor as a set of particles, each particle includes a mean and a variance of the state of calibration of the sensor defining the feasible space of the parameters of the state of calibration of the sensor, and wherein the processor
 updates iteratively, until a termination condition is met, the mean and the variance of at least one particle using a difference between the estimated state of calibration of the sensor estimated for the particle and the measured state of calibration of the sensor determined for the particle; 
 updates the mean and the variance of the probabilistic distribution of the state of calibration of the sensor as a function of the updated mean and the updated variance of the particle. 
 
     
     
       3. The system of  claim 2 , wherein, for the iteration updating the particle, the processor is configured to
 determine the mean of the estimated state of calibration of the sensor that results in the state of the vehicle estimated for the particle according to the measurement model; 
 determine the variance of the estimated state of calibration of the sensor as a combination of an uncertainty of the measurements and the variance of the particle; 
 update the mean of the sampled state of calibration of the sensor of the particle using the mean of the estimated state of calibration of the sensor; and 
 update the variance of the sampled state of calibration of the sensor of the particle using the variance of the estimated state of calibration of the sensor. 
 
     
     
       4. The system of  claim 3 , wherein the processor determines the variance of the estimated state of calibration of the sensor as the combination of the uncertainty of the measurements and a set of variances of the set of particles. 
     
     
       5. The system of  claim 4 , wherein the number of particles in the set of particle are varying over time. 
     
     
       6. The system of  claim 1 , wherein the function uses a weighted combination of the sampled states of calibration of the sensor. 
     
     
       7. The system of  claim 1 , wherein the sensor is calibrated using the updated probabilistic distribution of the state of calibration of the sensor. 
     
     
       8. The system of  claim 1 , wherein the at least one sensor includes a first sensor to measure an angle indicative of the steering angle of the steering wheel of the vehicle and a second sensor to measure at least one of a lateral acceleration and a heading rate, wherein the motion model includes the state of calibration of the first sensor, but does not include the state of calibration of the second sensor, and wherein the measurement model includes both the state of calibration of the first sensor and the state of calibration of the second sensor. 
     
     
       9. The system of  claim 8 ,
 wherein the processor updates the mean and the variance of the probabilistic distribution of the state of calibration of the first sensor based on the function of a difference of weighted sampled states of calibration of the first sensor and weighted estimated states of calibration of the first sensor, and 
 wherein the processor updates the mean and the variance of a probabilistic distribution of the state of calibration of the second sensor based on the function of a difference of weighted estimated states of calibration of the second sensor and the sensor measurement. 
 
     
     
       10. The system of  claim 1 , wherein the state of the vehicle includes a velocity and a heading rate of the vehicle,
 wherein the motion model of the vehicle includes a combination of a deterministic component of the motion and a probabilistic component of the motion, wherein the deterministic component of the motion is independent from the state of calibration of the sensor and defines the motion of the vehicle as a function of time, wherein the probabilistic component of the motion includes the state of calibration of the sensor having an uncertainty and defines disturbance on the motion of the vehicle, 
 
       wherein the measurement model of the vehicle includes a combination of a deterministic component of the measurement model independent from the state of calibration of the sensor and a probabilistic component of the measurement model that includes the state of calibration of the sensor. 
     
     
       11. 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. 
 
     
     
       12. The method of  claim 11 , wherein the set of sampled states of calibration of the sensor represents the state of calibration of the sensor as a set of particles, each particle includes a mean and a variance of the state of calibration of the sensor defining the feasible space of the parameters of the state of calibration of the sensor, comprising:
 updating iteratively, until a termination condition is met, the mean and the variance of at least one particle using a difference between the estimated state of calibration of the sensor estimated for the particle and the measured state of calibration of the sensor determined for the particle; 
 
       updating the mean and the variance of the probabilistic distribution of the state of calibration of the sensor as a function of the updated mean and the updated variance of the particle. 
     
     
       13. The method of  claim 12 , further comprising, for the iteration updating the particle,
 determining the mean of the estimated state of calibration of the sensor that results in the state of the vehicle estimated for the particle according to the measurement model; 
 determining the variance of the estimated state of calibration of the sensor as a combination of an uncertainty of the measurements and the variance of the particle; 
 updating the mean of the sampled state of calibration of the sensor of the particle using the mean of the estimated state of calibration of the sensor; and 
 updating the variance of the sampled state of calibration of the sensor of the particle using the variance of the estimated state of calibration of the sensor. 
 
     
     
       14. The method of  claim 13 , wherein the variance of the estimated state of calibration of the sensor is determined as the combination of the uncertainty of the measurements and a set of variances of the set of particles. 
     
     
       15. The method of  claim 14 , wherein the number of particles are varying over time. 
     
     
       16. The method of  claim 11 , wherein the function uses a weighted combination of the sampled states of calibration of the sensor. 
     
     
       17. The method of  claim 11 , further comprising:
 calibrating the sensor using the updated probabilistic distribution of the state of calibration of the sensor. 
 
     
     
       18. The method of  claim 11 , wherein the at least one sensor includes a first sensor to measure an angle indicative of the steering angle of the steering wheel of the vehicle and a second sensor to measure at least one of a lateral acceleration and a heading rate, wherein the motion model includes the state of calibration of the first sensor, but does not include the state of calibration of the second sensor, and wherein the measurement model includes both the state of calibration of the first sensor and the state of calibration of the second sensor, comprising:
 updating the mean and the variance of the probabilistic distribution of the state of calibration of the first sensor based on the function of a difference of weighted sampled states of calibration of the first sensor and weighted estimated states of calibration of the first sensor, and 
 updating the mean and the variance of a probabilistic distribution of the state of calibration of the second sensor based on the function of a difference of weighted estimated states of calibration of the second sensor and the sensor measurement. 
 
     
     
       19. The method of  claim 11 , wherein the state of the vehicle includes a velocity and a heading rate of the vehicle,
 wherein the motion model of the vehicle includes a combination of a deterministic component of the motion and a probabilistic component of the motion, wherein the deterministic component of the motion is independent from the state of calibration of the sensor and defines the motion of the vehicle as a function of time, wherein the probabilistic component of the motion includes the state of calibration of the sensor having an uncertainty and defines disturbance on the motion of the vehicle, 
 wherein the measurement model of the vehicle includes a combination of a deterministic component of the measurement model independent from the state of calibration of the sensor and a probabilistic component of the measurement model that includes the state of calibration of the sensor. 
 
     
     
       20. 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.