This example shows how to perform track-to-track fusion in Simulink® with Sensor Fusion and Tracking Toolbox™. In the context of autonomous driving, the example illustrates how to build a decentralized tracking architecture using a track fuser block. In the example, each vehicle perform tracking independently as well as fuse tracking information received from other vehicles. This example closely follows the Track-to-Track Fusion for Automotive Safety Applications MATLAB® example.
Automotive safety applications largely rely on the situational awareness of the vehicle. A better situational awareness provides the basis to a successful decision-making for different situations. To achieve this, vehicles can benefit from intervehicle data fusion. This example illustrates the workflow in Simulink for fusing data from two vehicles to enhance situational awareness of the vehicle.
Prior to running this example, the drivingScenario
object was used to create the same scenario defined in Track-to-Track Fusion for Automotive Safety Applications. The roads and actors from this scenario were then saved to the scenario object file Scene.mat
.
In the Tracking and Fusion section of the model there are two subsystems which implements the target tracking and fusion capabilities of Vehicle1
and Vehicle2
in this scenario.
Vehicle1
This subsystem includes the Scenario Reader (Automated Driving Toolbox) block that reads the actor pose data from the saved file. The block converts the actor poses from the world coordinates of the scenario into ego vehicle coordinates. The actor poses are streamed on a bus generated by the block. The actor poses are used by the Sensor Simulation subsystem, which generates radar and vision detections. These detections are then passed to the JPDA Tracker V1
block which processes the detections to generate a list of tracks. The tracks are then passed into a Track Concatenation1
block, which concatenates these input tracks. The first input to the Track Concatenation1
block is the local tracks from the JPDA tracker and the second input is the tracks received from the other vehicle's track fuser. To transform local tracks to central tracks, the track fuser needs the parameter information about the local tracks. However, this information is not available from the direct outputs of the JPDA tracker. Therefore, a helper Update Pose block is used to supply these information by reading the data from the v1Pose.mat file. The updated tracks are then broadcasted to T2TF Tracker V1
block as an input. Finally, the trackFuser
T2TF Tracker V1
block fuses the local vehicle tracks with the tracks received from the other vehicle's track fuser. After each update, the track fuser on each vehicle broadcasts its fused tracks to be fed into the update of the other vehicle's track fuser in the next time stamp.
Vehicle2
Vehicle2
subsystem follows similar setup as Vehicle1
subsystem as described above.
Visualization
The Visualization block is implemented using the MATLAB System block and is defined using HelperTrackDisplay
block. The block uses RunTimeObject
parameter of the blocks to display their outputs. See Access Block Data During Simulation (Simulink) for further information on how to access block outputs during simulation.
After running the model, you visualize the results as on the figure. The animation below shows the results for this simulation.
The visualization includes two panels. The left panel shows the detections, local tracks, and fused tracks that Vehicle1
generated during the simulation and represents the situational awareness of the Vehicle1
. The right panel shows the situational awareness of Vehicle2
.
The recorded detections are represented by black circles. The local and fused tracks from Vehicle1
are represented by square and diamond respectively. The local and fused tracks from Vehicle2
represented by a solid black square and diamond. Notice that during the start of simulation, Vehicle1
detects vehicles parked on the right side of the street, and tracks associated with the parked vehicles are confirmed. Currently Vehicle2
only detects Vehicle1
which is immediately in front of it. As the simulation continues the confirmed tracks from Vehicle1
are broadcasts to the fuser on Vehicle2
. After fusing the tracks, vehicle2
becomes aware of the objects prior to detecting these objects on its own. Similarly, Vehicle2
tracks are broadcasts to Vehicle1
. Vehicle1
fuses these tracks and becomes aware of the objects prior to detecting them on its own.
In particular, you observe that the pedestrian standing between the blue and purple car on the right side of the street is detected and tracked by Vehicle1
. Vehicle2
first becomes aware of the pedestrian by fusing the track from Vehicle1
at around 0.8 seconds. It takes Vehicle2
roughly 3 seconds before it starts detecting the pedestrian using its own sensor. The ability to track a pedestrian based on inputs from Vehicle1
allows Vehicle2
to extend its situational awareness and to mitigate the risk of accident.
This example showed how to perform track-to-track fusion in Simulink. You learned how to perform tracking using a decentralized tracking architecture, where each vehicle is responsible for maintaining its own local tracks, fuse tracks from other vehicles, and communicate the tracks to the other vehicle. You also used a JPDA tracker block to generate the local tracks.