В этом примере показано, как создать код для алгоритма слияния на уровне дорожки в сценарии, где дорожки происходят из разнородных источников с различными определениями состояний. Этот пример основан на примере Track-Level Fusion of Radar и Lidar Data (Sensor Fusion and Tracking Toolbox), в котором пространства состояний дорожек, генерируемых из лидарных и радиолокационных источников, различны.
Можно создать код для trackFuser(Панель инструментов слияния и отслеживания датчиков) с помощью Coder™ MATLAB ®. Для этого необходимо изменить код в соответствии со следующими ограничениями:
Функция ввода генерации кода
Следуйте инструкциям по использованию системных объектов при создании кода MATLAB (кодер MATLAB). Для создания кода сначала необходимо определить функцию начального уровня, в которой определен объект. Кроме того, функция не может использовать массивы объектов в качестве входных или выходных данных. В этом примере функция начального уровня определяется как гетерогенная функция Fuser. Функция должна находиться в пути при создании кода для нее. Поэтому он не может быть частью этого сценария в реальном времени и прилагается в этом примере. Функция принимает локальные дорожки и текущее время в качестве входных и выходных центральных дорожек.
Чтобы сохранить состояние fuser между вызовами функции, вы определяете fuser как 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
Однородные конфигурации источника
В этом примере конфигурации радара и источника лидара определяются иначе, чем в оригинальном примере объединения радаров и данных лидара (Sensor Fusion and Tracking Toolbox). В исходном примере CentralToLocalTransformFcn и LocalToCentralTransformFcn свойства двух конфигураций источника различны, поскольку они используют разные дескрипторы функций. Это делает исходные конфигурации гетерогенным массивом ячеек. Такое определение является правильным и действительным при выполнении в MATLAB. Однако при создании кода во всех конфигурациях источника должны использоваться одни и те же дескрипторы функций. Чтобы избежать различных дескрипторов функций, необходимо определить одну функцию для преобразования дорожек из центрального (fuser) определения в локальное (source) определение и одну функцию для преобразования из локального в центральное. Каждая из этих функций переключается между функциями преобразования, определенными для отдельных источников в исходном примере. Обе функции являются частью гетерогенной функции Fuser.
Вот код для 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 те же, что и в исходном примере, но перемещены из реального сценария в гетерогенную функцию FuserFuser. Вы также добавляете lidar2central и central2lidar функции к гетерогенной функции Fuser. Эти две функции преобразуются из определения дорожки, используемого фьюзером, в определение дорожки лидара.
Перед созданием кода убедитесь, что пример по-прежнему выполняется после всех изменений, внесенных в fuser. Файл 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(Панель инструментов слияния и отслеживания датчиков). При создании кода функция начального уровня не может использовать массив объектов. Вместо этого определяется массив структур.
Используется структура oneLocalTrack определить входы, поступающие с радиолокационных и лидарных путей. При создании кода конкретные типы данных каждого поля в структуре должны быть определены точно так же, как типы, определенные для соответствующих свойств в записанных дорожках. Кроме того, размер каждого поля должен быть определен правильно. Вы используете coder.typeof (Кодер MATLAB) для указания полей с переменным размером: State, StateCovariance, и TrackLogicState. Вы определяете localTracks ввод с помощью oneLocalTrack структура и coder.typeof функция, поскольку количество входных дорожек изменяется от нуля до восьми на каждом шаге. Функция используется codegen (Кодер MATLAB) для создания кода.
Примечания:
Если входные дорожки используют различные типы для 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(Панель инструментов слияния и отслеживания датчиков)