Main Content

This example shows how to generate code for a track-level fusion algorithm in a scenario where the tracks originate from heterogeneous sources with different state definitions. This example is based on the Track-Level Fusion of Radar and Lidar Data (Sensor Fusion and Tracking Toolbox) example, in which the state spaces of the tracks generated from lidar and radar sources are different.

You can generate code for a `trackFuser`

(Sensor Fusion and Tracking Toolbox) using MATLAB® Coder™. To do so, you must modify your code to comply with the following limitations:

**Code Generation Entry Function**

Follow the instructions on how to use System Objects in MATLAB Code Generation (MATLAB Coder). For code generation, you must first define an entry-level function, in which the object is defined. Also, the function cannot use arrays of objects as inputs or outputs. In this example, you define the entry-level function as the heterogeneousInputsFuser function. The function must be on the path when you generate code for it. Therefore, it cannot be part of this live script and is attached in this example. The function accepts local tracks and current time as input and outputs central tracks.

To preserve the state of the fuser between calls to the function, you define the fuser as a `persistent`

variable. On the first call, you must define the fuser variable because it is empty. The rest of the following code steps the `trackFuser`

and returns the fused tracks.

function tracks = heterogeneousInputsFuser(localTracks,time) %#codegen persistent fuser if isempty(fuser) % Define the radar source configuration radarConfig = fuserSourceConfiguration('SourceIndex',1,... 'IsInitializingCentralTracks',true,... 'CentralToLocalTransformFcn',@central2local,... 'LocalToCentralTransformFcn',@local2central); % Define the lidar source configuration lidarConfig = fuserSourceConfiguration('SourceIndex',2,... 'IsInitializingCentralTracks',true,... 'CentralToLocalTransformFcn',@central2local,... 'LocalToCentralTransformFcn',@local2central); % Create a trackFuser object fuser = trackFuser(... 'MaxNumSources', 2, ... 'SourceConfigurations',{radarConfig;lidarConfig},... 'StateTransitionFcn',@helperctcuboid,... 'StateTransitionJacobianFcn',@helperctcuboidjac,... 'ProcessNoise',diag([1 3 1]),... 'HasAdditiveProcessNoise',false,... 'AssignmentThreshold',[250 inf],... 'ConfirmationThreshold',[3 5],... 'DeletionThreshold',[5 5],... 'StateFusion','Custom',... 'CustomStateFusionFcn',@helperRadarLidarFusionFcn); end tracks = fuser(localTracks, time); end

**Homogeneous Source Configurations**

In this example, you define the radar and lidar source configurations differently than in the original Track-Level Fusion of Radar and Lidar Data (Sensor Fusion and Tracking Toolbox) example. In the original example, the `CentralToLocalTransformFcn`

and `LocalToCentralTransformFcn`

properties of the two source configurations are different because they use different function handles. This makes the source configurations a heterogeneous cell array. Such a definition is correct and valid when executing in MATLAB. However, in code generation, all source configurations must use the same function handles. To avoid the different function handles, you define one function to transform tracks from central (fuser) definition to local (source) definition and one function to transform from local to central. Each of these functions switches between the transform functions defined for the individual sources in the original example. Both functions are part of the heterogeneousInputsFuser function.

Here is the code for the `local2central`

function, which uses the `SourceIndex`

property to determine the correct function to use. Since the two types of local tracks transform to the same definition of central track, there is no need to predefine the central track.

function centralTrack = local2central(localTrack) switch localTrack.SourceIndex case 1 % radar centralTrack = radar2central(localTrack); otherwise % lidar centralTrack = lidar2central(localTrack); end end

The function `central2local`

transforms the central track into a radar track if `SourceIndex`

is 1 or into a lidar track if `SourceIndex`

is 2. Since the two tracks have a different definition of `State`

, `StateCovariance`

, and `TrackLogicState`

, you must first predefine the output. Here is the code snippet for the function:

function localTrack = central2local(centralTrack) state = 0; stateCov = 1; coder.varsize('state', [10, 1], [1 0]); coder.varsize('stateCov', [10 10], [1 1]); localTrack = objectTrack('State', state, 'StateCovariance', stateCov); switch centralTrack.SourceIndex case 1 localTrack = central2radar(centralTrack); case 2 localTrack = central2lidar(centralTrack); otherwise % This branch is never reached but is necessary to force code % generation to use the predefined localTrack. end end

The functions `radar2central`

and `central2radar`

are the same as in the original example but moved from the live script to the heterogeneousInputsFuser function. You also add the `lidar2central`

and `central2lidar`

functions to the heterogeneousInputsFuser function. These two functions convert from the track definition that the fuser uses to the lidar track definition.

Before generating code, make sure that the example still runs after all the changes made to the fuser. The file `lidarRadarData.mat`

contains the same scenario as in the original example. It also contains a set of radar and lidar tracks recorded at each step of that example. You also use a similar display to visualize the example and define the same `trackGOSPAMetric`

objects to evaluate the tracking performance.

% Load the scenario and recorded local tracks load('lidarRadarData.mat','scenario','localTracksCollection') display = helperTrackFusionCodegenDisplay('FollowActorID',3); showLegend(display,scenario); % Radar GOSPA gospaRadar = trackGOSPAMetric('Distance','custom',... 'DistanceFcn',@helperRadarDistance,... 'CutoffDistance',25); % Lidar GOSPA gospaLidar = trackGOSPAMetric('Distance','custom',... 'DistanceFcn',@helperLidarDistance,... 'CutoffDistance',25); % Central/Fused GOSPA gospaCentral = trackGOSPAMetric('Distance','custom',... 'DistanceFcn',@helperLidarDistance,... % State space is same as lidar 'CutoffDistance',25); gospa = zeros(3,0); missedTargets = zeros(3,0); falseTracks = zeros(3,0); % Ground truth for metrics. This variable updates every time step % automatically, because it is a handle to the actors. groundTruth = scenario.Actors(2:end); fuserStepped = false; fusedTracks = objectTrack.empty; idx = 1; clear heterogeneousInputsFuser while advance(scenario) time = scenario.SimulationTime; localTracks = localTracksCollection{idx}; if ~isempty(localTracks) || fuserStepped fusedTracks = heterogeneousInputsFuser(localTracks,time); fuserStepped = true; end radarTracks = localTracks([localTracks.SourceIndex]==1); lidarTracks = localTracks([localTracks.SourceIndex]==2); % Capture GOSPA and its components for all trackers [gospa(1,idx),~,~,~,missedTargets(1,idx),falseTracks(1,idx)] = gospaRadar(radarTracks, groundTruth); [gospa(2,idx),~,~,~,missedTargets(2,idx),falseTracks(2,idx)] = gospaLidar(lidarTracks, groundTruth); [gospa(3,idx),~,~,~,missedTargets(3,idx),falseTracks(3,idx)] = gospaCentral(fusedTracks, groundTruth); % Update the display display(scenario,[],[], radarTracks,... [],[],[],[], lidarTracks, fusedTracks); idx = idx + 1; end

To generate code, you must define the input types for both the radar and lidar tracks and the timestamp. In both the original script and in the previous section, the radar and lidar tracks are defined as arrays of `objectTrack`

(Sensor Fusion and Tracking Toolbox) objects. In code generation, the entry-level function cannot use an array of objects. Instead, you define an array of structures.

You use the struct `oneLocalTrack`

to define the inputs coming from radar and lidar tracks. In code generation, the specific data types of each field in the struct must be defined exactly the same as the types defined for the corresponding properties in the recorded tracks. Furthermore, the size of each field must be defined correctly. You use the `coder.typeof`

(MATLAB Coder) function to specify fields that have variable size: `State`

, `StateCovariance`

, and `TrackLogicState`

. You define the `localTracks`

input using the `oneLocalTrack`

struct and the `coder.typeof`

function, because the number of input tracks varies from zero to eight in each step. You use the function `codegen`

(MATLAB Coder) to generate the code.

Notes:

If the input tracks use different types for the

`State`

and`StateCovariance`

properties, you must decide which type to use, double or single. In this example, all tracks use double precision and there is no need for this step.If the input tracks use different definitions of

`StateParameters`

, you must first create a superset of all`StateParameters`

and use that superset in the`StateParameters`

field. A similar process must be done for the`ObjectAttributes`

field. In this example, all tracks use the same definition of`StateParameters`

and`ObjectAttributes`

.

% Define the inputs to fuserHeterogeneousInputs for code generation oneLocalTrack = struct(... 'TrackID', uint32(0), ... 'BranchID', uint32(0), ... 'SourceIndex', uint32(0), ... 'UpdateTime', double(0), ... 'Age', uint32(0), ... 'State', coder.typeof(1, [10 1], [1 0]), ... 'StateCovariance', coder.typeof(1, [10 10], [1 1]), ... 'StateParameters', struct, ... 'ObjectClassID', double(0), ... 'TrackLogic', 'History', ... 'TrackLogicState', coder.typeof(false, [1 10], [0 1]), ... 'IsConfirmed', false, ... 'IsCoasted', false, ... 'IsSelfReported', false, ... 'ObjectAttributes', struct); localTracks = coder.typeof(oneLocalTrack, [8 1], [1 0]); fuserInputArguments = {localTracks, time}; codegen heterogeneousInputsFuser -args fuserInputArguments;

You run the generated code like you ran the MATLAB code, but first you must reinitialize the scenario, the GOSPA objects, and the display.

You use the `toStruct`

(Sensor Fusion and Tracking Toolbox) object function to convert the input tracks to arrays of structures.

Notes:

If the input tracks use different data types for the

`State`

and`StateCovariance`

properties, make sure to cast the`State`

and`StateCovariance`

of all the tracks to the data type you chose when you defined the`oneLocalTrack`

structure above.If the input tracks required a superset structure for the fields

`StateParameters`

or`ObjectAttributes`

, make sure to populate these structures correctly before calling the`mex`

file.

You use the `gospaCG`

variable to keep the GOSPA metrics for this run so that you can compare them to the GOSPA values from the MATLAB run.

% Rerun the scenario with the generated code fuserStepped = false; fusedTracks = objectTrack.empty; gospaCG = zeros(3,0); missedTargetsCG = zeros(3,0); falseTracksCG = zeros(3,0); idx = 1; clear heterogeneousInputsFuser_mex reset(display); reset(gospaRadar); reset(gospaLidar); reset(gospaCentral); restart(scenario); while advance(scenario) time = scenario.SimulationTime; localTracks = localTracksCollection{idx}; if ~isempty(localTracks) || fuserStepped fusedTracks = heterogeneousInputsFuser_mex(toStruct(localTracks),time); fuserStepped = true; end radarTracks = localTracks([localTracks.SourceIndex]==1); lidarTracks = localTracks([localTracks.SourceIndex]==2); % Capture GOSPA and its components for all trackers [gospaCG(1,idx),~,~,~,missedTargetsCG(1,idx),falseTracksCG(1,idx)] = gospaRadar(radarTracks, groundTruth); [gospaCG(2,idx),~,~,~,missedTargetsCG(2,idx),falseTracksCG(2,idx)] = gospaLidar(lidarTracks, groundTruth); [gospaCG(3,idx),~,~,~,missedTargetsCG(3,idx),falseTracksCG(3,idx)] = gospaCentral(fusedTracks, groundTruth); % Update the display display(scenario,[],[], radarTracks,... [],[],[],[], lidarTracks, fusedTracks); idx = idx + 1; end

At the end of the run, you want to verify that the generated code provided the same results as the `MATLAB`

code. Using the GOSPA metrics you collected in both runs, you can compare the results at the high level. Due to numerical roundoffs, there may be small differences in the results of the generated code relative to the `MATLAB`

code. To compare the results, you use the absolute differences between GOSPA values and check if they are all smaller than 1e-10. The results show that the differences are very small.

% Compare the GOSPA values from MATLAB run and generated code areGOSPAValuesEqual = all(abs(gospa-gospaCG)<1e-10,'all'); disp("Are GOSPA values equal up to the 10th decimal (true/false)? " + string(areGOSPAValuesEqual))

Are GOSPA values equal up to the 10th decimal (true/false)? true

In this example, you learned how to generate code for a track-level fusion algorithm when the input tracks are heterogeneous. You learned how to define the `trackFuser`

and its `SourceConfigurations`

property to support heterogeneous sources. You also learned how to define the input in compilation time and how to pass it to the mex file in runtime.

The following functions are used by the GOSPA metric.

`helperLidarDistance`

Function to calculate a normalized distance between the estimate of a track in radar state-space and the assigned ground truth.

function dist = helperLidarDistance(track, truth) % Calculate the actual values of the states estimated by the tracker % Center is different than origin and the trackers estimate the center rOriginToCenter = -truth.OriginOffset(:) + [0;0;truth.Height/2]; rot = quaternion([truth.Yaw truth.Pitch truth.Roll],'eulerd','ZYX','frame'); actPos = truth.Position(:) + rotatepoint(rot,rOriginToCenter')'; % Actual speed and z-rate actVel = [norm(truth.Velocity(1:2));truth.Velocity(3)]; % Actual yaw actYaw = truth.Yaw; % Actual dimensions. actDim = [truth.Length;truth.Width;truth.Height]; % Actual yaw rate actYawRate = truth.AngularVelocity(3); % Calculate error in each estimate weighted by the "requirements" of the % system. The distance specified using Mahalanobis distance in each aspect % of the estimate, where covariance is defined by the "requirements". This % helps to avoid skewed distances when tracks under/over report their % uncertainty because of inaccuracies in state/measurement models. % Positional error. estPos = track.State([1 2 6]); reqPosCov = 0.1*eye(3); e = estPos - actPos; d1 = sqrt(e'/reqPosCov*e); % Velocity error estVel = track.State([3 7]); reqVelCov = 5*eye(2); e = estVel - actVel; d2 = sqrt(e'/reqVelCov*e); % Yaw error estYaw = track.State(4); reqYawCov = 5; e = estYaw - actYaw; d3 = sqrt(e'/reqYawCov*e); % Yaw-rate error estYawRate = track.State(5); reqYawRateCov = 1; e = estYawRate - actYawRate; d4 = sqrt(e'/reqYawRateCov*e); % Dimension error estDim = track.State([8 9 10]); reqDimCov = eye(3); e = estDim - actDim; d5 = sqrt(e'/reqDimCov*e); % Total distance dist = d1 + d2 + d3 + d4 + d5; end

`helperRadarDistance`

Function to calculate a normalized distance between the estimate of a track in radar state-space and the assigned ground truth.

function dist = helperRadarDistance(track, truth) % Calculate the actual values of the states estimated by the tracker % Center is different than origin and the trackers estimate the center rOriginToCenter = -truth.OriginOffset(:) + [0;0;truth.Height/2]; rot = quaternion([truth.Yaw truth.Pitch truth.Roll],'eulerd','ZYX','frame'); actPos = truth.Position(:) + rotatepoint(rot,rOriginToCenter')'; actPos = actPos(1:2); % Only 2-D % Actual speed actVel = norm(truth.Velocity(1:2)); % Actual yaw actYaw = truth.Yaw; % Actual dimensions. Only 2-D for radar actDim = [truth.Length;truth.Width]; % Actual yaw rate actYawRate = truth.AngularVelocity(3); % Calculate error in each estimate weighted by the "requirements" of the % system. The distance specified using Mahalanobis distance in each aspect % of the estimate, where covariance is defined by the "requirements". This % helps to avoid skewed distances when tracks under/over report their % uncertainty because of inaccuracies in state/measurement models. % Positional error estPos = track.State([1 2]); reqPosCov = 0.1*eye(2); e = estPos - actPos; d1 = sqrt(e'/reqPosCov*e); % Speed error estVel = track.State(3); reqVelCov = 5; e = estVel - actVel; d2 = sqrt(e'/reqVelCov*e); % Yaw error estYaw = track.State(4); reqYawCov = 5; e = estYaw - actYaw; d3 = sqrt(e'/reqYawCov*e); % Yaw-rate error estYawRate = track.State(5); reqYawRateCov = 1; e = estYawRate - actYawRate; d4 = sqrt(e'/reqYawRateCov*e); % Dimension error estDim = track.State([6 7]); reqDimCov = eye(2); e = estDim - actDim; d5 = sqrt(e'/reqDimCov*e); % Total distance dist = d1 + d2 + d3 + d4 + d5; % A constant penalty for not measuring 3-D state dist = dist + 3; end

`trackFuser`

(Sensor Fusion and Tracking Toolbox)