System and method for out of sequence measurement processing with uncertain models

A non-transitory computer-readable medium having program code recorded thereon for out of sequence measurement (OOSM) state estimation, the program code being executed by a processor and comprising:

• program code to forward predict a head estimate of a buffer computed at a current time to a future time to provide a first particle;
• program code to forward predict the head estimate computed at the current time and an out of sequence measurement (OOSM) to the future time as a second particle;
• program code to forward predict the head estimate computed at the current time without the OOSM to the future time as a third particle;
• and
• program code to update the head estimate of the buffer according to a fusion of the first particle, the second particle, and the third particle.

A system for out of sequence measurement (OOSM) state estimation, the system comprising:

• a head estimate prediction module to forward predict a head estimate of a buffer computed at a current time to a future time to provide a first particle;
• a head estimate branching model to forward predict the head estimate computed at the current time and an out of sequence measurement (OOSM) to the future time as a second particle and to forward predict the head estimate computed at the current time without the OOSM to the future time as a third particle;
• a particle fusion module to update the head estimate of the buffer according to a fusion of the first particle, the second particle, and the third particle;
• and
• a vehicle state estimation module to estimate a vehicle state based on the updated head estimate of the buffer according to the fusion of the first particle, the second particle, and the third particle.