Updating intensities in a PHD filter based on a sensor track ID

A tracking system comprising:

• one or more processing devices onboard a vehicle;
• a plurality of sensors onboard the vehicle and operatively coupled to the one or more processing devices;
• and
• one or more data storage devices onboard the vehicle and including instructions which, when executed by the one or more processing devices, cause the one or more processing devices to track multiple objects in an environment around the vehicle with a probabilistic hypothesis density filter, wherein the instructions cause the one or more processing devices to:
• obtain a plurality of sensor measurements corresponding to a first object with at least one sensor onboard the vehicle;
• obtain, by the at least one sensor, a first track identifier (ID) for the plurality of sensor measurements, wherein the first track ID is correlated across time to the plurality of sensor measurements;
• generate a Tk first track intensity for the first object based on measurement statistics from the plurality of sensor measurements, the Tk first track intensity including a weight, a state mean vector, and a state covariance matrix of statistics of a track of the first object at time Tk;
• generate a Tk+1 first predicted intensity for the first object based on the Tk first track intensity, the Tk+1 first predicted intensity corresponding to time Tk+1;
• obtain a Tk+1 sensor measurement from a first sensor of the plurality of sensors, the first sensor providing a second track ID for the Tk+1 sensor measurement, wherein the Tk+1 sensor measurement corresponds to time Tk+1;
• compare the second track ID to the first track ID;
• selectively update the Tk+1 first predicted intensity with Tk+1 measurement statistics from the Tk+1 sensor measurement, based on whether the second track ID matches the first track ID to generate a Tk+1 first measurement-to-track intensity for the first object at time Tk+1;
• generate a Tk+1 second track intensity for a second object based on the new intensity, the Tk+1 second track intensity corresponding to time Tk+1, generate a Tk+2 second predicted intensity based on the Tk+1 second track intensity, the Tk+2 second predicted intensity corresponding to time Tk+2;
• associate the second track ID with the Tk+2 second predicted intensity;
• obtain a Tk+2 sensor measurement from the first sensor, the first sensor providing a third track ID for the Tk+2 sensor measurement, wherein the Tk+2 sensor measurement corresponds to time Tk+2;
• compare the third track ID to the second track ID;
• compute a statistical distance between the Tk+2 second predicted intensity and Tk+2 measurement statistics from the Tk+2 sensor measurement;
• and
• selectively update the Tk+2 second predicted intensity with the Tk+2 measurement statistics based on whether the third track ID matches the second track ID and based on whether the statistical distance is less than a threshold to generate a Tk+2 second measurement-to-track intensity for the second object at time Tk+2,
• wherein the probabilistic hypothesis density filter is operative to maintain a single intensity for each object being tracked.

A non-transitory computer readable medium including instructions which, when executed by one or more processing devices, cause the one or more processing devices to:

• track multiple objects in an environment around a vehicle with a probabilistic hypothesis density filter, wherein the instructions cause the one or more processing devices to:
• obtain a plurality of sensor measurements corresponding to a first object with at least one sensor onboard the vehicle;
• obtain, by the at least one sensor, a first track identifier (ID) for the plurality of sensor measurements, wherein the first track ID is correlated across time to the plurality of sensor measurements;
• generate a Tk first track intensity for the first object based on measurement statistics from the plurality of sensor measurements, the Tk first track intensity including a weight, a state mean vector, and a state covariance matrix of statistics of a track of the first object at time Tk;
• generate a Tk+1 first predicted intensity for the first object based on the Tk first track intensity, the Tk+1 first predicted intensity corresponding to time Tk+1;
• obtain a Tk+1 sensor measurement from a first sensor of the plurality of sensors, the first sensor providing a second track ID for the Tk+1 sensor measurement, wherein the Tk+1 sensor measurement corresponds to time Tk+1;
• compare the second track ID to the first track ID;
• selectively update the Tk+1 first predicted intensity with Tk+1 measurement statistics from the Tk+1 sensor measurement, based on whether the second track ID matches the first track ID to generate a Tk+1 first measurement-to-track intensity for the first object at time Tk+1;
• generate a Tk+1 second track intensity for a second object based on the new intensity, the Tk+1 second track intensity corresponding to time Tk+1;
• generate a Tk+2 second predicted intensity based on the Tk+1 second track intensity, the Tk+2 second predicted intensity corresponding to time Tk+2;
• associate the second track ID with the Tk+2 second predicted intensity;
• obtain a Tk+2 sensor measurement from the first sensor, the first sensor providing a third track ID for the Tk+2 sensor measurement, wherein the Tk+2 sensor measurement corresponds to time Tk+2;
• compare the third track ID to the second track ID;
• compute a statistical distance between the Tk+2 second predicted intensity and Tk+2 measurement statistics from the Tk+2 sensor measurement;
• and
• selectively update the Tk+2 second predicted intensity with the Tk+2 measurement statistics based on whether the third track ID matches the second track ID and based on whether the statistical distance is less than a threshold to
• generate a Tk+2 second measurement-to-track intensity for the second object at time Tk+2;
• wherein the probabilistic hypothesis density filter is operative to maintain a single intensity for each object being tracked.