Расширенное отслеживание объектов с помощью лидар для наземного наблюдения в аэропорту
Фартук представляет собой определенную зону в аэропорту, предназначенную для размещения воздушных судов в целях погрузки или разгрузки пассажиров, почты или груза, заправки, стоянки или обслуживания [1]. Перроны аэропорта обычно являются высоко динамичными и неоднородными окружениями, где персонал фартука и транспортные средства работают в непосредственной близости друг от друга. Из-за такого характера фартуков он представляет собой более высокий риск аварий наземного обслуживания с участием самолетов, а также наземного персонала. В качестве эффективного метода улучшения ситуации были предложены системы наблюдения за фартуками на основе лидара, которые служат мерой снижения высокого риска в фартуках [2].
В этом примере показано, как симулировать данные Лидара для сцены перронного трафика и отслеживать наземный трафик с помощью расширенного трекера объектов GGIW-PHD (Gamma Gaussian Inverse Wishart PHD).
Setup
В этом примере моделируется сценарий, в котором самолет входит в область ворот. В место стоянки самолет направляется тремя маршалками, по одному с каждой стороны самолета и по одному перед ним. Моделируете наземное движение около места стоянки самолета. После того, как самолет припаркован, наземное движение, а также маршалки начинают движение в сторону самолета. В сложение с самолетом, входящим в зону ворот, вы также имитируете два самолета, уже припаркованных у ворот. Сценарий, используемый в этом примере, был создан с помощью Tracking Scenario Designer и экспортирован в функцию MATLAB ®, чтобы соединить ее с нисходящими функциями. Экспортированная функция была изменена в MATLAB, чтобы задать Mesh свойство для каждой платформы и для завершения траекторий ко времени окончания сценария. The 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. Этот рабочий процесс сегментации данных лидара и кластеризации в ограничивающие рамки склонен к несовершенствам кластеризации, когда объекты слишком близко. Это особенно актуально в случае окружений фартука. Альтернативным способом отслеживания объектов с помощью данных лидара является использование расширенных трекеров объектов. В отличие от обычных трекеров, трекеры с расширенными объектами предназначены для отслеживания объектов, которые производят более одного измерения на датчик.
Setup расширенного отслеживания объектов и показателей эффективности
Расширенный отслеживатель объектов
В этом примере вы используете реализацию GGIW (Gamma Gaussian Reverse Wishart) трекера плотности гипотезы вероятностей (PHD). Эта модель GGIW использует три распределения, чтобы задать целевую модель: Гауссовское распределение, чтобы описать кинематику центра движения цели, такую как ее положение и скорость; Обратное распределение Wishart (IW) для описания эллипсоидальной степени мишени; Гамма- распределение, чтобы описать ожидаемое количество измерений от цели. Для получения дополнительной информации о фильтре GGIW-PHD см. [3].
PHD-трекер расширенного объекта использует алгоритм разбиения для обработки входного набора измерений. Алгоритм разбиения отвечает за определение нескольких возможных гипотез сегментации для измерений датчика. Поскольку количество общей гипотезы для сегментации обычно очень велико, методы приближения, такие как разбиение на расстояния (см partitionDetections), Предсказания разделения и максимизации ожиданий (EM) используются [3]. Алгоритм разбиения на расстояния работает аналогично методам кластеризации на расстоянии с различием, что он производит несколько возможных разбиений. Алгоритм разбиения на предсказания и EM использует предсказания от трекера об объектах, чтобы помочь разбиению измерений на несколько возможных кластеров. Этот метод использования предсказаний от трекера важен, когда объекты пространственно близки друг к другу, например, в среде фартука. В этом примере вы используете класс helper 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 свойство фильтра giwphd.
% 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 класс для оценки эффективности трекера. Metric GOSPA нацелен на анализ эффективности трекера путем предоставления единого значения затрат. Более низкое значение стоимости представляет собой лучшую эффективность отслеживания. Чтобы использовать метрику GOSPA, задается функция расстояния между дорожкой и истиной. Эта функция расстояния вычисляет стоимость назначения дорожки и истинности друг другу. Он также используется, чтобы представлять локализацию или точность уровня дорожки оценки. Расстояние между дорожкой и истинностью в этом примере определяется с помощью 'Custom' distance function trackTruthDistance, включенный в Вспомогательные функции ниже. Поскольку трекер GGIW-PHD оценивает геометрический центр, функция пользовательского расстояния использует положение геометрического центра платформы, чтобы задать позиционную ошибку.
gospaObj = trackGOSPAMetric('Distance','custom','DistanceFcn',@trackTruthDistance);
Вы используете класс helper 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] Мунд, Йоханнес, Лотар Мейер и Хартмут Фрике. «Требования к эффективности лИДАР и оптимизированное позиционирование датчика для снижения рисков в облаке точек на апронах аэропорта». Материалы шестой Международной конференции по исследованиям в области воздушного транспорта. 2014.
[3] Granström, Karl, et al. «При расширенном отслеживании цели с использованием PHD-фильтров». (2012).