Расширенное отслеживание объекта с лидаром для наземного наблюдения аэропорта

Фартук является заданной областью в аэропорту, предназначенном, чтобы вместить самолет в целях загрузить или разгрузить пассажиров, почту или груз, заправку, парковку или обслуживание [1]. Фартуки аэропорта являются обычно очень динамическими и разнородными средами, где персонал фартука и транспортные средства действуют в непосредственной близости друг от друга. Из-за такой природы фартуков, это представляет более высокий риск для несчастных случаев наземного обслуживания включающий самолет, а также наземный персонал. Основанные на лидаре системы наблюдения в фартуках были предложены как эффективный метод улучшить изображение ситуации и служить мерой, чтобы смягчить высокий риск в фартуках [2].

Этот пример показывает вам, как симулировать данные о Лидаре для сцены трафика фартука и наземного трафика дорожки с помощью GGIW-PHD (Гэмма Госсиэн Инверс Уишарт PHD) расширенное объектное средство отслеживания.

Сценарий Setup

В этом примере вы симулируете сценарий, где самолет вводит в область логического элемента. Самолет ведется в его место для парковки тремя marshallers, один на каждой стороне самолета и один перед ним. Вы симулируете наземный трафик около места для парковки самолета. После того, как самолет припаркован, наземный трафик, а также marshallers начинает двигать самолет. В дополнение к самолету, вводящему в область логического элемента, вы также симулируете два самолета, уже припаркованные в логическом элементе. Сценарий, используемый в этом примере, был создан при помощи Tracking Scenario Designer и экспортирован в функцию MATLAB®, чтобы соединить его с нисходящими функциональностями. Экспортируемая функция была изменена в MATLAB, чтобы задать Mesh свойство для каждой платформы и завершать траектории ко времени окончания сценария. Mesh свойство Platform позволяет вам задавать геометрию платформы для симуляции лидара. В этом примере вы задаете геометрию для самолета как tracking.scenario.airplaneMesh. Другие объекты в сцене представлены с помощью кубоидов. Для получения дополнительной информации о, как создать сценарий, обратитесь к createScenario функция в конце этого примера.

scenario = createScenario;

Чтобы чувствовать сцену, вы используете полный датчик лидара поля зрения с помощью 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) принимает, что каждый датчик сообщает об одном измерении на объект. Чтобы использовать обычные средства отслеживания для отслеживания объектов с помощью лидара, облако точек от потенциальных объектов обычно предварительно обрабатывается и кластеризируется в новый тип измерений. Эти новые измерения обычно задают позиционные, а также размерные аспекты объекта, например, измерения ограничительной рамки. Для полного рабочего процесса при использовании обычных средств отслеживания с данными о лидаре обратитесь к Транспортным средствам Дорожки Используя Лидар: От Облака точек до примера Списка Дорожек. Этот рабочий процесс сегментирующихся данных о лидаре и кластеризирующийся в ограничительные рамки подвержен кластеризирующимся недостаткам, когда объекты становятся слишком близкими. Это особенно верно в случае сред фартука. Альтернативный путь к отслеживаемым объектам с помощью данных о лидаре состоит в том, чтобы использовать расширенные объектные средства отслеживания. В отличие от обычных средств отслеживания, расширенные объектные средства отслеживания спроектированы к отслеживаемым объектам, которые производят больше чем одно измерение на датчик.

Setup расширенное объектное средство отслеживания и показатели производительности

Расширенное объектное средство отслеживания

В этом примере вы используете GGIW (Гэмма Госсиэн Инверс Уишарт) реализация средства отслеживания плотности гипотезы вероятности (PHD). Эта модель GGIW использует три распределения, чтобы задать целевую модель: Распределение Гаусса, чтобы описать кинематику центра движения цели, такого как его положение и скорость; распределение Инверсии Уишарта (IW), чтобы описать эллипсоидальную степень цели; Гамма распределение, чтобы описать ожидаемое количество измерений от цели. Для получения дополнительной информации о фильтре GGIW-PHD обратитесь к [3].

Расширенное средство отслеживания объекта PHD использует алгоритм разделения, чтобы обработать входной набор измерения. Алгоритм разделения ответственен за определение нескольких возможная гипотеза сегментации для измерений датчика. Когда количество общей гипотезы для сегментации является обычно очень большими, методами приближенных вычислений как Разделение Расстояния (см. partitionDetections), Разделение Предсказания и Максимизация ожидания (EM) используются [3]. Делящий расстояние алгоритм работает похожий на основанные на расстоянии методы кластеризации с различием, что это производит несколько возможных разделов. Алгоритм разделения предсказания и EM использует предсказания от средства отслеживания об объектах ввести разделение измерений в несколько возможных кластеров. Этот метод, чтобы использовать предсказания от средства отслеживания важен, когда объекты - пространственно друг близко к другу такой как в среде фартука. В этом примере вы используете класс помощника 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' функция расстояния trackTruthDistance, включенный в Функции Поддержки ниже. Когда средство отслеживания 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.

Отследите обслуживание

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

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

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

Figure contains 4 axes. Axes 1 with title Total GOSPA contains an object of type line. Axes 2 with title Localization contains an object of type line. Axes 3 with title Missed Target contains an object of type line. Axes 4 with title False Tracks contains an object of type line.

Точность уровня дорожки

Точность уровня дорожки относится к точности в оценке состояния каждого объекта, такого как его положение, скорость и размерности. Можно заметить, что, который дорожки лежат на основной истине объектов. Когда объект начинает перемещаться, вектор скорости построен, представляя направление и величину скорость. Заметьте, что предполагаемая скорость находится в том же направлении как объектное движение. Фильтр 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] Mund, Иоганнес, Лотар Мейер и Хартмут Фрик. "Требования к производительности LiDAR и Оптимизированное Расположение Датчика для Точки Облачное Уменьшение рисков в Фартуках Аэропорта". Продолжения 6-й Международной конференции по вопросам Исследования в Воздушных перевозках. 2014.

[3] Granström, Карл, и др. "На расширенном целевом отслеживании с помощью фильтров PHD". (2012).

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