Setup для генерации синтетических данных

Сценарий, используемый в этом примере, создается с помощью drivingScenario. Данные с радара и датчиков лидара моделируются с помощью drivingRadarDataGenerator и lidarPointCloudGenerator, соответственно. Создание сценария и моделей датчиков заворачивается в функцию helper helperCreateRadarLidarScenario. Для получения дополнительной информации о сценарии и генерации синтетических данных см. раздел «Программное создание сценария вождения».

% For reproducible results
rng(2019);

% Create scenario, ego vehicle and get radars and lidar sensor
[scenario, egoVehicle, radars, lidar] = helperCreateRadarLidarScenario;

На Автомобиль , оборудованный датчиком установлены четыре 2-D радиолокационных датчика. Передний и задний радарные датчики имеют поле зрения 45 степеней. Левый и правый радарные датчики имеют поле зрения 150 степеней. Каждый радар имеет разрешение 6 степеней по азимуту и 2,5 метра по области значений. Ego также монтируется с одним 3-D датчиком лидара с полем зрения 360 степеней по азимуту и 40 степени по повышению. Лидар имеет разрешение 0,2 степеней в азимуте и 1,25 степени в повышение (32 повышения канала). Визуализируйте строение датчиков и моделируемые данные о датчике в анимации ниже. Заметьте, что радары имеют более высокое разрешение, чем объекты, и, следовательно, возвращают несколько измерений на объект. Также заметьте, что лидар взаимодействует с низкополюсным mesh актёра, а также с поверхностью дороги, чтобы вернуть несколько точек от этих объектов.

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

Как упоминалось, радары имеют более высокое разрешение, чем объекты, и возвращают несколько обнаружений на объект. Обычные трекеры, такие как Global Nearest Neighbor (GNN) и Joint Probabilistic Data Association (JPDA), предполагают, что датчики возвращают самое большее одно обнаружение на объект за скан. Поэтому обнаружение с датчиков высокого разрешения должно быть либо кластеризовано перед обработкой его обычными трекерами, либо должно быть обработано с помощью расширенных трекеров объектов. Расширенные трекеры объектов не требуют предварительной кластеризации обнаружений и обычно оценивают как кинематические состояния (для примера, положения и скорости), так и степень объектов. Для более подробного сравнения между обычными трекерами и расширенными трекерами объектов смотрите пример Расширенного Отслеживания Объектов Дорожных Транспортных Средств с Радаром и Камерой (Sensor Fusion and Tracking Toolbox).

В целом, расширенные трекеры объектов предлагают лучшую оценку объектов, так как они обрабатывают кластеризацию и ассоциацию данных одновременно с использованием временной истории треков. В этом примере радиолокационные обнаружения обрабатываются с помощью Гауссова гипотезы о вероятности (GM-PHD) трекера (trackerPHD (Sensor Fusion and Tracking Toolbox) и gmphd (Sensor Fusion and Tracking Toolbox)) с прямоугольной целевой моделью. Для получения дополнительной информации о настройке трекера см. раздел «GM-PHD Rectangular Object Tracker» примера Расширенного отслеживания объектов Транспортных средств шоссе с радаром и камерой (Sensor Fusion and Tracking Toolbox).

Алгоритм отслеживания объектов с помощью радиолокационных измерений заворачивается внутрь класса helper, helperRadarTrackingAlgorithm, реализованный как Системная object™. Этот класс выводит массив objectTrack (Sensor Fusion and Tracking Toolbox) объекты и определить их состояние согласно следующему соглашению:

radarTrackingAlgorithm = helperRadarTrackingAlgorithm(radars);

Алгоритм отслеживания Лидара

Подобно радарам, датчик лидара также возвращает несколько измерений на объект. Кроме того, датчик возвращает большое число точек с дороги, которую необходимо удалить, прежде чем использовать в качестве входов для алгоритма отслеживания объектов. В то время как лидарные данные из препятствий могут быть непосредственно обработаны с помощью расширенного алгоритма отслеживания объектов, обычные алгоритмы отслеживания все еще более распространены для отслеживания с использованием лидарных данных. Первая причина такого тренда в основном наблюдается из-за большей вычислительной сложности трекеров расширенного объекта для больших наборов данных. Второй причиной являются инвестиции в расширенные детекторы на основе глубокого обучения, такие как PointPillars [1], VoxelNet [2] и PIXOR [3], которые могут сегментировать облако точек и возвращать обнаружение ограничивающих прямоугольников для транспортных средств. Эти детекторы могут помочь в преодолении снижения эффективности обычных трекеров из-за неподходящей кластеризации.

В этом примере данные лидара обрабатываются с помощью обычного объединенного вероятностного трекера (JPDA), сконфигурированного с взаимодействующим фильтром с несколькими моделями (IMM). Предварительная обработка лидарных данных для удаления облака точек выполняется с помощью основанного на RANSAC алгоритма плоской подгонки, и ограничительные рамки формируются путем выполнения основанного на Евклиде алгоритма дистанционной кластеризации. Для получения дополнительной информации об алгоритме смотрите пример Track Vehicles Using Lidar: From Point Cloud to Track List (Sensor Fusion and Tracking Toolbox). По сравнению со связанным примером отслеживание выполняется в системе координат сценария, и трекер настраивается по-разному, чтобы отслеживать объекты разных размеров. Далее состояния переменных заданы по-разному, чтобы ограничить движение дорожек в направлении ее расчетного угла рыскания.

Алгоритм отслеживания объектов, используя данные лидара, заворачивается внутрь класса helper, helperLidarTrackingAlgorithm реализован как Системный объект. Этот класс выводит массив objectTrack (Sensor Fusion and Tracking Toolbox) возражает и определяет их состояние согласно следующему соглашению:

Общие для радиолокационного алгоритма состояния заданы аналогично. Кроме того, как датчик 3-D, трекер лидара выводит три дополнительных состояния, и, которые относятся к z-координате (m), z-скорости (m/s) и высоте (m) отслеживаемого объекта соответственно.

lidarTrackingAlgorithm = helperLidarTrackingAlgorithm(lidar);

Настройте Fuser, метрики и визуализацию

Fuser

Далее вы настроите алгоритм слияния для сплавления списка треков от радаров и лидарных трекеров. Подобно другим алгоритмам отслеживания, первым шагом к настройке алгоритма слияния на уровне дорожки является определение выбора вектора состояния (или пространства состояний) для слитых или центральных дорожек. В этом случае пространство состояний для слитых дорожек выбирается таким же, как и лидар. После выбора центрального пространства треков, вы задаете преобразование центрального состояния треков в локальное состояние треков. В этом случае местное пространство состояния дорожки относится к состояниям радиолокационных и лидарных дорожек. Для этого вы используете fuserSourceConfiguration (Sensor Fusion and Tracking Toolbox) объект.

Определите строение источника радара. The helperRadarTrackingAlgorithm выводит дорожки с SourceIndex установите значение 1. The SourceIndex предоставляется как свойство на каждом трекере, чтобы уникально идентифицировать его и позволяет алгоритму слияния различать дорожки из различных источников. Поэтому вы устанавливаете SourceIndex свойство строения радара аналогично свойствам радарных дорожек. Вы задаете IsInitializingCentralTracks на true чтобы позволить неназначенным радиолокационным трекам инициировать новые центральные трекы. Затем вы задаете преобразование дорожки в центральном пространстве состояний в радиолокационное пространство состояний и наоборот. Вспомогательная функция central2radar и radar2central выполните эти два преобразования и включены в конец этого примера.

radarConfig = fuserSourceConfiguration('SourceIndex',1,...
    'IsInitializingCentralTracks',true,...
    'CentralToLocalTransformFcn',@central2radar,...
    'LocalToCentralTransformFcn',@radar2central);

Определите строение источника лидара. Поскольку пространство состояний лидарной дорожки совпадает с центральным дорожкой, вы не задаете никаких преобразований.

lidarConfig = fuserSourceConfiguration('SourceIndex',2,...
    'IsInitializingCentralTracks',true);

Следующим шагом является определение алгоритма слияния состояний. Алгоритм слияния состояний принимает несколько состояний и ковариаций состояний в центральном пространстве состояний в качестве входных данных и возвращает слитую оценку состояния и ковариаций. В этом примере вы используете ковариацию пересечения, предоставленный функцией helper, helperRadarLidarFusionFcn. Типовой алгоритм ковариационного пересечения для двух Гауссовых оценок со средним и ковариационным состояниями может быть определен согласно следующим уравнениям:

где и являются слитым состоянием и ковариацией и и смешивают коэффициенты из каждой оценки. Обычно эти коэффициенты смешения оценивают путем минимизации определяющего или следа слитой ковариации. В этом примере веса смешения оцениваются путем минимизации определяющего позиционной ковариации каждой оценки. Кроме того, поскольку радар не оценивает 3-D состояния, 3-D состояния объединяются только с лидарами. Для получения дополнительной информации см. helperRadarLidarFusionFcn функция, показанная в конце этого скрипта.

Далее вы собираете всю информацию с помощью trackFuser объект.

% The state-space of central tracks is same as the tracks from the lidar,
% therefore you use the same state transition function. The function is
% defined inside the helperLidarTrackingAlgorithm class.
f = lidarTrackingAlgorithm.StateTransitionFcn;

% Create a trackFuser object
fuser = trackFuser('SourceConfigurations',{radarConfig;lidarConfig},...
    'StateTransitionFcn',f,...
    'ProcessNoise',diag([1 3 1]),...
    'HasAdditiveProcessNoise',false,...
    'AssignmentThreshold',[250 inf],...
    'ConfirmationThreshold',[3 5],...
    'DeletionThreshold',[5 5],...
    'StateFusion','Custom',...
    'CustomStateFusionFcn',@helperRadarLidarFusionFcn);

Метрики

В этом примере вы оцениваете эффективность каждого алгоритма, используя Обобщенную метрику Оптимального Назначения Подшаблона (GOSPA). Вы настраиваете три отдельные метрики, используя trackGOSPAMetric (Sensor Fusion and Tracking Toolbox) для каждого из трекеров. Целью GOSPA является оценка эффективности системы отслеживания путем предоставления скаляра затрат. Меньшее значение метрики указывает на лучшую эффективность алгоритма отслеживания.

Чтобы использовать метрику GOSPA с пользовательскими моделями движения, такими как используемая в этом примере, вы устанавливаете Distance свойство to 'custom' и задайте функцию расстояния между дорожкой и связанной с ней основной истиной. Эти функции расстояния, показанные в конце этого примера helperRadarDistance, и helperLidarDistance.

% 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);

Визуализация

Визуализация для этого примера реализована с использованием класса helper helperLidarRadarTrackFusionDisplay. Отображение разделено на 4 панели. Это отображение строит графики измерений и дорожек с каждого датчика, а также оценки слитых дорожек. Легенда для отображения показана ниже. Кроме того, дорожки аннотируются их уникальными тождествами (TrackID) а также префикс. Префиксы «R», «L» и «F» обозначают радар, лидар и сплавленную оценку, соответственно.

% Create a display.
% FollowActorID controls the actor shown in the close-up
% display
display = helperLidarRadarTrackFusionDisplay('FollowActorID',3);

% Show persistent legend
showLegend(display,scenario);

Запуск сценария и трекеров

Затем вы продвигаете сценарий, генерируете синтетические данные со всех датчиков и обрабатываете их, чтобы сгенерировать дорожки от каждой из систем. Вы также вычисляете метрику для каждого трекера, используя основную истину, доступную из сценария.

% Initialzie GOSPA metric and its components for all tracking algorithms.
gospa = zeros(3,0);
missTarget = zeros(3,0);
falseTracks = zeros(3,0);

% Initialize fusedTracks
fusedTracks = objectTrack.empty(0,1);

% A counter for time steps elapsed for storing gospa metrics.
idx = 1;

% Ground truth for metrics. This variable updates every time-step
% automatically being a handle to the actors.
groundTruth = scenario.Actors(2:end);

while advance(scenario)
    % Current time
    time = scenario.SimulationTime;

    % Collect radar and lidar measurements and ego pose to track in
    % scenario frame. See helperCollectSensorData below.
    [radarDetections, ptCloud, egoPose] = helperCollectSensorData(egoVehicle, radars, lidar, time);

    % Generate radar tracks
    radarTracks = radarTrackingAlgorithm(egoPose, radarDetections, time);

    % Generate lidar tracks and analysis information like bounding box
    % detections and point cloud segmentation information
    [lidarTracks, lidarDetections, segmentationInfo] = ...
        lidarTrackingAlgorithm(egoPose, ptCloud, time);

    % Concatenate radar and lidar tracks
    localTracks = [radarTracks;lidarTracks];

    % Update the fuser. First call must contain one local track
    if ~(isempty(localTracks) && ~isLocked(fuser))
        fusedTracks = fuser(localTracks,time);
    end

    % Capture GOSPA and its components for all trackers
    [gospa(1,idx),~,~,~,missTarget(1,idx),falseTracks(1,idx)] = gospaRadar(radarTracks, groundTruth);
    [gospa(2,idx),~,~,~,missTarget(2,idx),falseTracks(2,idx)] = gospaLidar(lidarTracks, groundTruth);
    [gospa(3,idx),~,~,~,missTarget(3,idx),falseTracks(3,idx)] = gospaCentral(fusedTracks, groundTruth);

    % Update the display
    display(scenario, radars, radarDetections, radarTracks, ...
        lidar, ptCloud, lidarDetections, segmentationInfo, lidarTracks,...
        fusedTracks);

    % Update the index for storing GOSPA metrics
    idx = idx + 1;
end

% Update example animations
updateExampleAnimations(display);

Оценка эффективности

Оцените эффективность каждого трекера, используя визуализацию, а также количественные метрики. Анализ различных событий в сценарии и понимание того, как схема слияния на уровне дорожки помогает достичь лучшей оценки состояния транспортного средства.

Отслеживайте техническое обслуживание

В анимации ниже показан полный запуск каждые три временных шагов. Обратите внимание, что каждая из трех систем слежения - радар, лидар и слияние треков - смогли отследить все четыре транспортных средств в сценарии, и никакие ложные следы не были подтверждены.

Можно также количественно измерить этот аспект эффективности с помощью «пропущенного целевого» и «ложного трека» компонентов метрики GOSPA. Заметьте на рисунках ниже, что пропущенный целевой компонент начинается с более высокого значения из-за задержки установления и опускается до нуля примерно за 5-10 шагов для каждой системы слежения. Кроме того, заметьте, что компонент false track равен нулю для всех систем, что указывает на отсутствие подтвержденных ложных треков.

% Plot missed target component
figure; plot(missTarget','LineWidth',2); legend('Radar','Lidar','Fused');
title("Missed Target Metric"); xlabel('Time step'); ylabel('Metric'); grid on;

% Plot false track component
figure; plot(falseTracks','LineWidth',2); legend('Radar','Lidar','Fused');
title("False Track Metric"); xlabel('Time step'); ylabel('Metric'); grid on;

Точность трека-уровня

Уровень дорожки или точность локализации каждого трекера также могут быть количественно оценены метрикой GOSPA на каждом временном шаге. Меньшее значение указывает на лучшую точность отслеживания. Поскольку не было пропущенных целей или ложных дорожек, метрика захватывает ошибки локализации, следующие из оценки состояния каждого транспортного средства.

Обратите внимание, что метрика GOSPA для сплавленных оценок ниже, чем метрика для отдельного датчика, что указывает на то, что точность дорожки увеличилась после слияния оценок дорожки с каждого датчика.

% Plot GOSPA
figure; plot(gospa','LineWidth',2); legend('Radar','Lidar','Fused');
title("GOSPA Metric"); xlabel('Time step'); ylabel('Metric'); grid on;

Близко расположенные цели

Как упоминалось ранее, этот пример использует кластеризацию на основе Евклидова расстояния и аппроксимацию ограничивающего прямоугольника, чтобы передать лидарные данные в обычный алгоритм отслеживания. Алгоритмы кластеризации обычно страдают, когда объекты тесно разнесены. С помощью строений детектора, используемых в этом примере, когда проходящее транспортное средство приближается к транспортному средству перед автомобилем , оборудованным датчиком, детектор объединяет облако точек от каждого транспортного средства в большую ограничивающую коробку. В анимации ниже можно заметить, что дорожка отошла от центра транспортного средства. Поскольку трек был сообщен с большей степенью уверенности в его оценке для нескольких шагов, сплавление, оцененное первоначально, также было затронуто. Однако, когда неопределенность увеличивается, ее связь с сплавленной оценкой становится слабее. Это связано с тем, что алгоритм ковариационного пересечения выбирает вес смешения для каждой назначенной дорожки на основе уверенности каждой оценки.

Этот эффект также получен в метрике GOSPA. Вы можете заметить на графике метрики GOSPA выше, что метрика лидара показывает пик вокруг 65-го временного шага.

Радарные дорожки не затрагиваются во время этого события по двум основным причинам. Во-первых, радарный датчик выводит информацию о скорости области значений в каждом обнаружении, которая отличается от уровня шума для проезжающего автомобиля по сравнению с более медленным движущимся автомобилем. Это приводит к увеличению статистического расстояния между обнаружениями от отдельных автомобилей. Во-вторых, расширенные трекеры объектов оценивают несколько возможных гипотез кластеризации против предсказанных треков, что приводит к отказу от неподходящих кластеров и принятию правильных кластеров. Обратите внимание, что для правильного выбора оптимальных кластеров расширенными трекерами объектов фильтр для дорожки должен быть устойчивым до такой степени, чтобы он мог захватывать различие между двумя кластерами. Для примера дорожка с высоким уровнем технологического шума и сильно неопределенными размерностями может быть не в состоянии правильно заявить кластер из-за его преждевременного возраста и более высокой гибкости для учета неопределенных событий.

Цели на большой области значений

Когда цели отходят от радарных датчиков, точность измерений ухудшается из-за снижения отношения сигнал/шум в детекторе и ограниченного разрешения датчика. Это приводит к высокой неопределенности в измерениях, что, в свою очередь, снижает точность дорожки. Заметьте в отображение крупным планом ниже, что оценка дорожки с радара находится дальше от основной истины для радарного датчика и сообщается с более высокой неопределенностью. Однако датчик лидара сообщает о достаточном количестве измерений в облаке точек, чтобы сгенерировать «усаженный» ограничивающий прямоугольник. Эффект усадки, смоделированный в модели измерения для алгоритма отслеживания лидара, позволяет трекеру поддерживать дорожку с правильными размерностями. В таких ситуациях вес смешения лидара выше, чем радар, и позволяет, чтобы сплавленная оценка была более точной, чем оценка радара.

Сводные данные

В этом примере вы научились настраивать алгоритм слияния треков для сплавления треков с радаров и датчиков лидара. Вы также научились вычислять алгоритм отслеживания, используя Обобщённый Оптимальный Подшаблон Метрика и связанные с ним компоненты.

Служебные функции

collectSensorData

Функция для генерации радиолокационных и лидарных измерений в текущем временном шаге.

function [radarDetections, ptCloud, egoPose] = helperCollectSensorData(egoVehicle, radars, lidar, time)

% Current poses of targets with respect to ego vehicle
tgtPoses = targetPoses(egoVehicle);

radarDetections = cell(0,1);
for i = 1:numel(radars)
    thisRadarDetections = step(radars{i},tgtPoses,time);
    radarDetections = [radarDetections;thisRadarDetections]; %#ok<AGROW>
end

% Generate point cloud from lidar
rdMesh = roadMesh(egoVehicle);
ptCloud = step(lidar, tgtPoses, rdMesh, time);

% Compute pose of ego vehicle to track in scenario frame. Typically
% obtained using an INS system. If unavailable, this can be set to
% "origin" to track in Ego vehicle's frame.
egoPose = pose(egoVehicle);

end

radar2cental

Функция для преобразования дорожки в пространстве состояний радара в дорожку в центральном пространстве состояний.

function centralTrack = radar2central(radarTrack)

% Initialize a track of the correct state size
centralTrack = objectTrack('State',zeros(10,1),...
    'StateCovariance',eye(10));

% Sync properties of radarTrack except State and StateCovariance with
% radarTrack See syncTrack defined below.
centralTrack = syncTrack(centralTrack,radarTrack);

xRadar = radarTrack.State;
PRadar = radarTrack.StateCovariance;

H = zeros(10,7); % Radar to central linear transformation matrix
H(1,1) = 1;
H(2,2) = 1;
H(3,3) = 1;
H(4,4) = 1;
H(5,5) = 1;
H(8,6) = 1;
H(9,7) = 1;

xCentral = H*xRadar;  % Linear state transformation
PCentral = H*PRadar*H'; % Linear covariance transformation

PCentral([6 7 10],[6 7 10]) = eye(3); % Unobserved states

% Set state and covariance of central track
centralTrack.State = xCentral;
centralTrack.StateCovariance = PCentral;

end

central2radar

Функция для преобразования дорожки в центральном пространстве состояний в дорожку в радиолокационном пространстве состояний.

function radarTrack = central2radar(centralTrack)

% Initialize a track of the correct state size
radarTrack = objectTrack('State',zeros(7,1),...
    'StateCovariance',eye(7));

% Sync properties of centralTrack except State and StateCovariance with
% radarTrack See syncTrack defined below.
radarTrack = syncTrack(radarTrack,centralTrack);

xCentral = centralTrack.State;
PCentral = centralTrack.StateCovariance;

H = zeros(7,10); % Central to radar linear transformation matrix
H(1,1) = 1;
H(2,2) = 1;
H(3,3) = 1;
H(4,4) = 1;
H(5,5) = 1;
H(6,8) = 1;
H(7,9) = 1;

xRadar = H*xCentral;  % Linear state transformation
PRadar = H*PCentral*H'; % Linear covariance transformation

% Set state and covariance of radar track
radarTrack.State = xRadar;
radarTrack.StateCovariance = PRadar;
end

syncTrack

Функция для синхронизации свойств одной дорожки с другой, кроме свойств «State» и «StateCovariation»

function tr1 = syncTrack(tr1,tr2)
props = properties(tr1);
notState = ~strcmpi(props,'State');
notCov = ~strcmpi(props,'StateCovariance');

props = props(notState & notCov);
for i = 1:numel(props)
    tr1.(props{i}) = tr2.(props{i});
end
end

pose

Функция для возврата положения автомобиля , оборудованного датчиком как структуры.

function egoPose = pose(egoVehicle)
egoPose.Position = egoVehicle.Position;
egoPose.Velocity = egoVehicle.Velocity;
egoPose.Yaw = egoVehicle.Yaw;
egoPose.Pitch = egoVehicle.Pitch;
egoPose.Roll = egoVehicle.Roll;
end

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 penality for not measuring 3-D state
dist = dist + 3;

end

helperRadarLidarFusionFcn

Функция для слияния состояний и ковариаций состояний в центральном пространстве состояний дорожки

function [x,P] = helperRadarLidarFusionFcn(xAll,PAll)
n = size(xAll,2);
dets = zeros(n,1);

% Initialize x and P
x = xAll(:,1);
P = PAll(:,:,1);

onlyLidarStates = false(10,1);
onlyLidarStates([6 7 10]) = true;

% Only fuse this information with lidar
xOnlyLidar = xAll(onlyLidarStates,:);
POnlyLidar = PAll(onlyLidarStates,onlyLidarStates,:);

% States and covariances for intersection with radar and lidar both
xToFuse = xAll(~onlyLidarStates,:);
PToFuse = PAll(~onlyLidarStates,~onlyLidarStates,:);

% Sorted order of determinants. This helps to sequentially build the
% covariance with comparable determinations. For example, two large
% covariances may intersect to a smaller covariance, which is comparable to
% the third smallest covariance.
for i = 1:n
    dets(i) = det(PToFuse(1:2,1:2,i));
end
[~,idx] = sort(dets,'descend');
xToFuse = xToFuse(:,idx);
PToFuse = PToFuse(:,:,idx);

% Initialize fused estimate
thisX = xToFuse(:,1);
thisP = PToFuse(:,:,1);
 
% Sequential fusion
for i = 2:n
    [thisX,thisP] = fusecovintUsingPos(thisX, thisP, xToFuse(:,i), PToFuse(:,:,i));
end

% Assign fused states from all sources
x(~onlyLidarStates) = thisX;
P(~onlyLidarStates,~onlyLidarStates,:) = thisP;

% Fuse some states only with lidar source
valid = any(abs(xOnlyLidar) > 1e-6,1);
xMerge = xOnlyLidar(:,valid);
PMerge = POnlyLidar(:,:,valid);

if sum(valid) > 1
    [xL,PL] = fusecovint(xMerge,PMerge);
elseif sum(valid) == 1
    xL = xMerge;
    PL = PMerge;
else
    xL = zeros(3,1);
    PL = eye(3);
end

x(onlyLidarStates) = xL;
P(onlyLidarStates,onlyLidarStates) = PL;

end

function [x,P] = fusecovintUsingPos(x1,P1,x2,P2)
% Covariance intersection in general is employed by the following
% equations: 
% P^-1 = w1*P1^-1 + w2*P2^-1 
% x = P*(w1*P1^-1*x1 + w2*P2^-1*x2);
% where w1 + w2 = 1 
% Usually a scalar representative of the covariance matrix like "det" or
% "trace" of P is minimized to compute w. This is offered by the function
% "fusecovint". However. in this case, the w are chosen by minimizing the
% determinants of "positional" covariances only.
n = size(x1,1);
idx = [1 2];
detP1pos = det(P1(idx,idx));
detP2pos = det(P2(idx,idx));
w1 = detP2pos/(detP1pos + detP2pos);
w2 = detP1pos/(detP1pos + detP2pos);
I = eye(n);

P1inv = I/P1;
P2inv = I/P2;

Pinv = w1*P1inv + w2*P2inv;
P = I/Pinv;

x = P*(w1*P1inv*x1 + w2*P2inv*x2);

end

Ссылки

[1] Lang, Alex H., et al. «PointPillars: быстрые энкодеры для обнаружения объектов из облаков точек». Материалы Конференции IEEE по компьютерному зрению и распознаванию шаблонов. 2019.

[2] Чжоу, Инь и Онсель Тузель. «Voxelnet: сквозное обучение для обнаружения 3D объектов на основе облака точек». Материалы Конференции IEEE по компьютерному зрению и распознаванию шаблонов. 2018.

[3] Ян, Интервал, Вэньцзе Ло и Ракель Уртасун. «Пиксор: 3D-обнаружение объектов в реальном времени из облаков точек». Материалы конференции IEEE по компьютерному зрению и распознаванию шаблонов. 2018.