Localizing and updating a map using interpolated lane edge data

1. A system comprising a controller including a processor and a memory, the memory including instructions such that the processor is configured to:

receive a sensor data representing a perceived lane edge;

receive map data including a lane edge;

interpolate the sensor data and interpolate the map data to derive a predicted mean, μ u , for each of the sensor data and the map data, and a predicted covariance, Σ v , for each of the sensor data and the map data, according to an interpolation scheme that follows μ v =G u (x v )μ u and Σ u =P u (x v )+G u (x v )Σ u G u (x v ) T where μ u is a known mean from the interpolation of the sensor data from a preceding timestep and the map data from a preceding timestep, Σ u is a known covariance from the interpolation of the sensor data from a preceding timestep and the map data from preceding timestep, G u (x v ) is an interpolator gain, P u (x v ) is an interpolation uncertainty, and xv is an x-axis location of the perceived lane edge in a Cartesian coordinate system;

fuse the interpolated sensor data and the interpolated map data; and

generate predicted lane edge centers based on the fused interpolated sensor data and the interpolated map data.

2. The system as recited in claim 1 , wherein the perceived lane edge represents a lane edge disposed on a roadway.

3. The system as recited in claim 2 , wherein the lane edge included in the map data corresponds to the perceived lane edge.

4. The system as recited in claim 1 , wherein the controller is further configured to generate the predicted lane edge centers using a Kalman filter.

5. The system as recited in claim 1 , wherein the controller is further configured to transmit the predicted lane edge centers to an autonomous controller.

6. The system as recited in claim 5 , wherein the autonomous controller is configured to operate a vehicle based on the predicted lane edge centers.

7. The system as recited in claim 6 , wherein the controller is further configured to compare a calculated average perception innovation squared metric to a predetermined average perception innovation squared metric threshold, wherein the calculated average perception innovation squared metric is based on the predicted lane edge centers.

8. The system as recited in claim 7 , wherein the autonomous controller is configured to operate the vehicle based on only the sensor data representing the perceived lane edge.

9. A method for determining a nominal path for a vehicle for autonomous driving, the method comprising:

receiving, via a controller, a sensor data representing perceived lane edges;

receiving map data, via a GPS, wherein the map data includes a lane edge;

converting the map data including the lane edge from a global coordinate system to an ego frame system relative to the vehicle;

interpolating the sensor data and interpolating the converted map data to derive a predicted mean, μ v , for each of the sensor data and the map data, and a predicted covariance, Σ v , for each of the sensor data and the map data, according to an interpolation scheme that follows μ u =G u (x v )μ u and Σ u (P u (x v )+G u (x v )Σ u G u (x v ) T where μ u is a known mean from the interpolation of the sensor data from a preceding timestep and the map data from a preceding timestep, Σ u is a known covariance from the interpolation of the sensor data from a preceding timestep and the map data from preceding timestep, G u (x v ) is an interpolator gain, P u (x v ) is an interpolation uncertainty, and xv is an x-axis location of the perceived lane edge in a Cartesian coordinate system;

fusing the interpolated sensor data and the interpolated map data to generate a fused set of data points;

comparing a distance the vehicle has traveled to a threshold, and updating the map data using the GPS when the distance is greater than or equal to the threshold;

predicting lane edges from the fused set of data points;

caching the predicted lane edges when the distance is less than the threshold;

calculating a difference between the predicted lane edges and the perceived lane edges;

commanding the vehicle to use only perceived lane edges for autonomous driving when the difference between the predicted lane edges and the perceived lane edges is greater than a threshold; and

calculating the nominal path of the vehicle using the predicted lane edges when the difference between the predicted lane edges and the perceived lane edges is less than the threshold.

10. The method as recited in claim 9 , wherein the perceived lane edges represent lane edges disposed on a roadway.

11. The method as recited in claim 9 , wherein the step of predicting lane edges from the fused set of data points includes using a Kalman filter.

12. The method as recited in claim 11 , further comprising:

updating the Kalman filter with the fused set of data points when the difference between the predicted lane edges and the perceived lane edges is less than the threshold.

13. A system for determining a nominal path for a vehicle for autonomous driving, the system comprising a controller including a processor and a memory, the memory including instructions such that the processor is configured to:

receive, via the controller, a sensor data representing perceived lane edges;

receive map data, via a GPS, wherein the map data includes a lane edge;

convert the map data including the lane edge from a global coordinate system to an ego frame system relative to the vehicle;

interpolate the sensor data and interpolate the converted map data to derive a predicted mean, μ v , for each of the sensor data and the map data, and a predicted covariance, Σ v , for each of the sensor data and the map data, according to an interpolation scheme that follows μ v =G u (x v )μ u and Σ v =P u (x v )+G u (x v )Σ u G u (x v ) T where μ u is a known mean from the interpolation of the sensor data from a preceding timestep and the map data from a preceding timestep, Σ u is a known covariance from the interpolation of the sensor data from a preceding timestep and the map data from preceding timestep, G u (x v ) is an interpolator gain, P u (x v ) is an interpolation uncertainty, and xv is an x-axis location of the perceived lane edge in a Cartesian coordinate system;

fuse the interpolated sensor data and the interpolated map data to generate a fused set of data points;

compare a distance the vehicle has traveled to a threshold, and update the map data using the GPS when the distance is greater than or equal to the threshold;

predict lane edges from the fused set of data points;

cache the predicted lane edges when the distance is less than the threshold;

calculate a difference between the predicted lane edges and the perceived lane edges;

command the vehicle to use only perceived lane edges for autonomous driving when the difference between the predicted lane edges and the perceived lane edges is greater than a threshold; and

calculate the nominal path of the vehicle using the predicted lane edges when the difference between the predicted lane edges and the perceived lane edges is less than the threshold.

14. The system as recited in claim 13 , wherein the step of predicting lane edges from the fused set of data points includes using a Kalman filter.

15. The method as recited in claim 14 , wherein the processor is further configured to update the Kalman filter with the fused set of data points when the difference between the predicted lane edges and the perceived lane edges is less than the threshold.