Systems and methods for vehicular navigation and localization

1. A system for vehicular navigation and localization, the system comprising:

one or more sensors to output sensor data;

one or more processors; and

a memory communicably coupled to the one or more processors and storing:

a computation module including instructions that when executed by the one or more processors cause the one or more processors to:

extract perceptual features from the sensor data;

extract unrouted-map features from unrouted map data;

combine the perceptual features and the unrouted-map features to produce first combined features data;

output, based at least in part on the first combined features data, parameters of a probability distribution for one or more steering trajectories that are available to a vehicle;

extract routed-map features from routed map data;

combine the routed-map features with the first combined features data to produce second combined features data; and

output, based at least in part on the second combined features data, a deterministic steering control signal corresponding to a particular one of the one or more steering trajectories;

a control module including instructions that when executed by the one or more processors cause the one or more processors to control steering of the vehicle based, at least in part, on the deterministic steering control signal; and

a localization module including instructions that when executed by the one or more processors cause the one or more processors to perform a localization of the vehicle based, at least in part, on the parameters of the probability distribution and an updated posterior estimate of a pose of the vehicle, wherein computing the updated posterior estimate of the pose of the vehicle includes performing double marginalization over the deterministic steering control signal and the pose of the vehicle.

2. The system of claim 1 , wherein the vehicle is an ego vehicle and the computation module includes further instructions that when executed by the one or more processors cause the one or more processors to:

predict that the ego vehicle will travel along a particular one of the one or more steering trajectories based, at least in part, on the parameters of the probability distribution; and

control one or more vehicle systems of the ego vehicle in response to the particular one of the one or more steering trajectories.

3. The system of claim 1 , wherein the vehicle is a road agent external to an ego vehicle and the computation module includes further instructions that when executed by the one or more processors cause the one or more processors to:

predict that the road agent will travel along a particular one of the one or more steering trajectories based, at least in part, on the parameters of the probability distribution; and

control one or more vehicle systems of the ego vehicle in response to the particular one of the one or more steering trajectories.

4. The system of claim 1 , wherein the localization module includes instructions to perform the localization of the vehicle by distinguishing a particular location in a set of locations from the other locations in the set of locations based, at least in part, on the parameters of the probability distribution.

5. The system of claim 1 , wherein the probability distribution is for a steering control command over inverse curvature conditioned on the perceptual features and the unrouted-map features.

6. The system of claim 5 , wherein the probability distribution is a Gaussian Mixture Model.

7. The system of claim 1 , wherein the localization module includes further instructions to:

detect that at least one of the localization of the vehicle and the unrouted map data is inconsistent with the sensor data; and

correct the localization of the vehicle based, at least in part, on the parameters of the probability distribution.

8. A non-transitory computer-readable medium for vehicular navigation and localization and storing instructions that when executed by one or more processors cause the one or more processors to:

extract perceptual features from sensor data;

extract unrouted-map features from unrouted map data;

combine the perceptual features and the unrouted-map features to produce first combined features data;

output, based at least in part on the first combined features data, parameters of a probability distribution for one or more steering trajectories that are available to a vehicle;

extract routed-map features from routed map data;

combine the routed-map features with the first combined features data to produce second combined features data;

output, based at least in part on the second combined features data, a deterministic steering control signal corresponding to a particular one of the one or more steering trajectories;

control steering of the vehicle based, at least in part, on the deterministic steering control signal; and

perform a localization of the vehicle based, at least in part, on the parameters of the probability distribution and an updated posterior estimate of a pose of the vehicle, wherein computing the updated posterior estimate of the pose of the vehicle includes performing double marginalization over the deterministic steering control signal and the pose of the vehicle.

9. A method of vehicular navigation and localization, the method comprising:

extracting perceptual features from sensor data;

extracting unrouted-map features from unrouted map data;

combining the perceptual features and the unrouted-map features to produce first combined features data;

outputting, based at least in part on the first combined features data, parameters of a probability distribution for one or more steering trajectories that are available to a vehicle;

extracting routed-map features from routed map data;

combining the routed-map features with the first combined features data to produce second combined features data;

outputting, based at least in part on the second combined features data, a deterministic steering control signal corresponding to a particular one of the one or more steering trajectories;

controlling steering of the vehicle based, at least in part, on the deterministic steering control signal; and

performing a localization of the vehicle based, at least in part, on the parameters of the probability distribution and an updated posterior estimate of a pose of the vehicle, wherein computing the updated posterior estimate of the pose of the vehicle includes performing double marginalization over the deterministic steering control signal and the pose of the vehicle.

10. The method of claim 9 , wherein the vehicle is an ego vehicle and the method further comprises:

predicting that the ego vehicle will travel along a particular one of the one or more steering trajectories based, at least in part, on the parameters of the probability distribution; and

controlling one or more vehicle systems of the ego vehicle in response to the particular one of the one or more steering trajectories.

11. The method of claim 9 , wherein the vehicle is a road agent external to an ego vehicle and the method further comprises:

predicting that the road agent will travel along a particular one of the one or more steering trajectories based, at least in part, on the parameters of the probability distribution; and

controlling one or more vehicle systems of the ego vehicle in response to the particular one of the one or more steering trajectories.

12. The method of claim 9 , wherein performing the localization of the vehicle includes distinguishing a particular location in a set of locations from the other locations in the set of locations based, at least in part, on the parameters of the probability distribution.

13. The method of claim 9 , further comprising:

detecting that at least one of the localization of the vehicle and the unrouted map data is inconsistent with the sensor data; and

correcting the localization of the vehicle based, at least in part, on the parameters of the probability distribution.