В этом примере показано, как сгенерировать код для алгоритма сплава уровня дорожки в сценарии, где дорожки происходят из неоднородных источников с различными определениями состояния. Этот пример основан на Fusion Уровня Дорожки Данных о Радаре и Лидаре (Sensor Fusion and Tracking Toolbox) пример, в котором пространства состояний дорожек, сгенерированных из источников лидара и радара, отличаются.
Можно сгенерировать код для trackFuser (Sensor Fusion and Tracking Toolbox) с помощью 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 Уровня Дорожки Данных о Радаре и Лидаре (Sensor Fusion and Tracking Toolbox) пример. В исходном примере, 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. Эти две функции преобразуют из определения дорожки, которое термофиксатор использует для определения дорожки лидара.
Прежде, чем сгенерировать код, убедитесь, что пример все еще выполняется после всех изменений, внесенных в термофиксатор. Файл 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 (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)
