В этом примере показано, как отслеживать автотранспортные средства вокруг автомобиля , оборудованного датчиком. Транспортные средства являются расширенными объектами, размерности которых охватывают несколько камеры разрешения датчика. В результате датчики сообщают о нескольких обнаружениях этих объектов в одном скане. В этом примере вы будете использовать различные методы отслеживания расширенных объектов для отслеживания транспортных средств шоссе и оценки результатов их эффективности отслеживания.
В обычных подходах к отслеживанию, таких как глобальный ближайший соседний (multiObjectTracker, trackerGNN), совместная вероятностная ассоциация данных (trackerJPDA) и отслеживание нескольких гипотез (trackerTOMHT), отслеживаемые объекты возвращают по одному обнаружению на скан датчика. С разработкой датчиков, которые имеют лучшее разрешение, таких как радар с высоким разрешением, датчики обычно возвращают более одного обнаружения объекта. Для примера, изображение ниже изображает несколько обнаружений для одного автомобиля, который охватывает несколько камеры разрешения радара. В таких случаях метод, используемый для отслеживания объектов, известен как расширенное отслеживание объектов [1].
Ключевым преимуществом использования датчика высокого разрешения является получение дополнительной информации об объекте, например, о его размерностях и ориентации. Эта дополнительная информация может улучшить вероятность обнаружения и уменьшить частоту ложных предупреждений.
Расширенные объекты представляют новые проблемы для обычных трекеров, потому что эти трекеры предполагают одно обнаружение на каждый объект на датчик. В некоторых случаях можно кластеризировать данные о датчике, чтобы предоставить обычным трекерам одно обнаружение на объект. Однако при этом преимущество использования датчика высокого разрешения может быть утрачено.
Напротив, расширенные трекеры объектов могут обрабатывать несколько обнаружений на объект. Кроме сложения, эти трекеры могут оценивать не только кинематические состояния, такие как положение и скорость объекта, но и размерности и ориентацию объекта. В этом примере вы отслеживаете транспортные средства вокруг автомобиль , оборудованный датчиком с помощью следующих трекеров:
Обычный многообъектный трекер, использующий модель «точка-цель», multiObjectTracker
GGIW-PHD (Гамма-Гауссов Обратный Wishart PHD) трекер, trackerPHD с ggiwphd фильтр
GM-PHD (Гауссова смесь PHD) трекер, trackerPHD с gmphd фильтр с использованием прямоугольной целевой модели
Вы будете оценивать результаты отслеживания всех трекеров, используя trackErrorMetrics и trackAssignmentMetrics, которые обеспечивают множественные меры эффективности трекера. Результаты также будут оценены с помощью Optimal SubPattern Assignment Metric (OSPA), trackOSPAMetric, который направлен на оценку эффективности трекера с помощью комбинированного счета.
Сценарий
В этом примере есть автомобили , оборудованные датчиком и еще четыре транспортных средства: машина перед автомобилем , оборудованным датчиком в центральной полосе, машина за автомобиль , оборудованный датчиком в центральной полосе, грузовик перед автомобилем , оборудованным датчиком в правой полосе и обгон в левой полосе.
В этом примере вы симулируете автомобиль , оборудованный датчиком, которая имеет 6 радарных датчиков и 2 датчика зрения, покрывающих поле зрения 360 градусов. Датчики имеют некоторое перекрытие и некоторую погрешность покрытия. Автомобиль , оборудованный датчиком оснащён радарным датчиком большой дальности и датчиком зрения как на передней, так и на задней стороне транспортного средства. Каждая сторона транспортного средства имеет два радиолокационных датчика ближнего действия, каждый из которых охватывает 90 степени. Один датчик на каждой боковой крышке от середины транспортного средства до задней части. Другой датчик на каждой боковой стороне закрывается с середины транспортного средства вперед.
% Create the scenario exPath = fullfile(matlabroot,'examples','driving_fusion','main'); addpath(exPath) [scenario, egoVehicle, sensors] = helperCreateScenario; % Create the display object display = helperExtendedTargetTrackingDisplay; % Create the Animation writer to record each frame of the figure for % animation writing. Set 'RecordGIF' to true to enable GIF writing. gifWriter = helperGIFWriter('Figure',display.Figure,... 'RecordGIF',false);
Метрики
В этом примере вы используете некоторые ключевые метрики для оценки эффективности отслеживания каждого трекера. В частности, вы оцениваете трекеры на основе их точности в оценке позиций, скоростей, размерностей (длина и ширина) и ориентаций объектов. Эти метрики могут быть оценены с помощью trackErrorMetrics класс. Чтобы задать ошибку отслеживаемой цели из ее основной истины, этот пример использует функцию 'custom' error, helperExtendedTargetError, перечисленный в конце этого примера.
Вы также будете оценивать эффективность на основе таких метрик, как количество ложных треков или избыточных треков. Эти метрики могут быть вычислены с помощью trackAssignmentMetrics класс. Чтобы задать расстояние между отслеживаемой целью и объектом истинности, в этом примере используется функция 'custom' error, helperExtendedTargetDistance, перечисленный в конце этого примера. Функция определяет метрику расстояния как сумму расстояний в положении, скорости, размерности и рыскании.
trackErrorMetrics и trackAssignmentMetrics обеспечение множественных показателей эффективности алгоритма отслеживания. Вы также будете оценивать эффективность на основе Оптимальной метрики назначения подшаблона (OSPA), которая обеспечивает одно значение баллов для алгоритма отслеживания на каждом временном шаге. Эта метрика может быть вычислена с помощью trackOSPAMetric класс. Функция 'custom' distance, заданная для OSPA, аналогична метрикам назначения.
% Function to return the errors given track and truth. errorFcn = @(track,truth)helperExtendedTargetError(track,truth); % Function to return the distance between track and truth distFcn = @(track,truth)helperExtendedTargetDistance(track,truth); % Function to return the IDs from the ground truth. The default % identifier assumes that the truth is identified with PlatformID. In % drivingScenario, truth is identified with an ActorID. truthIdFcn = @(x)[x.ActorID]; % Create metrics object. tem = trackErrorMetrics(... 'ErrorFunctionFormat','custom',... 'EstimationErrorLabels',{'PositionError','VelocityError','DimensionsError','YawError'},... 'EstimationErrorFcn',errorFcn,... 'TruthIdentifierFcn',truthIdFcn); tam = trackAssignmentMetrics(... 'DistanceFunctionFormat','custom',... 'AssignmentDistanceFcn',distFcn,... 'DivergenceDistanceFcn',distFcn,... 'TruthIdentifierFcn',truthIdFcn,... 'AssignmentThreshold',30,... 'DivergenceThreshold',35); % Create ospa metric object tom = trackOSPAMetric(... 'Distance','custom',... 'DistanceFcn',distFcn,... 'TruthIdentifierFcn',truthIdFcn);
The multiObjectTracker Системный object™ принимает по одному обнаружению на объект на датчик и использует глобальный ближайший соседний подход, чтобы связать обнаружения с треками. Он принимает, что каждый объект может быть обнаружен датчиком в скане самое большее один раз. В этом случае моделируемые радарные датчики имеют достаточно высокое разрешение, чтобы генерировать несколько обнаружений на объект. Если эти обнаружения не кластеризованы, трекер генерирует несколько треков на объект. Кластеризация возвращает одно обнаружение на кластер за счет большей ковариации неопределенности и потери информации об истинных размерностях объекта. Кластеризация также затрудняет различение двух объектов, когда они близки друг к другу, например, когда одно транспортное средство пропускает другое транспортное средство.
trackerRunTimes = zeros(0,3); ospaMetric = zeros(0,3); % Create a multiObjectTracker tracker = multiObjectTracker(... 'FilterInitializationFcn', @helperInitPointFilter, ... 'AssignmentThreshold', 30, ... 'ConfirmationThreshold', [4 5], ... 'DeletionThreshold', 3); % Reset the random number generator for repeatable results seed = 2018; S = rng(seed); timeStep = 1; % For multiObjectTracker, the radar reports in Ego Cartesian frame and does % not report velocity. This allows us to cluster detections from multiple % sensors. for i = 1:6 sensors{i}.HasRangeRate = false; sensors{i}.DetectionCoordinates = 'Body'; end
Запуск сценария
while advance(scenario) && ishghandle(display.Figure) % Get the scenario time time = scenario.SimulationTime; % Collect detections from the ego vehicle sensors [detections,isValidTime] = helperDetect(sensors, egoVehicle, time); % Update the tracker if there are new detections if any(isValidTime) % Detections must be clustered first for the point tracker detectionClusters = helperClusterRadarDetections(detections); % Update the tracker tic % confirmedTracks are in scenario coordinates confirmedTracks = updateTracks(tracker, detectionClusters, time); t = toc; % Update the metrics % a. Obtain ground truth groundTruth = scenario.Actors(2:end); % All except Ego % b. Update assignment metrics tam(confirmedTracks,groundTruth); [trackIDs,truthIDs] = currentAssignment(tam); % c. Update error metrics tem(confirmedTracks,trackIDs,groundTruth,truthIDs); % d. Update ospa metric ospaMetric(timeStep,1) = tom(confirmedTracks, groundTruth); % Update bird's-eye-plot % Convert tracks to ego coordinates for display confirmedTracksEgo = helperConvertToEgoCoordinates(egoVehicle, confirmedTracks); display(egoVehicle, sensors, detections, confirmedTracksEgo, detectionClusters); drawnow; % Record tracker run times trackerRunTimes(timeStep,1) = t; timeStep = timeStep + 1; % Capture frames for animation gifWriter(); end end % Capture the cumulative track metrics. The error metrics show the averaged % value of the error over the simulation. assignmentMetricsMOT = tam.trackMetricsTable; errorMetricsMOT = tem.cumulativeTruthMetrics; % Write GIF if requested writeAnimation(gifWriter,'multiObjectTracking');
Эти результаты показывают, что при кластеризации трекер может отслеживать объекты в сцене. Однако это также показывает, что дорожка, связанная с обгоняющим транспортным средством (желтым цветом), перемещается от передней части транспортного средства в начале сценария к задней части транспортного средства в конце. В начале сценария транспортного средства обгона находится позади автомобиля , оборудованного датчиком (синяя), поэтому радар и обнаружения зрения производятся с его передней стороны. Когда машина обгона проходит автомобиль , оборудованный датчиком, радары обнаруживаются со стороны машины обгона, а затем с ее задней стороны, и дорожка перемещается к задней части машины.
Можно также увидеть, что кластеризация не идеальна. Когда проезжающий мимо автомобиль проходит за автомобиль , оборудованный датчиком (фиолетовый), обе трассы слегка смещаются налево из-за несовершенной кластеризации. Резервная дорожка создается на дорожке первоначально из-за нескольких кластеров, созданных при пропуске части бокового ребра. Кроме того, избыточная дорожка появляется на проходящем транспортном средстве во время конца, потому что расстояния между его обнаружениями увеличиваются.
В этом разделе вы используете GGIW-PHD трекер (trackerPHD с ggiwphd) для отслеживания объектов. В отличие от multiObjectTracker, который использует один фильтр на дорожку, GGIW-PHD является мультицелевым фильтром, который описывает плотность гипотезы вероятностей (PHD) сценария. Чтобы смоделировать расширенную цель, GGIW-PHD использует следующие распределения:
Гамма: Положительное значение для описания ожидаемого количества обнаружений.
Гауссов: Вектор состояния для описания кинематического состояния цели
Обратный-Wishart: Положительно-определенная матрица для описания эллиптической степени.
Модель принимает, что каждое распределение независимо друг от друга. Таким образом, плотность гипотезы вероятностей (PHD) в фильтре GGIW-PHD описывается взвешенной суммой функций плотности вероятностей нескольких компонентов GGIW.
PHD-трекер требует вычисления обнаруживаемости каждого компонента в плотности. Вычисление обнаруживаемости требует строений каждого датчика, используемого с трекером. Эти строения определяются для trackerPHD использование trackingSensorConfiguration класс. Проверьте helperCreateSensorConfigurations функция, чтобы увидеть, как свойства датчика могут использоваться, чтобы задать строения датчика для трекера.
% Set up sensor configurations % sensorConfigurations = helperCreateSensorConfigurations(sensors,egoVehicle); % The transform function and filter initialization functions are state and % filter dependent. Therefore, they are not set in the helper function. for i = 1:numel(sensorConfigurations) % You can use a different technique to initialize a filter for each % sensor by using a different function for each configuration. sensorConfigurations{i}.FilterInitializationFcn = @helperInitGGIWFilter; % Tracks are defined in constant turn-rate state-space in the scenario % coordinates. The MeasurementFcn for constant turn-rate model can be % used as the transform function. sensorConfigurations{i}.SensorTransformFcn = @ctmeas; end
Определите трекер.
В отличие от трекера точечных объектов, который обычно учитывает один раздел (кластер) обнаружений, trackerPHD создает несколько возможных разделов набора обнаружений и оценивает его относительно текущих компонентов в PHD-фильтре. Значения 3 и 5 в функции ниже задают нижнее и верхнее расстояние Махаланобиса между обнаружениями. Это эквивалентно определению того, что каждый кластер обнаружений должна быть не менее 3 разрешений отдельно и не более 5 разрешений отдельно друг от друга. Функция helper оборачивается вокруг partitionDetections и не использует измерения уровня области значений для разбиения обнаружений с боковых радаров.
partFcn = @(x)helperPartitioningFcn(x,3,5); tracker = trackerPHD('SensorConfigurations', sensorConfigurations,... 'PartitioningFcn',partFcn,... 'AssignmentThreshold',450,...% Minimum negative log-likelihood of a detection cell (multiple detections per cell) to add birth components. 'ExtractionThreshold',0.75,...% Weight threshold of a filter component to be declared a track 'ConfirmationThreshold',0.85,...% Weight threshold of a filter component to be declared a confirmed track 'MergingThreshold',50,...% Threshold to merge components 'HasSensorConfigurationsInput',true... % Tracking is performed in scenario frame and hence sensor configurations change with time );
Запустите симуляцию.
% Release and restart all objects. restart(scenario); release(tem); release(tam); % No penality for trackerPHD tam.AssignmentThreshold = tam.AssignmentThreshold - 2; release(display); display.PlotClusteredDetection = false; gifWriter.pFrames = {}; for i = 1:numel(sensors) release(sensors{i}); if i <= 6 sensors{i}.HasRangeRate = true; sensors{i}.DetectionCoordinates = 'Sensor spherical'; end end % Restore random seed. rng(seed) % First time step timeStep = 1; % Run the scenario while advance(scenario) && ishghandle(display.Figure) % Get the scenario time time = scenario.SimulationTime; % Get the poses of the other vehicles in ego vehicle coordinates ta = targetPoses(egoVehicle); % Collect detections from the ego vehicle sensors [detections, isValidTime, configurations] = helperDetect(sensors, egoVehicle, time, sensorConfigurations); % Update the tracker with all the detections. Note that there is no % need to cluster the detections before passing them to the tracker. % Also, the sensor configurations are passed as an input to the % tracker. tic % confirmedTracks are in scenario coordinates confirmedTracks = tracker(detections,configurations,time); t = toc; % Update the metrics % a. Obtain ground truth groundTruth = scenario.Actors(2:end); % All except Ego % b. Update assignment metrics tam(confirmedTracks,groundTruth); [trackIDs,truthIDs] = currentAssignment(tam); % c. Update error metrics tem(confirmedTracks,trackIDs,groundTruth,truthIDs); % d. Update ospa metric ospaMetric(timeStep,2) = tom(confirmedTracks, groundTruth); % Update the bird's-eye plot % Convert tracks to ego coordinates for display confirmedTracksEgo = helperConvertToEgoCoordinates(egoVehicle, confirmedTracks); display(egoVehicle, sensors, detections, confirmedTracksEgo); drawnow; % Record tracker run times trackerRunTimes(timeStep,2) = t; timeStep = timeStep + 1; % Capture frames for GIF gifWriter(); end % Capture the truth and track metrics tables assignmentMetricsGGIWPHD = tam.trackMetricsTable; errorMetricsGGIWPHD = tem.cumulativeTruthMetrics; % Write GIF if requested writeAnimation(gifWriter,'ggiwphdTracking');
Эти результаты показывают, что GGIW-PHD может обрабатывать несколько обнаружений на объект на датчик, не требуя сначала кластеризации этих обнаружений. Кроме того, при помощи нескольких обнаружений трекер оценивает положение, скорость, размерности и ориентацию каждого объекта. Штриховая эллиптическая форма на рисунке демонстрирует ожидаемую степень целевого значения. Функция инициализации фильтра задает несколько возможных размеров и их относительных весов с помощью нескольких компонентов. Список может быть расширен, чтобы добавить больше размеров с добавленной вычислительной сложностью. Напротив, можно также инициализировать один компонент на каждое обнаружение с более высокой неопределенностью в размерностях. Это позволит трекеру автоматически оценить размерности объектов. При этом точность оценки будет зависеть от наблюдаемости целевых размерностей и будет подвержена усадке и расширению путевых размерностей при перемещении целей по автомобилю , оборудованному датчиком.
Фильтр GGIW-PHD принимает, что обнаружения распределены вокруг эллиптического центра цели. Поэтому дорожки стремятся следовать наблюдаемым фрагментам транспортного средства. Такие наблюдаемые фрагменты включают заднюю поверхность транспортного средства, которая находится непосредственно перед автомобилем , оборудованным датчиком или передней поверхностью транспортного средства непосредственно позади автомобиля , оборудованного датчиком для примера, заднюю и переднюю поверхность транспортного средства непосредственно перед и позади автомобиля , оборудованного датчиком соответственно. Напротив, длина и ширина проходящего транспортного средства были полностью наблюдены во время симуляции. Поэтому его оцененный эллипс имеет лучшее перекрытие с фактической формой.
Прямоугольный
В этом разделе вы используете GM-PHD трекер (trackerPHD с gmphd) и прямоугольной целевой модели (initctrectgmphd) для отслеживания объектов. В отличие от ggiwphd, который использует эллиптическую форму для отслеживания протяженности, gmphd позволяет вам использовать Гауссово распределение, чтобы задать форму по вашему выбору. Прямоугольная целевая модель определяется моделями движения, ctrect и ctrectjac и модели измерения, ctrectmeas и ctrectmeasjac.
Строения датчика, определенные для trackerPHD ранее, остаются прежними, кроме определения SensorTransformFcn и FilterInitializationFcn.
for i = 1:numel(sensorConfigurations) sensorConfigurations{i}.FilterInitializationFcn = @helperInitRectangularFilter; % Initialize a rectangular target gmphd sensorConfigurations{i}.SensorTransformFcn = @ctrectcorners; % Use corners to calculate detection probability end % Define tracker using new sensor configurations tracker = trackerPHD('SensorConfigurations', sensorConfigurations,... 'PartitioningFcn',partFcn,... 'AssignmentThreshold',600,...% Minimum negative log-likelihood of a detection cell to add birth components 'ExtractionThreshold',0.85,...% Weight threshold of a filter component to be declared a track 'ConfirmationThreshold',0.95,...% Weight threshold of a filter component to be declared a confirmed track 'MergingThreshold',50,...% Threshold to merge components 'HasSensorConfigurationsInput',true... % Tracking is performed in scenario frame and hence sensor configurations change with time ); % Release and restart all objects. restart(scenario); for i = 1:numel(sensors) release(sensors{i}); end release(tem); release(tam); release(display); display.PlotClusteredDetection = false; gifWriter.pFrames = {}; % Restore random seed. rng(seed) % First time step timeStep = 1; % Run the scenario while advance(scenario) && ishghandle(display.Figure) % Get the scenario time time = scenario.SimulationTime; % Get the poses of the other vehicles in ego vehicle coordinates ta = targetPoses(egoVehicle); % Collect detections from the ego vehicle sensors [detections, isValidTime, configurations] = helperDetect(sensors, egoVehicle, time, sensorConfigurations); % Update the tracker with all the detections. Note that there is no % need to cluster the detections before passing them to the tracker. % Also, the sensor configurations are passed as an input to the % tracker. tic % confirmedTracks are in scenario coordinates confirmedTracks = tracker(detections,configurations,time); t = toc; % Update the metrics % a. Obtain ground truth groundTruth = scenario.Actors(2:end); % All except Ego % b. Update assignment metrics tam(confirmedTracks,groundTruth); [trackIDs,truthIDs] = currentAssignment(tam); % c. Update error metrics tem(confirmedTracks,trackIDs,groundTruth,truthIDs); % d. Update ospa metric ospaMetric(timeStep,3) = tom(confirmedTracks, groundTruth); % Update the bird's-eye plot % Convert tracks to ego coordinates for display confirmedTracksEgo = helperConvertToEgoCoordinates(egoVehicle, confirmedTracks); display(egoVehicle, sensors, detections, confirmedTracksEgo); drawnow; % Record tracker run times trackerRunTimes(timeStep,3) = t; timeStep = timeStep + 1; % Capture frames for GIF gifWriter(); end % Capture the truth and track metrics tables assignmentMetricsGMPHD = tam.trackMetricsTable; errorMetricsGMPHD = tem.cumulativeTruthMetrics; % Write GIF if requested writeAnimation(gifWriter,'gmphdTracking'); % Return the random number generator to its previous state rng(S) rmpath(exPath)
Эти результаты показывают, что GM-PHD также может обрабатывать несколько обнаружений на объект на датчик. Подобно GGIW-PHD, он также оценивает размер и ориентацию объекта. Функция инициализации фильтра использует такой же подход как и GGIW-PHD трекер и инициализирует несколько компонентов для разных размеров.
Можно заметить, что предполагаемые дорожки, которые моделируются как прямоугольники, хорошо соответствуют моделируемому объекту основная истина, изображенному сплошными цветовыми закрашенными фигурами. В частности, пути способны правильно отслеживать форму транспортного средства вместе с кинематическим центром.
Оцените эффективность отслеживания каждого трекера, используя количественные метрики, такие как ошибка расчета в положении, скорости, размерностях и ориентации. Также оцените назначения дорожек, используя такие метрики, как избыточные и ложные дорожки.
Метрики назначения
helperPlotAssignmentMetrics(assignmentMetricsMOT, assignmentMetricsGGIWPHD, assignmentMetricsGMPHD);
Метрики назначения иллюстрируют, что избыточные и ложные дорожки были инициализированы и подтверждены трекером объектов точек. Эти треки происходят из-за несовершенной кластеризации, где обнаружения, принадлежащие одной цели, были кластеризованы в несколько кластеризованных обнаружений. В отличие от этого, GGIW-PHD трекер и GM-PHD трекер поддерживают треки на всех четырех мишенях и не создают никаких ложных или избыточных треков. Эти метрики показывают, что оба расширенных трекера объектов правильно разделяют обнаружения и связывают их с правильными треками.
Метрики ошибок
helperPlotErrorMetrics(errorMetricsMOT, errorMetricsGGIWPHD, errorMetricsGMPHD);
График показывает средние ошибки расчета для трех типов трекеров, используемых в этом примере. Поскольку трекер точки объектов не оценивает рыскание и размерности объектов, они теперь показаны на графиках. Трекер точечных объектов способен оценить кинематику объектов с разумной точностью. Ошибка положения автомобиля за автомобиль , оборудованный датчиком выше, потому что он был перетащен налево, когда проезжающее транспортное средство обогнало это транспортное средство. Это также программный продукт несовершенной кластеризации, когда объекты близки друг к другу.
Как описано ранее, трекер GGIW-PHD принимает, что измерения распределены вокруг области объекта, что приводит к центру дорожек на наблюдаемых частях транспортного средства. Это также можно увидеть в метриках ошибки положения для TruthID 2 и 4. Трекер способен оценить размерности объекта с точностью около 0,3 метра для транспортных средств впереди и позади автомобиля , оборудованного датчиком. Из-за большей уверенности, определенной для размерностей транспортных средств в helperInitGGIWFilter функция, трекер не свернет длину этих транспортных средств, даже когда наиболее подходящий эллипс имеет очень низкую длину. Поскольку проходящее мимо транспортное средство (TruthID 3) наблюдалось на всех размерностях, его размерности измеряются более точно, чем другие транспортные средства. Однако, когда проходящее мимо транспортное средство маневрирует относительно автомобиля , оборудованного датчиком, ошибка в оценке рыскания выше.
GM-PHD в этом примере использует прямоугольную форму целевой модели и использует полученные измерения, чтобы оценить ожидаемые измерения на контуре цели. Эта модель помогает трекеру более точно оценить форму и ориентацию. Однако процесс оценки ожидаемых измерений на ребрах прямоугольной цели является в вычислительном отношении более дорогим.
Метрика OSPA
Как описано ранее, метрика OSPA предназначена для описания эффективности алгоритма отслеживания с использованием одного счета. Заметьте, что OSPA достаточно захватывает эффективность алгоритма отслеживания, который уменьшается с GM-PHD до GGIW-PHD на трекер точки-цели, как описано с помощью метрик ошибки и назначения.
ospaFig = figure; plot(ospaMetric,'LineWidth',2); legend('Point Target Tracker','GGIW-PHD Tracker','Rectangular GM-PHD Tracker'); xlabel('Time step (k)'); ylabel('OSPA');
Ранее вы узнали о различных методах, допущениях, которые они делают о целевых моделях, и полученной эффективности слежения. Теперь сравните время выполнения трекеров. Заметьте, что фильтр GGIW-PHD предлагает значительные вычислительные преимущества по сравнению с GM-PHD за счет снижения эффективности отслеживания.
runTimeFig = figure; h = plot(trackerRunTimes(3:end,:)./trackerRunTimes(3:end,1),'LineWidth',2); legend('Point Target Tracker','GGIW-PHD Tracker','Rectangular GM-PHD Tracker'); xlabel('Time step (k)'); ylabel('$$\frac{t_{tracker}}{t_{multiObjectTracker}}$$','interpreter','latex','fontsize',14); ylim([0 max([h.YData]) + 1]);
Этот пример показал, как отслеживать объекты, которые возвращают несколько обнаружений в одном скане датчика с помощью различных подходов. Эти подходы могут использоваться для отслеживания объектов с датчиками высокого разрешения, такими как радар или лазерный датчик.
[1] Гранстрём, Карл, Маркус Баум и Стефан Ройтер. «Расширенное отслеживание объектов: введение, обзор и приложения». Журнал Усовершенствований в Information Fusion. Том 12, № 2, декабрь 2017.
[2] Гранстрём, Карл, Кристиан Лундквист и Умут Оргунер. «Слежение за прямоугольными и эллиптическими расширенными целями с помощью лазерных измерений». 14-я Международная конференция по информационному слиянию. IEEE, 2011.
[3] Гранстрём, Карл. «Расширенное отслеживание целей с использованием PHD-фильтров». 2012
helperExtendedTargetError
Функция для определения ошибки между отслеживаемой целью и связанной основной истиной.
function [posError,velError,dimError,yawError] = helperExtendedTargetError(track,truth) % Errors as a function of target track and associated truth. % Get true information from the ground truth. truePos = truth.Position(1:2)'; % Position is at the rear axle for all vehicles. We would like to compute % the error from the center of the vehicle rot = [cosd(truth.Yaw) -sind(truth.Yaw);sind(truth.Yaw) cosd(truth.Yaw)]; truePos = truePos + rot*[truth.Wheelbase/2;0]; trueVel = truth.Velocity(1:2); trueYaw = truth.Yaw(:); trueDims = [truth.Length;truth.Width]; % Get estimated value from track. % GGIW-PHD tracker outputs a struct field 'Extent' and 'SourceIndex' % GM-PHD tracker outputs struct with but not 'Extent' % multiObjectTracker outputs objectTrack if isa(track,'objectTrack') estPos = track.State([1 3]); estVel = track.State([2 4]); % No yaw or dimension information in multiObjectTracker. estYaw = nan; estDims = [nan;nan]; elseif isfield(track,'Extent') % trackerPHD with GGIWPHD estPos = track.State([1 3]); estVel = track.State([2 4]); estYaw = atan2d(estVel(2),estVel(1)); d = eig(track.Extent); dims = 2*sqrt(d); estDims = [max(dims);min(dims)]; else % trackerPHD with GMPHD estPos = track.State(1:2); estYaw = track.State(4); estVel = [track.State(3)*cosd(estYaw);track.State(3)*sind(estYaw)]; estDims = track.State(6:7); end % Compute 2-norm of error for each attribute. posError = norm(truePos(:) - estPos(:)); velError = norm(trueVel(:) - estVel(:)); dimError = norm(trueDims(:) - estDims(:)); yawError = norm(trueYaw(:) - estYaw(:)); end
helperExtendedTargetDistance
Функция для определения расстояния между дорожкой и основной истиной.
function dist = helperExtendedTargetDistance(track,truth) % This function computes the distance between track and a truth. % Copyright 2019-2020 The MathWorks, Inc. % Errors in each aspect [posError,velError,dimError,yawError] = helperExtendedTargetError(track,truth); % For multiObjectTracker, add a constant penalty for not estimating yaw % and dimensions if isnan(dimError) dimError = 1; end if isnan(yawError) yawError = 1; end % Distance is the sum of errors dist = posError + velError + dimError + yawError; end
helperInitGGIWFilter
Функция для создания фильтра ggiwphd из камеры обнаружения.
function phd = helperInitGGIWFilter(varargin) % helperInitGGIWFilter A function to initialize the GGIW-PHD filter for the % Extended Object Tracking example % Create a ggiwphd filter using 5 states and the constant turn-rate models. phd = ggiwphd(zeros(5,1),eye(5),... 'StateTransitionFcn',@constturn,... 'StateTransitionJacobianFcn',@constturnjac,... 'MeasurementFcn',@ctmeas,... 'MeasurementJacobianFcn',@ctmeasjac,... 'HasAdditiveMeasurementNoise',true,... 'HasAdditiveProcessNoise',false,... 'ProcessNoise',diag([1 1 3]),... 'MaxNumComponents',1000,... 'ExtentRotationFcn',@extentRotFcn,... 'PositionIndex',[1 3]); % If the function is called with no inputs i.e. the predictive portion of % the birth density, no components are added to the mixture. if nargin == 0 % Nullify to return 0 components. nullify(phd); else % When called with detections input, add two components to the filter, % one for car and one for truck, More components can be added based on % prior knowledge of the scenario, example, pedestrian or motorcycle. % This is a "multi-model" type approach. Another approach can be to add % only 1 component with a higher covariance in the dimensions. The % later is computationally less demanding, but has a tendency to track % observable dimensions of the object. For example, if only the back is % visible, the measurement noise may cause the length of the object to % shrink. % Detections detections = varargin{1}; % Enable elevation measurements to create a 3-D filter using % initctggiwphd if detections{1}.SensorIndex < 7 for i = 1:numel(detections) detections{i}.Measurement = [detections{i}.Measurement(1);0;detections{i}.Measurement(2:end)]; detections{i}.MeasurementNoise = blkdiag(detections{i}.MeasurementNoise(1,1),0.4,detections{i}.MeasurementNoise(2:end,2:end)); detections{i}.MeasurementParameters(1).HasElevation = true; end end phd3d = initctggiwphd(detections); % Set states of the 2-D filter using 3-D filter phd.States = phd3d.States(1:5); phd.StateCovariances = phd3d.StateCovariances(1:5,1:5); phd.DegreesOfFreedom = 1000; phd.ScaleMatrices = (1000-4)*diag([4.7/2 1.8/2].^2); % Add truck dimensions as second component append(phd,phd); phd.ScaleMatrices(:,:,2) = (1000-4)*diag([8.1/2 2.45/2].^2); phd.GammaForgettingFactors = [1.03 1.03]; % Relative weights of the components. Can be treated as probability of % existence of a car vs a truck on road. phd.Weights = [0.7 0.3]; end end function R = extentRotFcn(x,dT) % Rotation of the extent during prediction. w = x(5); theta = w*dT; R = [cosd(theta) -sind(theta);sind(theta) cosd(theta)]; end
helperInitRectangularFilter
Функция для создания прямоугольного целевого фильтра gmphd из камеры обнаружения
function filter = helperInitRectangularFilter(varargin) % helperInitRectangularFilter A function to initialize the rectangular % target PHD filter for the Extended Object Tracking example % Copyright 2019 The MathWorks, Inc. if nargin == 0 % If called with no inputs, simply use the initctrectgmphd function to % create a PHD filter with no components. filter = initctrectgmphd; % Set process noise filter.ProcessNoise = diag([1 3]); else % When called with detections input, add two components to the filter, % one for car and one for truck, More components can be added based on % prior knowledge of the scenario, example, pedestrian or motorcycle. % This is a "multi-model" type approach. Another approach can be to add % only 1 component with a higher covariance in the dimensions. The % later is computationally less demanding, but has a tendency to track % observable dimensions of the object. For example, if only the back is % visible, the measurement noise may cause the length of the object to % shrink. % Detections detections = varargin{1}; % Create a GM-PHD filter with rectangular model filter = initctrectgmphd(detections); % Length width of a passenger car filter.States(6:7,1) = [4.7;1.8]; % High certainty in dimensions lCov = 1e-4; wCov = 1e-4; lwCorr = 0.5; lwCov = sqrt(lCov*wCov)*lwCorr; filter.StateCovariances(6:7,6:7,1) = [lCov lwCov;lwCov wCov]; % Add one more component by appending the filter with itself. append(filter,filter); % Set length and width to a truck dimensions filter.States(6:7,2) = [8.1;2.45]; % Relative weights of each component filter.Weights = [0.7 0.3]; end end
