Localization determination for vehicle operation

An autonomous or semi-autonomous vehicle comprising:

• a processor configured to execute instructions stored on a non-transitory computer readable medium to:
• determine vehicle operational information of the vehicle, including sensing a position of the vehicle in a global coordinate system;
• determine a metric location estimate of the vehicle using the vehicle operational information, including determining the metric location estimate using the sensed position as input to a non-linear loss function with a Kalman filter to mitigate effects of un-modeled sensor error, wherein the metric location estimate is an output of the Kalman filter;
• determine operational environment information of a portion of a vehicle transportation network, the operational environment information including sensor data of a portion of the vehicle transportation network that is observable to the vehicle;
• determine a topological location estimate of the vehicle within the vehicle transportation network using the metric location estimate and the operational environment information, wherein the topological location estimate includes a lane membership of the vehicle within the vehicle transportation network;
• and
• traverse, by the vehicle, the vehicle transportation network based on the topological location estimate of the vehicle as input to perform a vehicle operation.

An autonomous or semi-autonomous vehicle, comprising:

• a processor configured to execute instructions stored on a non-transitory computer readable medium to:
• determine vehicle operational information of the vehicle, including sensing a position of the vehicle in a global coordinate system;
• determine a metric location estimate of the vehicle using the vehicle operational information, including determining the metric location estimate using the sensed position as input to a non-linear loss function with a Kalman filter to mitigate effects of un-modeled sensor error, wherein the metric location estimate is an output of the Kalman filter:
• determine operational environment information of a portion of a vehicle transportation network, the operational environment information including sensor data of a portion of the vehicle transportation network that is observable to the vehicle;
• determine a topological location estimate of the vehicle within the vehicle transportation network using the metric location estimate and the operational environment information, wherein the topological location estimate includes a lane membership of the vehicle within the vehicle transportation network;
• and
• traverse, by the vehicle, the vehicle transportation network based on the topological location estimate of the vehicle as input to perform a vehicle operation,
• wherein the processor is configured to execute the instructions stored on the non-transitory computer readable medium to:
• determine the vehicle operational information by determining a longitude and a latitude of the vehicle from the sensed position at multiple temporal points while the vehicle is traversing the vehicle transportation network;
• determine the metric location estimate by:
• determining a location estimate for respective temporal points of the multiple temporal points;
• determining a projected location estimate for respective, subsequent temporal points;
• and
• determining the metric location estimate of a current temporal point using the non-linear loss function with the Kalman filter to mitigate the effects of un-modeled sensor error, the non-linear loss function based on a comparison of the location estimate with the projected location estimate for a same temporal point;
• and
• wherein the non-linear loss function comprises multiple radii defining concentric circles about a first projected location estimate at a current temporal point, the non-linear loss function:
• providing sensed values of the longitude and the latitude at the current temporal point to the Kalman filter, for determining the metric location estimate of the current temporal point, responsive to the location estimate for the current temporal point being located within a first circle having a smallest radius of the multiple radii;
• omitting the sensed values from the Kalman filter, for determining the metric location estimate of the current temporal point, responsive to the location estimate for the current temporal point being located outside of a second circle having a largest radius of the multiple radii;
• and
• applying a penalty to the sensed values to obtain penalized sensed values, and providing the penalized sensed values to the Kalman filter, for determining the location estimate of the current temporal point, responsive to the metric location estimate for the current temporal point being located outside of the first circle and inside of the second circle.