Перрон - это определенная зона в аэропорту, предназначенная для размещения воздушных судов в целях погрузки или разгрузки пассажиров, почты или груза, заправки топливом, стоянки или технического обслуживания [1]. Перроны аэропортов, как правило, являются очень динамичными и разнородными средами, где персонал перронов и транспортные средства работают в непосредственной близости друг от друга. Из-за такого характера перронов он представляет более высокий риск для наземных аварий с участием воздушных судов, а также наземного персонала. В качестве эффективного метода улучшения картины ситуации и меры по снижению высокого риска на перронах предложены системы наблюдения на основе Лидара [2].
В этом примере показано, как моделировать данные Лидара для сцены фартового трафика и отслеживать наземный трафик с помощью расширенного трекера объектов GGIW-PHD (Gamma Gaussian Inverse Wishart PHD).
В этом примере моделируется сценарий, в котором самолет входит в область ворот. Самолет направляется в место своей стоянки тремя маршаллерами, по одному с каждой стороны самолета и по одному перед ним. Вы имитируете наземное движение рядом с местом стоянки самолета. После того, как воздушное судно припарковано, наземное движение, а также маршаллеры начинают движение в сторону воздушного судна. В дополнение к самолету, входящему в зону ворот, вы также имитируете два самолета, уже припаркованных у ворот. Сценарий, использованный в этом примере, был создан с помощью конструктора сценариев отслеживания и экспортирован в функцию MATLAB ®, чтобы связать его с нижестоящими функциональными возможностями. Экспортированная функция была изменена в MATLAB для указания Mesh свойство для каждой платформы и для завершения траекторий до времени окончания сценария. Mesh имущества Platform позволяет определить геометрию платформы для моделирования лидара. В этом примере геометрия для самолета задается как tracking.scenario.airplaneMesh. Другие объекты в сцене представлены с помощью кубоидов. Для получения дополнительной информации о создании сценария см. createScenario в конце этого примера.
scenario = createScenario;
Для восприятия сцены используется датчик лидара поля зрения 360 градусов с помощью monostaticLidarSensor object™ системы. Датчик имеет 64 канала возвышения и установлен на терминале на высоте 8 метров от земли. Разрешение датчика по азимуту определяется как 0,32 градуса. При такой конфигурации датчик выдает в общей сложности 72 000 точек на сканирование. Этот набор точек данных с высоким разрешением обычно называют облаком точек. Данные от датчика моделируются на частоте 5 Гц путем указания UpdateRate датчика.
% Create a lidar sensor lidar = monostaticLidarSensor( ... 'SensorIndex',1,... 'UpdateRate',5,... 'HasINS',true,... 'AzimuthLimits',[-180 180],... 'ElevationLimits',[-10 27],... 'AzimuthResolution',0.32,... 'ElevationResolution',0.5781,... 'DetectionCoordinates','scenario',... 'MountingLocation',[50 -15 -8],... 'MountingAngles',[-90 0 0],... 'MaxRange',150,... 'RangeAccuracy',0.002); % Get access to terminal platform terminal = scenario.Platforms{1}; % Mount the lidar on the platform terminal.Sensors = lidar;
Вы можете наблюдать сцену и смоделированные лидарные данные с высоты птичьего полета, а также с крыши терминала в анимации ниже. Облако точек обозначается цветными точками, где цвет меняется с синего на желтый с высотой. Обратите внимание, что возврат лидара содержит отражения от окружающей среды, такой как земля.
В анимации также можно увидеть, что лидарный датчик возвращает несколько измерений на объект. Обычные многообъектные трекеры, такие как trackerGNN (GNN) трекер, trackerJPDA Трекер (JPDA) предполагает, что каждый датчик сообщает об одном измерении на объект. Для использования обычных трекеров для отслеживания объектов с использованием лидара облако точек из потенциальных объектов обычно предварительно обрабатывается и группируется в новый тип измерений. Эти новые измерения обычно определяют позиционные, а также размерные аспекты объекта, например измерение ограничивающей рамки. Полный рабочий процесс по использованию обычных трекеров с данными лидара см. в примере Track Vehicles Using Lidar: From Point Cloud to Track List. Этот рабочий процесс сегментирования данных лидара и кластеризации в ограничивающие рамки подвержен недостаткам кластеризации, когда объекты становятся слишком близкими. Особенно это касается фартуковых сред. Альтернативным способом отслеживания объектов с использованием данных лидара является использование расширенных средств отслеживания объектов. В отличие от обычных трекеров, расширенные трекеры объектов предназначены для отслеживания объектов, которые производят более одного измерения на датчик.
Расширенный отслеживание объектов
В этом примере используется реализация GGIW (Gamma Gaussian Inverse Wishart) трекера плотности гипотез вероятности (PHD). Эта модель GGIW использует три распределения для задания целевой модели: гауссово распределение для описания кинематики центра движения цели, таких как ее положение и скорость; Распределение обратного Вишарта (IW) для описания эллипсоидальной протяженности мишени; Гамма-распределение для описания ожидаемого количества измерений от цели. Для получения дополнительной информации о фильтре GGIW-PHD см. [3].
Расширенный объект PHD-трекер использует алгоритм разбиения для обработки набора входных измерений. Алгоритм разделения отвечает за определение множественных возможных гипотез сегментации для измерений датчиков. Поскольку общее количество гипотез для сегментации обычно очень велико, методы аппроксимации, такие как разбиение на расстояния (см. partitionDetections), используется прогнозное разбиение и максимизация ожидания (EM) [3]. Алгоритм разделения на расстояния работает аналогично методам кластеризации на основе расстояния с той разницей, что он создает несколько возможных разделов. Алгоритм предсказания-разбиения и ЭМ использует предсказания от трекера об объектах для содействия разбиению измерений на множество возможных кластеров. Этот метод использования прогнозов от трекера необходим, когда объекты пространственно близки друг к другу, например, в фартовой среде. В этом примере используется класс помощника helperPartitioningAlgorithm для разделения набора измерений с использованием прогнозов от трекера. Вы получаете эти прогнозы, используя predictTracksToTime функция trackerPHD.
% A handle object to pass predicted tracks into partitioning algorithm partitioner = helperPartitioningAlgorithm; % A function which takes detections as inputs and return partitions partitioningFcn = @(detections)partitionDetections(partitioner,detections);
Чтобы настроить PHD-трекер, сначала определите конфигурацию датчика с помощью trackingSensorConfiguration объект. Свойства моделируемого лидарного датчика используются для задания свойств конфигурации. Также определяется функция initFilter, которая инициализирует постоянную скорость. ggiwphd фильтр. Эта функция охватывает initcvggiwphd и повышает определенность в распределении Гамма и распределении Обратного Вишарта цели. Он также настраивает фильтр для работы с большим количеством обнаружений, указывая MaxNumDetections свойство фильтра ggiwphd.
% Sensor's field of view defined in same order as returned % by sensor's transform function sensorLimits = [lidar.AzimuthLimits;lidar.ElevationLimits;0 lidar.MaxRange]; % Sensor's resolution defined in same order as limits sensorResolution = [lidar.AzimuthResolution;lidar.ElevationResolution;lidar.MaxRange]; % Parameters to transform state of the track by transform function. trackToSensorTransform = struct('Frame','spherical',... 'OriginPosition',lidar.MountingLocation(:),... 'Orientation',rotmat(quaternion(lidar.MountingAngles,'eulerd','ZYX','frame'),'frame'),... 'IsParentToChild',true,... 'HasVelocity',false); % A function to initiate a PHD filter by this sensor filterInitFcn = @initFilter; config = trackingSensorConfiguration(1,... 'IsValidTime',true,...% update with sensor on each step call 'FilterInitializationFcn',filterInitFcn,...% Function to initialize a PHD filter 'SensorTransformFcn',@cvmeas,...% Transformation function from state to az,el,r 'SensorTransformParameters',trackToSensorTransform,...% Parameters for transform function 'SensorLimits',sensorLimits,... 'SensorResolution',sensorResolution,... 'DetectionProbability',0.95... % Probability of detecting the target );
Затем соберете эту информацию и создайте расширенный PHD-трекер объектов с помощью trackerPHD object™ системы.
tracker = trackerPHD('SensorConfigurations',config,... 'PartitioningFcn',partitioningFcn,... 'AssignmentThreshold',30,...% -log-likelihood beyond which a measurment cell initializes new components 'ConfirmationThreshold',0.9,...% Threshold to call a component as confirmed track 'ExtractionThreshold',0.75,...% Threshold to call a component as track 'MergingThreshold',25,...% Threshold to merge components with same Label 'LabelingThresholds',[1.1 0.1 0.05]); % Prevents track-splitting
Метрики
Затем можно настроить калькулятор показателей GOSPA с помощью trackGOSPAMetric класс для оценки производительности трекера. Показатель GOSPA предназначен для анализа производительности трекера путем предоставления единой стоимости. Меньшее значение стоимости обеспечивает более высокую производительность отслеживания. Для использования метрики GOSPA определяется функция расстояния между дорожкой и истинностью. Эта функция расстояния вычисляет стоимость назначения дорожки и истинности друг другу. Он также используется для представления точности оценки на уровне локализации или отслеживания. Расстояние между дорожкой и истинностью в этом примере определяется с помощью 'Custom' дистанционная функция trackTrackDistance, включенная в вспомогательные функции ниже. Когда GGIW-PHD трекер оценивает геометрический центр, пользовательская функция расстояния использует положение геометрического центра платформы для определения позиционной ошибки.
gospaObj = trackGOSPAMetric('Distance','custom','DistanceFcn',@trackTruthDistance);
Вы используете класс помощника helperAirportTrackingDisplay для визуализации истинности земли, смоделированных данных и оценочных дорожек.
display = helperAirportTrackingDisplay;
Затем выполняется сценарий, моделируются возвраты от лидарного датчика и обрабатываются с помощью трекера. Вы также рассчитываете метрику GOSPA для дорожек, используя доступную истинность грунта. В этом примере предполагается, что предварительная обработка данных лидара для удаления возвратов лидара из среды, такой как земля и терминал, уже выполнена. Таким образом, возврат из среды удаляется с использованием доступной информации о истинности грунта из моделирования о возврате лидара.
% Initialize tracks confTracks = struct.empty(0,1); % Ground truth for metric platforms = scenario.Platforms; trackablePlatforms = platforms(cellfun(@(x)x.ClassID,platforms) > 0); % Initialize metric values gospaMetrics = zeros(0,4); % Number of tracks recording for MATLAB vs MEX numTrackML = zeros(0,1); % Advance scenario while advance(scenario) time = scenario.SimulationTime; % Call lidarDetect on terminal to simulate point cloud returns. [ptCloud, ~, groundTruthIdx] = lidarDetect(terminal,time); % Pack returns as objectDetection. Included in Supporting Functions detections = packAsObjectDetection(ptCloud, groundTruthIdx, time); % Provide predicted tracks for prediction partitioning if isLocked(tracker) predictedTracks = predictTracksToTime(tracker,'all',time); partitioner.PredictedTrackList = predictedTracks; end % Call tracker step using detections and time confTracks = tracker(detections, time); % Calculate OSPA metric truthStruct = cellfun(@(x)x.pose,trackablePlatforms); [~,gospa,~,loc,missT,falseT] = gospaObj(confTracks, trackablePlatforms); gospaMetrics(end+1,:) = [gospa loc missT falseT]; %#ok<SAGROW> % Update the display display(scenario, ptCloud, confTracks); end
Затем оцените производительность каждого трекера с помощью визуализации, а также количественных показателей. В анимации ниже треки представлены синими квадратами, аннотированными их идентичностями (TrackID). Tон эллипсоид вокруг объекта описывает степень, оцененную фильтром GGIW-PHD.
Техническое обслуживание пути
До стоянки самолета трекер может отслеживать все объекты, кроме маршала справа от самолета. Это происходит потому, что маршаллер оставался незамеченным датчиком в его исходном положении. После стоянки летательного аппарата трекер способен реагировать на движение объектов для оценки их положения, а также скорости. Трекер способен поддерживать трассу на всех объектах примерно до 34 секунд. После этого маршаллер слева закупоривается за реактивными двигателями самолёта и его трасса сбрасывается трекером из-за множественных промахов. По мере приближения наземного движения к самолёту трекер способен отслеживать их как отдельные объекты и не путал их измерения друг с другом. Это было возможно, потому что предсказания трекера об объектах помогали правильно разделить набор измерений.
Можно также количественно проанализировать эту производительность трекера, используя метрику GOSPA и ее компоненты. Обратите внимание, что компонент ложной дорожки метрики остается нулевым, что указывает на отсутствие подтвержденных ложных дорожек. Ненулевой пропущенный целевой компонент представляет необнаруженный маршаллер. Обратите внимание, что как только трекер устанавливает дорожку на этом объекте, компонент падает до нуля. Пропущенный целевой компонент снова поднимается до ненулевого значения, которое представляет падение дорожки из-за окклюзии объекта.
figure; order = ["Total GOSPA","Localization","Missed Target","False Tracks"]; for i = 1:4 ax = subplot(2,2,i); plot(gospaMetrics(:,i),'LineWidth',2); title(order(i)); xlabel('Time'); ylabel('Metric'); grid on; end
Точность на уровне отслеживания
Точность на уровне дорожки относится к точности оценки состояния каждого объекта, такого как его положение, скорость и размеры. Можно заметить, что эти следы лежат на основании истинности объектов. Когда объект начинает движение, строится вектор скорости, представляющий направление и величину скорости. Обратите внимание, что расчетная скорость находится в том же направлении, что и движение объекта. Фильтр GGIW-PHD предполагает, что измерения от цели распределены в предполагаемом эллипсоидальном масштабе цели. Это приводит к тому, что фильтр оценивает размер объекта с использованием наблюдаемых или неокклюзированных областей объектов. Обратите внимание, что пути на припаркованных наземных транспортных средствах имеют смещение по направлению к наблюдаемым сторонам.
Неточности уровня дорожки также могут быть количественно оценены с использованием компонента локализации метрики GOSPA. Общая метрика GOSPA является комбинированной метрикой всех ее компонентов и, следовательно, фиксирует эффект точности локализации, пропущенных целей и ложных дорожек.
В этом примере показано, как моделировать данные лидара для сценария фартука и обрабатывать их с помощью расширенного объекта GGIW-PHD-трекера. Также вы научились оценивать производительность трекера с помощью метрики GOSPA и связанных с ней компонентов.
packAsObjectDetection
function detections = packAsObjectDetection(ptCloud, groundTruthIdx, time) % Get x,y,z locations from the cell array loc = ptCloud{1}'; % Points which are nan reflect those rays which did not intersect with any % object nanPoints = any(isnan(loc),1); % groundTruthIdx{1} represents the ground truth of the points. The first % column represents PlatformID and second column represents ClassID. % ClassID for environment is specified as 0. envPoints = groundTruthIdx{1}(:,2) == 0; % nanPoints or envPoints are remove toRemove = nanPoints | envPoints'; % Keep valid return loc = loc(:,~toRemove); % Pack each return as objectDetection. n = sum(~toRemove); % Allocate memory detections = repmat({objectDetection(time,[0;0;0],'MeasurementNoise',1e-2*eye(3))},n,1); % Fill measurements for i = 1:n detections{i}.Measurement = loc(:,i); end end
createScenario
function scenario = createScenario % Create Scenario scenario = trackingScenario; scenario.UpdateRate = 0; planeMesh = tracking.scenario.airplaneMesh; % Create platforms Terminal = platform(scenario,'ClassID',0); Terminal.Dimensions = struct( ... 'Length', 100, ... 'Width', 29, ... 'Height', 20, ... 'OriginOffset', [0 0 10]); Aircraft = platform(scenario,'ClassID',1,'Mesh',planeMesh); Aircraft.Dimensions = struct( ... 'Length', 39, ... 'Width', 34, ... 'Height', 13.4, ... 'OriginOffset', [39/2 0 13.4/2]); Aircraft.Signatures{1} = ... rcsSignature(... 'Pattern', [20 20;20 20], ... 'Azimuth', [-180 180], ... 'Elevation', [-90;90], ... 'Frequency', [0 1e+20]); Aircraft.Trajectory = waypointTrajectory(... [73.2829 -80.5669 0;60.7973 -37.3337 0],... [0;11.25],... 'GroundSpeed',[8;0],... 'Course',[106.1085;106.1085],... 'Orientation',quaternion([106.1085 0 0;106.1085 0 0],'eulerd','ZYX','frame')); delayTrajectory(Aircraft,0.1,70); Ground = platform(scenario,'ClassID',0); Ground.Trajectory.Position = [0 0 0]; Ground.Dimensions = struct('Length',500,... 'Width',500,... 'Height',1,... 'OriginOffset',[0 0 -0.5]); Plane = platform(scenario,'ClassID',1,'Mesh',planeMesh); Plane.Dimensions = struct( ... 'Length', 40, ... 'Width', 30, ... 'Height', 10, ... 'OriginOffset', [0 0 5]); Plane.Signatures{1} = ... rcsSignature(... 'Pattern', [20 20;20 20], ... 'Azimuth', [-180 180], ... 'Elevation', [-90;90], ... 'Frequency', [0 1e+20]); Plane.Trajectory.Position = [-11 -50.3 0]; Plane.Trajectory.Orientation = quaternion([90 0 0], 'eulerd','zyx','frame'); Marshall = platform(scenario,'ClassID',6); Marshall.Dimensions = struct( ... 'Length', 0.24, ... 'Width', 0.45, ... 'Height', 1.7, ... 'OriginOffset', [0 0 0.85]); s = [57.3868 -25.5244 0;59.41 -32.53 0]; s2 = [57.3868 -25.5244 0;60.5429 -36.4531 0]; Marshall.Trajectory = waypointTrajectory( ... s2, ... [0;8], ... 'Course', [-75;-75], ... 'GroundSpeed', [0;0], ... 'ClimbRate', [0;0], ... 'Orientation', quaternion([0.793353340291235 0 0 -0.608761429008721;0.793353340291235 0 0 -0.608761429008721])); delayTrajectory(Marshall, 12, 70); Marshall1 = platform(scenario,'ClassID',6); Marshall1.Dimensions = struct( ... 'Length', 0.24, ... 'Width', 0.45, ... 'Height', 1.7, ... 'OriginOffset', [0 0 0.85]); Marshall1.Trajectory = waypointTrajectory( ... [50 -100 0;56.5 -89.3 0;56.8 -73.6 0;60.9 -59.5 0], ... [0;6.28880678506828;14.2336975651715;38.5523666710762], ... 'Course', [52.5692412453125;71.0447132029417;101.247131543705;107.567935050594], ... 'GroundSpeed', [2;2;2;0], ... 'ClimbRate', [0;0;0;0], ... 'Orientation', quaternion([0.896605327597113 0 0 0.442830539286162;0.813888868238491 0 0 0.581020576363237;0.634412633965184 0 0 0.772994572985708;0.590831447702589 0 0 0.806795017588522])); delayTrajectory(Marshall1, 12, 70); Marshall2 = platform(scenario,'ClassID',6); Marshall2.Dimensions = struct( ... 'Length', 0.24, ... 'Width', 0.45, ... 'Height', 1.7, ... 'OriginOffset', [0 0 0.85]); Marshall2.Trajectory = waypointTrajectory( ... [135.1 -87.7 0;118.3 -87.7 0;112.2 -73.3 0;80.8 -47.6 0], ... [0;10.3490919743252;18.1872070129823;42], ... 'Course', [155.957629971433;124.12848387907;114.734795167842;153.606988602699], ... 'GroundSpeed', [2;2;2;0], ... 'ClimbRate', [0;0;0;0], ... 'Orientation', quaternion([0.896605327597113 0 0 0.442830539286162;0.813888868238491 0 0 0.581020576363237;0.634412633965184 0 0 0.772994572985708;0.590831447702589 0 0 0.806795017588522])); delayTrajectory(Marshall2, 12, 70); Loader = platform(scenario,'ClassID',2); Loader.Dimensions = struct( ... 'Length', 10, ... 'Width', 4, ... 'Height', 4, ... 'OriginOffset', [0 0 2]); Loader.Trajectory = waypointTrajectory( ... [25.2 -42.2 0;39.4500 -42.3000 0;53.7 -42.4 0], ... [0;5;10], ... 'Course',[-0.4021;-0.4021;-0.4021],... 'Orientation',[quaternion([-0.4021 0 0],'eulerd','ZYX','frame');quaternion([-0.4021 0 0],'eulerd','ZYX','frame');quaternion([-0.4021 0 0],'eulerd','ZYX','frame')],... 'GroundSpeed',[0;5.8;0],... 'ClimbRate',[0;0;0],... 'AutoPitch', false, ... 'AutoBank', false); delayTrajectory(Loader, 12, 70); Loader2 = platform(scenario,'ClassID',2); Loader2.Dimensions = struct( ... 'Length', 10, ... 'Width', 4, ... 'Height', 4, ... 'OriginOffset', [0 0 2]); Loader2.Trajectory = waypointTrajectory( ... [27.142823040363762 -75 0;42.5614 -71.9000 0;57.98 -68.8 0], ... [0;5;10], ... 'Course', [11.368107148451321;11.368107148451321;11.368107148451321], ... 'Orientation',quaternion([11.368107148451321 0 0;11.368107148451321 0 0;11.368107148451321 0 0],'eulerd','ZYX','frame'),... 'GroundSpeed', [0;5.824096001626275;0], ... 'ClimbRate', [0;0;0], ... 'AutoPitch', false, ... 'AutoBank', false); delayTrajectory(Loader2, 50, 70); Power = platform(scenario,'ClassID',2); Power.Dimensions = struct( ... 'Length', 5, ... 'Width', 2, ... 'Height', 1, ... 'OriginOffset', [0 0 0.5]); Power.Trajectory = waypointTrajectory( ... [27.2312963703295 -20.7687036296705 0;40.1656 -27.6344 0;53.1 -34.5 0], ... [0;5;10], ... 'Course', [-27.9596882700088;-27.9596882700088;-27.9596882700088], ... 'Orientation',[quaternion([-27.9596882700088 0 0],'eulerd','ZYX','frame');quaternion([-27.9596882700088 0 0],'eulerd','ZYX','frame');quaternion([-27.9596882700088 0 0],'eulerd','ZYX','frame')],... 'GroundSpeed', [0;5.8574;0], ... 'ClimbRate', [0;0;0], ... 'AutoPitch', false, ... 'AutoBank', false); delayTrajectory(Power, 20, 70); Refueller = platform(scenario,'ClassID',2); Refueller.Dimensions = struct( ... 'Length', 7, ... 'Width', 3, ... 'Height', 2, ... 'OriginOffset', [0 0 1]); Refueller.Trajectory = waypointTrajectory( ... [92.3 -31.6 0;83.3 -36.7 0;74.3 -41.8 0], ... [0;5;10], ... 'Course', [-150.4612;-150.4612;-150.4612], ... 'Orientation',[quaternion([-150.4612 0 0],'eulerd','ZYX','frame');quaternion([-150.4612 0 0],'eulerd','ZYX','frame');quaternion([-150.4612 0 0],'eulerd','ZYX','frame')],... 'GroundSpeed', [0;4.137825515896000;0], ... 'ClimbRate', [0;0;0], ... 'AutoPitch', false, ... 'AutoBank', false); delayTrajectory(Refueller, 20, 70); Car = platform(scenario,'ClassID',2); Car.Dimensions = struct( ... 'Length', 4.7, ... 'Width', 1.8, ... 'Height', 1.4, ... 'OriginOffset', [0 0 0.7]); Car.Trajectory = waypointTrajectory( ... [111.1 -44.8 0;93.8100 -48.1725 0;76.5200 -51.5450 0], ... [0;7.046336423986581;14.092672847973162], ... 'Course', [-169.250904745059;-169.250904745059;-169.250904745059], ... 'Orientation',[quaternion([-169.250904745059 0 0],'eulerd','ZYX','frame');quaternion([-169.250904745059 0 0],'eulerd','ZYX','frame');quaternion([-169.250904745059 0 0],'eulerd','ZYX','frame')],... 'GroundSpeed', [0;5;0], ... 'ClimbRate', [0;0;0], ... 'AutoPitch', false, ... 'AutoBank', false); delayTrajectory(Car, 20, 70); Plane1 = platform(scenario,'ClassID',1,'Mesh',planeMesh); Plane1.Dimensions = struct( ... 'Length', 40, ... 'Width', 30, ... 'Height', 10, ... 'OriginOffset', [0 0 5]); Plane1.Signatures{1} = ... rcsSignature(... 'Pattern', [20 20;20 20], ... 'Azimuth', [-180 180], ... 'Elevation', [-90;90], ... 'Frequency', [0 1e+20]); Plane1.Trajectory.Position = [110 0 0]; Plane1.Trajectory.Orientation = quaternion([180 0 0], 'eulerd','zyx','frame'); function delayTrajectory(plat, tOffset, tEnd) traj = plat.Trajectory; wp = traj.Waypoints; toa = traj.TimeOfArrival; g = traj.GroundSpeed; c = traj.Course; cr = traj.ClimbRate; o = traj.Orientation; tEnd = max(toa(end),tEnd); wp = [repmat(wp(1,:),1,1);wp;repmat(wp(end,:),1,1)]; toa = [0;toa + tOffset;tEnd]; g = [0;g;0]; c = [c(1);c;c(end)]; cr = [0;cr;0]; o = [repmat(o(1),1,1);o;repmat(o(end),1,1)]; newTraj = waypointTrajectory(wp,toa,...; 'Course',c,... 'GroundSpeed',g,... 'ClimbRate',cr,... 'Orientation',o); plat.Trajectory = newTraj; end end
initFilter
function filter = initFilter (varargin) % This function interacts with the tracker in two signatures % filter = initFilter() is called during prediction stages to add % components to PHD. % % filter = initFilter(detections) is called during correction stages to add % components to the PHD from detection sets which have a low-likelihood % against existing tracks. All detections in the input have the assumption % to belong to the same target. % Adds zero components during prediction stages filter = initcvggiwphd(varargin{:}); % If called with a detection if nargin == 1 % Get expected size expSize = filter.ScaleMatrices/(filter.DegreesOfFreedom - 4); % Higher the dof, higher the certainty in dimensions dof = 50; filter.DegreesOfFreedom = dof; filter.ScaleMatrices = (dof-4)*expSize; % Get expected number of detections expNumDets = filter.Shapes/filter.Rates; % shape/rate^2 = uncertainty; uncertainty = (expNumDets/4)^2; filter.Shapes = expNumDets^2/uncertainty; filter.Rates = expNumDets/uncertainty; end % GammaForgettingFactors acts like process noise % for measurement rate filter.GammaForgettingFactors(:) = 1.03; % Temporal Decay acts like process noise for % dimensions. Lower the decay, higher the variance increase filter.TemporalDecay = 500; % Specify MaxNumDetections filter.MaxNumDetections = 10000; end
trackTruthDistance
function dist = trackTruthDistance(track, truth) % The tracks estimate the center of the object. Compute the origin position rOriginToCenter = -truth{1}.Dimensions.OriginOffset(:); rot = quaternion([truth{1}.Orientation],'eulerd','ZYX','frame'); actPos = truth{1}.Position(:) + rotatepoint(rot,rOriginToCenter')'; % Estimated track position estPos = track.State(1:2:end); % Error distance is between position. dist = norm(actPos(:) - estPos); end
[1] Вебер, Людвиг. Международная организация гражданской авиации. Kluwer Law International BV, 2017.
[2] Мунд, Йоханнес, Лотар Мейер и Хартмут Фрике. «Требования к производительности LiDAR и оптимизированное позиционирование датчиков для снижения рисков на базе облака точек в перронах аэропортов». Материалы шестой Международной конференции по исследованиям в области воздушного транспорта. 2014.
[3] Granström, Karl, et al. «На расширенном слежении за целью с помощью PHD-фильтров». (2012).