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

Сценарий, используемый в этом примере, создается с помощью drivingScenario (Automated Driving Toolbox). Данные из датчиков радара и лидара симулированы с помощью drivingRadarDataGenerator (Automated Driving Toolbox) и lidarPointCloudGenerator (Automated Driving Toolbox), соответственно. Создание сценария и моделей датчика перенесено в функцию помощника helperCreateRadarLidarScenario. Для получения дополнительной информации о сценарии и синтетической генерации данных, относитесь, чтобы Создать Ведущий Сценарий Программно (Automated Driving Toolbox).

% For reproducible results
rng(2020);

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

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

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

Как упомянуто, радары имеют более высокое разрешение, чем объекты и возвращают несколько обнаружений на объект. Обычные средства отслеживания, такие как Глобальный самый близкий сосед (GNN) и Объединенная вероятностная ассоциация данных (JPDA) принимают, что датчики возвращают самое большее одно обнаружение на объект на скан. Поэтому обнаружения от датчиков с высоким разрешением должны или кластеризироваться прежде, чем обработать его с обычными средствами отслеживания или должны быть обработаны с помощью расширенных объектных средств отслеживания. Расширенные объектные средства отслеживания не требуют предварительной кластеризации обнаружений и обычно оценивают и кинематические состояния (например, положение и скорость) и степень объектов. Для более подробного сравнения между обычными средствами отслеживания и расширенными объектными средствами отслеживания, обратитесь к Расширенному Объектному Отслеживанию Дорожных транспортных средств с примером Радара и Камеры.

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

Алгоритм для отслеживания объектов с помощью радарных измерений перенесен в классе помощника, helperRadarTrackingAlgorithm, реализованный как Система object™. Этот класс выводит массив objectTrack объекты и задают свое состояние согласно следующему соглашению:

radarTrackingAlgorithm = helperRadarTrackingAlgorithm(radars);

Лоцируйте алгоритм отслеживания

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

В этом примере данные о лидаре обрабатываются с помощью обычного средства отслеживания объединенной вероятностной ассоциации данных (JPDA), сконфигурированного с фильтром взаимодействующей многоуровневой модели (IMM). Предварительная обработка данных о лидаре, чтобы удалить облако точек выполняется при помощи основанного на RANSAC плоского алгоритма подбора, и ограничительные рамки формируются путем выполнения Евклидового алгоритма кластеризации расстояния. Для получения дополнительной информации об алгоритме, обратитесь к Транспортным средствам Дорожки Используя Лидар: От Облака точек до примера Списка Дорожек. Сравненный соединенный пример, отслеживание выполняется в системе координат сценария, и средство отслеживания настраивается по-другому на отслеживаемые объекты различных размеров. Далее состояния переменных заданы по-другому, чтобы ограничить движение дорожек в направлении его предполагаемого угла рыскания.

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

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

lidarTrackingAlgorithm = helperLidarTrackingAlgorithm(lidar);

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

Термофиксатор

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

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

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

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

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

Следующий шаг должен задать алгоритм сплава состояния. Алгоритм сплава состояния берет несколько состояний и ковариаций состояния в центральном пространстве состояний, как введено и возвращает сплавленную оценку состояния и ковариаций. В этом примере вы используете перекрестный алгоритм ковариации, предоставленный функцией помощника, 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);

Метрики

В этом примере вы оцениваете эффективность каждого алгоритма с помощью Обобщенной Оптимальной Метрики Присвоения SubPattern (GOSPA) метрика. Вы настраиваете три отдельных метрики с помощью trackGOSPAMetric для каждого из средств отслеживания. Метрика GOSPA стремится оценивать эффективность системы слежения путем обеспечения скалярной стоимости. Нижнее значение метрики указывает на лучшую эффективность алгоритма отслеживания.

Использовать метрику GOSPA с пользовательскими моделями движения как та использовало в этом примере, вы устанавливаете Distance свойство к 'пользовательскому' и задает функцию расстояния между дорожкой и ее связанной основной истиной. Этими функциями расстояния, показанными в конце этого примера, является 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);

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

Визуализация для этого примера реализована с помощью класса помощника 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 шагах для каждой системы слежения. Кроме того, заметьте, что ложный компонент дорожки является нулем для всех систем, который указывает, что никакие ложные дорожки не были подтверждены.

% 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" и "StateCovariance"

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] Ленг, Алекс Х., и др. "PointPillars: Быстрые энкодеры для обнаружения объектов от облаков точек". Продолжения Конференции по IEEE по Компьютерному зрению и Распознаванию образов. 2019.

[2] Чжоу, Инь и Oncel Tuzel. "Voxelnet: от начала до конца изучение для облака точек основывало 3-е обнаружение объектов". Продолжения Конференции по IEEE по Компьютерному зрению и Распознаванию образов. 2018.

[3] Ян, Интервал, Вэньцзе Ло и Ракель Уртасун. "Pixor: 3-е обнаружение объектов в реальном времени от облаков точек". Продолжения конференции по IEEE по Компьютерному зрению и Распознаванию образов. 2018.