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

Этот пример использует:

В этом примере показано, как сгенерировать код для алгоритма сплава уровня дорожки в сценарии, где дорожки происходят из неоднородных источников с различными определениями состояния. Этот пример основан на Fusion Уровня Дорожки примера Данных о Радаре и Лидаре, в котором пространства состояний дорожек, сгенерированных из источников лидара и радара, отличаются.

Задайте термофиксатор дорожки для генерации кода

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

Функция записи генерации кода

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

Чтобы сохранить состояние термофиксатора между вызовами функции, вы задаете термофиксатор как persistent переменная. На первом вызове необходимо задать переменную термофиксатора, потому что это пусто. Остальная часть выполняющих шагов кода 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

Гомогенные исходные настройки

В этом примере вы задаете радар и лоцируете исходные настройки по-другому, чем в исходном Fusion Уровня Дорожки примера Данных о Радаре и Лидаре. В исходном примере, CentralToLocalTransformFcn и LocalToCentralTransformFcn свойства двух исходных настроек отличаются, потому что они используют различные указатели на функцию. Это делает исходные настройки неоднородным массивом ячеек. Такое определение правильно и допустимо при выполнении в MATLAB. Однако в генерации кода, все исходные настройки должны использовать те же указатели на функцию. Чтобы избежать различных указателей на функцию, вы задаете одну функцию, чтобы преобразовать дорожки от центрального (термофиксатор) определение локальному (источник) определение и одна функция, чтобы преобразовать от локального до центрального. Каждая из этих функций переключается между функциями, определяемыми преобразования для отдельных источников в исходном примере. Обе функции являются частью функции 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

Сгенерируйте код для термофиксатора дорожки

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

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

Примечания:

Если входные дорожки используют различные типы для State и StateCovariance свойства, необходимо решить который тип использовать, дважды или один. В этом примере все дорожки используют двойную точность и нет никакой потребности в этом шаге.
Если входные дорожки используют различные определения 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 возразите функции, чтобы преобразовать входные дорожки в массивы структур.

Примечания:

Если входные дорожки используют различные типы данных для State и StateCovariance свойства, убедитесь, что бросили State и StateCovariance из всех дорожек к типу данных вы выбрали, когда вы задали oneLocalTrack структура выше.
Если входные дорожки потребовали структуры надмножества для полей 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

Документация