Сгенерируйте код для трека Fuser с гетерогенными исходными треками

Этот пример показывает, как сгенерировать код для алгоритма слияния уровня дорожки в сценарии, где дорожки происходят из гетерогенных источников с различными определениями состояний. Этот пример основан на Track-Level Fusion of Radar и Лидаре Данных примере, в котором пространства состояний треков, сгенерированных из лидара и радиолокационных источников, отличаются.

Определите Track Fuser для генерации кода

Можно сгенерировать код для trackFuser использование MATLAB ® Coder™. Для этого необходимо изменить код в соответствии со следующими ограничениями:

Функция ввода генерации кода

Следуйте инструкциям по использованию системных объектов в генерации кода MATLAB (MATLAB Coder). Для генерации кода необходимо сначала определить функцию уровня входа, в которой определен объект. Кроме того, функция не может использовать массивы объектов в качестве входных или выходных параметров. В этом примере вы задаете функцию уровня входа как функцию гетерогенногоInputsFuser. Функция должна быть на пути, когда вы генерируете код для нее. Поэтому он не может быть частью этого live скрипта и прилагается в этом примере. Функция принимает локальные дорожки и текущее время как вход и выводит центральные дорожки.

Чтобы сохранить состояние фузера между вызовами функции, вы определяете фузер как persistent переменная. При первом вызове необходимо задать переменную fuser, поскольку она пуста. Остальная часть следующего кода шагает trackFuser и возвращает слитые дорожки.

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

Однородные строения

В этом примере вы задаете строения источника радара и лидара по-другому, чем в исходном примере Track-Level Fusion of Radar и Lidar Data. В исходном примере CentralToLocalTransformFcn и LocalToCentralTransformFcn свойства двух исходных строений различаются, поскольку они используют различные указатели на функцию. Это делает исходные строения неоднородным массивом ячеек. Такое определение верно и допустимо при выполнении в MATLAB. Однако при генерации кода все исходные строения должны использовать одни и те же указатели на функцию. Чтобы избежать различных указателей на функцию, вы задаете одну функцию для преобразования треков из центрального (fuser) определения в локальное (source) определение и одну функцию для преобразования из локального в центральное. Каждая из этих функций переключается между функциями преобразования, заданными для отдельных источников в исходном примере. Обе функции являются частью функции heterogeneousInputsFuser.

Вот код для local2central функция, которая использует SourceIndex свойство для определения правильной функции. Поскольку два типа локальных треков преобразуются в одно и то же определение центрального трека, нет необходимости предопределять центральный трек.

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

Функция central2local преобразует центральную дорожку в радиолокационную, если SourceIndex равен 1 или в лидарную дорожку, если SourceIndex равен 2. Поскольку эти два трека имеют разное определение State, StateCovariance, и TrackLogicStateСначала необходимо предопределить выход. Вот фрагмент кода для функции:

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

Функции radar2central и central2radar являются такими же, как в исходном примере, но перемещены из live скрипта в функцию heterogeneousInputsFuser. Вы также добавляете lidar2central и central2lidar выполняет функцию heterogeneousInputsFuser. Эти две функции преобразуются из определения дорожки, которое использует фузер, в определение лидарной дорожки.

Запустите пример в MATLAB

Перед генерацией кода убедитесь, что пример все еще запускается после всех изменений, внесенных в фузер. Файл lidarRadarData.mat содержит тот же сценарий, что и в исходном примере. Он также содержит набор радарных и лидарных дорожек, записанных на каждом шаге этого примера. Вы также используете аналогичное отображение, чтобы визуализировать пример и определить то же trackGOSPAMetric объекты для оценки эффективности отслеживания.

% 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

Сгенерируйте код для Track Fuser

Чтобы сгенерировать код, вы должны задать входные типы для дорожек радара и лидара и временной метки. Как в исходном скрипте, так и в предыдущем разделе, радарные и лидарные дорожки заданы как массивы objectTrack объекты. При генерации кода функция начального уровня не может использовать массив объектов. Вместо этого задается массив структур.

Вы используете struct oneLocalTrack для определения входов, поступающих от радиолокационных и лидарных дорожек. При генерации кода конкретные типы данных каждого поля в struct должны быть определены точно так же, как типы, заданные для соответствующих свойств в записанных дорожках. Кроме того, размер каждого поля должен быть определен правильно. Вы используете coder.typeof (MATLAB Coder) для задания полей с переменным размером: State, StateCovariance, и TrackLogicState. Вы задаете localTracks введите с использованием oneLocalTrack struct и coder.typeof функция, потому что количество входа дорожек изменяется от нуля до восьми на каждом шаге. Вы используете функцию codegen (MATLAB Coder), чтобы сгенерировать код.

Примечания:

  1. Если входные дорожки используют различные типы для State и StateCovariance свойства, вы должны решить, какой тип использовать, двойной или одинарный. В этом примере все дорожки используют двойную точность, и нет необходимости в этом шаге.

  2. Если входные дорожки используют различные определения StateParametersнеобходимо сначала создать супермножество всех StateParameters и используйте этот суперсет в StateParameters поле. Аналогичный процесс должен быть выполнен для ObjectAttributes поле. В этом примере все треки используют одно и то же определение StateParameters и 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;

Запуск примера с сгенерированным кодом

Вы запускаете сгенерированный код, как вы запускали код MATLAB, но сначала необходимо повторно инициализировать сценарий, объекты GOSPA и отображение.

Вы используете toStruct функция объекта для преобразования входных дорожек в массивы структур.

Примечания:

  1. Если входные дорожки используют различные типы данных для State и StateCovariance свойства, убедитесь, что приведено State и StateCovariance всех дорожек с типом данных, выбранным при определении oneLocalTrack структуру выше.

  2. Если входные дорожки требовали структуры супернабора для полей StateParameters или ObjectAttributesубедитесь, что эти структуры заполнены правильно, прежде чем вызывать mex файл.

Вы используете gospaCG переменная, чтобы сохранить метрики GOSPA для этого запуска, чтобы можно было сравнить их со значениями GOSPA из запуска MATLAB.

% 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

В конце запуска необходимо проверить, что сгенерированный код предоставил те же результаты, что и MATLAB код. Используя метрики GOSPA, собранные в обоих запусках, можно сравнить результаты на высоком уровне. Из-за численных округлений могут быть небольшие различия в результатах сгенерированного кода относительно MATLAB код. Чтобы сравнить результаты, вы используете абсолютные различия между значениями GOSPA и проверяете, все ли они меньше 1e-10. Результаты показывают, что различия очень маленькие.

% 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

Сводные данные

В этом примере вы научились генерировать код для алгоритма слияния уровня дорожки, когда входные дорожки неоднородны. Вы научились определять trackFuser и его SourceConfigurations свойство для поддержки неоднородных источников. Вы также научились определять вход во время компиляции и как передать его в файле MEX во время выполнения.

Вспомогательные функции

Следующие функции используются метрикой GOSPA.

helperLidarDistance

Функция для вычисления нормированного расстояния между оценкой дорожки в радиолокационном пространстве состояний и назначенной основной истиной.

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 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
Для просмотра документации необходимо авторизоваться на сайте