Сгенерируйте код для трека Fuser с гетерогенными исходными треками
Этот пример показывает, как сгенерировать код для алгоритма слияния уровня дорожки в сценарии, где дорожки происходят из гетерогенных источников с различными определениями состояний. Этот пример основан на примере Track-Level Fusion of Radar and Лидара Данных (Sensor Fusion and Tracking Toolbox), в котором пространства состояний треков, сгенерированных из лидара и радиолокационных источников, отличаются.
Определите Track Fuser для генерации кода
Можно сгенерировать код для trackFuser (Sensor Fusion and Tracking Toolbox) с использованием Coder™ MATLAB ®. Для этого необходимо изменить код в соответствии со следующими ограничениями:
Функция ввода генерации кода
Следуйте инструкциям по использованию системных объектов в генерации кода 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 (Sensor Fusion and Tracking Toolbox). В исходном примере 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 (Sensor Fusion and Tracking Toolbox) объекты. При генерации кода функция начального уровня не может использовать массив объектов. Вместо этого задается массив структур.
Вы используете 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 (Sensor Fusion and Tracking Toolbox) функция объекта для преобразования входных дорожек в массивы структур.
Примечания:
Если входные дорожки используют различные типы данных для
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
См. также
trackFuser (Sensor Fusion and Tracking Toolbox)