A method for determining an actual trajectory of a vehicle using object detection data and vehicle dynamics data. An object detection system identifies point objects and extended objects in proximity to the vehicle, where the point objects are less than a meter in length and width. An updated vehicle pose is calculated which optimally transposes the point objects in the scan data to a target list of previously-identified point objects. The updated vehicle pose is further refined by iteratively calculating a pose which optimally transposes the extended objects in the scan data to a target model of previously-identified extended objects, where the iteration is used to simultaneously determine a probability coefficient relating the scan data to the target model. The updated vehicle pose is used to identify the actual trajectory of the vehicle, which is compared to a planned path in a collision avoidance system.