Лидар и радарный Fusion в городском воздушном сценарии мобильности
В этом примере вы изучаете, как использовать мультиобъектные средства отслеживания, чтобы отследить различные Беспилотные Воздушные Транспортные средства (БПЛА) в городской среде. Вы создаете сцену с помощью uavScenario основанный на объектах при создании и доступных данных ландшафта онлайн. Вы используете модели датчика лидара и радара, чтобы сгенерировать синтетические данные о датчике. Наконец, вы используете различные алгоритмы отслеживания, чтобы оценить состояние всех БПЛА в сцене.
БПЛА спроектированы для широкого спектра операций. Много приложений установлены в городских средах, таких как доставка пакета беспилотника, воздушные такси и контроль линии электропередачи. Безопасность этих операций становится очень важной, когда количество приложений растет, делая управление городским воздушным пространством проблемой.
Создайте городской воздушный сценарий мобильности
В этом примере вы используете ландшафт и данные о создании Boulder, CO. Файл Цифровых данных о вертикальном изменении ландшафта (DTED) загружается с "SRTM Пусто Заполненный" набор данных, доступный от американской Геологической службы. Данные о создании, сохраненные в building.mat, загружается с OpenStreetMap, доступного онлайн. Вы создаете сценарий UAV с помощью этих данных.
dtedfile = "n39_w106_3arc_v2.dt1"; buildingfile = "building.mat"; scene = createScenario(dtedfile,buildingfile);
Затем вы добавляете несколько БПЛА в сценарий.
Чтобы смоделировать операцию доставки пакета, вы задаете траекторию, уезжающую с крыши создания и полета к другому созданию. Траектория состоит из трех участков. quadrotor взлетает вертикально, затем летит к следующему месту назначения доставки, и наконец приземляется вертикально на крышу.
waypointsA = [1895 90 20; 1915 108 35; 1900 115 20]; timeA = [0 25 50]; trajA = waypointTrajectory(waypointsA, "TimeOfArrival", timeA, "ReferenceFrame", "ENU", "AutoBank", true); uavA = uavPlatform("UAV", scene, "Trajectory", trajA, "ReferenceFrame", "ENU"); updateMesh(uavA, "quadrotor", {5}, [0 1 1], eye(4));
Вы добавляете другой UAV, чтобы смоделировать воздушное такси, летящее. Его траектория линейна, немного убывая. Вы используете fixedwing геометрия, чтобы смоделировать больший UAV, которые подходят для переноса людей.
waypointsB = [1940 120 50; 1800 50 20]; timeB = [0 41]; trajB = waypointTrajectory(waypointsB, "TimeOfArrival", timeB, "ReferenceFrame", "ENU", "AutoBank", true); uavB = uavPlatform("UAV2", scene, "Trajectory", trajB, "ReferenceFrame", "ENU"); updateMesh(uavB, "fixedwing", {10}, [0 1 1], eye(4));
Затем вы добавляете quadrotor с траекторией после уличного пути. Это могло представлять UAV, смотря линии энергосистемы в целях обслуживания.
waypointsC = [1950 60 35; 1900 60 35; 1890 80 35]; timeC = linspace(0,41,size(waypointsC,1)); trajC = waypointTrajectory(waypointsC, "TimeOfArrival", timeC, "ReferenceFrame", "ENU", "AutoBank", true); uavC = uavPlatform("UAV3", scene, "Trajectory", trajC, "ReferenceFrame", "ENU"); updateMesh(uavC, "quadrotor", {5}, [0 1 1], eye(4));
Наконец, вы добавляете эго UAV, UAV, ответственный за слежку за сценой и отслеживание различных движущихся платформ.
waypointsD = [1900 140 65; 1910 100 65]; timeD = [0 60]; trajD = waypointTrajectory(waypointsD, "TimeOfArrival", timeD, ... "ReferenceFrame", "ENU", "AutoBank", true, "AutoPitch", true); egoUAV = uavPlatform("EgoVehicle", scene, "Trajectory", trajD, "ReferenceFrame", "ENU"); updateMesh(egoUAV, "quadrotor", {5}, [0 0 1], eye(4));
Задайте набор датчика UAV
Смонтируйте датчики на автомобиле, оборудованном датчиком. Вы используете шайбу лидара, которая обычно используется в автомобильном [1]. Это - маленький датчик, который может быть присоединен на quadrotor. Вы используете следующую спецификацию для шайбы лидара:
Разрешение области значений: 3 см
Максимальная область значений: 100 м
360 азимутов степеней охватывают с разрешением на 0,2 °
30 вертикальных изменений степеней охватывают с разрешением на 2 °
Частота обновления: 10 Гц
Смонтируйтесь с наклоном на 90 °, чтобы посмотреть вниз
% Mount a lidar on the quadrotor lidarOrient = [90 90 0]; lidarSensor = uavLidarPointCloudGenerator("MaxRange",100, ... "RangeAccuracy", 0.03, ... "ElevationLimits", [-15 15], ... "ElevationResolution", 2, ... "AzimuthLimits", [-180 180], ... "AzimuthResolution", 0.2, ... "UpdateRate", 10, ... "HasOrganizedOutput", false); lidar = uavSensor("Lidar", egoUAV, lidarSensor, "MountingLocation", [0 0 -3], "MountingAngles",lidarOrient);
Затем вы добавляете радар с помощью radarDataGenerator Системный объект от Radar Toolbox. Чтобы добавить этот датчик в платформу UAV, необходимо задать класс пользовательского адаптера. Детали показывают в Симулировать Радарном Датчике, Смонтированном На UAV (UAV Toolbox) пример. В этом примере вы используете helperRadarAdaptor класс. Этот класс использует геометрию mesh целей, чтобы задать размерности кубоида для радарной модели. Mesh также используется, чтобы вывести простую подпись ЭПР для каждой цели. Вдохновленный радаром Echodyne EchoFlight UAV [2], радарная настройка выбрана как:
Частота: 24.45 - 24.65 ГГц
Поле зрения: вертикальное изменение азимута 80 ° на 120 °
Разрешение: 2 градуса в азимуте, 6 ° в вертикальном изменении
Полная частота развертки: 1 Гц
Чувствительность: 0 dBsm на уровне 200 м
Кроме того, вы конфигурируете радар, чтобы вывести несколько обнаружений на объект. Хотя радар может вывести дорожки, представляющие цели точки, вы хотите оценить степень цели, которая не доступна с дорожкой по умолчанию выход. Поэтому вы устанавливаете TargetReportFormat свойство к Detections так, чтобы обнаружения сырой нефти показания радара непосредственно.
% Mount a radar on the quadrotor. radarSensor = radarDataGenerator("no scanning","SensorIndex",1,... "FieldOfView",[120 80],... "UpdateRate", 1,... 'MountingAngles',[0 30 0],... "HasElevation", true,... "ElevationResolution", 6,... "AzimuthResolution", 2, ... "RangeResolution", 4, ... "RangeLimits", [0 200],... 'ReferenceRange',200,... 'CenterFrequency',24.55e9,... 'Bandwidth',200e6,... "TargetReportFormat","Detections",... "DetectionCoordinates","Sensor rectangular",... "HasFalseAlarms",false,... "FalseAlarmRate", 1e-7); radarcov = coverageConfig(radarSensor); radar = uavSensor("Radar",egoUAV,helperRadarAdaptor(radarSensor));
Задайте систему слежения
Лоцируйте обработку облака точек
Датчики лидара возвращают облака точек. Чтобы плавить лидар выход, облако точек должно кластеризироваться, чтобы извлечь обнаружения объектов. Сегмент ландшафт с помощью segmentGroundSMRF функция от Lidar Toolbox. Остающееся облако точек кластеризируется, и простой порог применяется к каждому кластерному среднему вертикальному изменению, чтобы отфильтровать обнаружения создания. Соответствуйте каждому кластеру кубоидом, чтобы извлечь обнаружение ограничительной рамки. Класс помощника helperLidarDetector доступный в этом примере имеет детали реализации.
Обнаружения кубоида лидара отформатированы с помощью objectDetection объект. Состояние измерения для этих обнаружений , где:
координаты центра кубоида вдоль Востока, Севера, и осей сценария, соответственно.
длина, ширина и высота кубоида, соответственно.
кватернион, задающий ориентацию кубоида относительно осей ENU.
lidarDetector = helperLidarDetector(scene)
lidarDetector =
helperLidarDetector with properties:
MaxWindowRadius: 3
GridResolution: 1.5000
SegmentationMinDistance: 5
MinDetectionsPerCluster: 2
MinZDistanceCluster: 20
EgoVehicleRadius: 10
Лоцируйте средство отслеживания
Вы используете целевое средство отслеживания точки, trackerJPDA, отслеживать обнаружения ограничительной рамки лидара. Средство отслеживания точки принимает, что каждый UAV может сгенерировать самое большее одно обнаружение на скан датчика. Это предположение допустимо, потому что вы кластеризировали облако точек в кубоиды. Чтобы установить средство отслеживания, необходимо задать модель движения и модель измерения. В этом примере вы моделируете динамику БПЛА с помощью увеличенной постоянной скоростной модели. Постоянная скоростная модель достаточна, чтобы отследить траектории, состоящие из прямых участков рейса или медленно различных сегментов. Кроме того, примите, что ориентация UAV является постоянной, и примите, что размерности БПЛА являются постоянными. В результате уравнения изменения состояния и изменения состояния дорожки и
Здесь, , , координаты вектора скорости кубоида вдоль сценария оси ENU. Вы отслеживаете ориентацию с помощью кватерниона из-за разрыва Углов Эйлера при использовании отслеживания фильтров. , временной интервал между обновлениями k и k+1, равно 0.1 секунды. Наконец, шум аддитивного процесса, который получает погрешность моделирования.
Внутренняя матрица перехода соответствует постоянной скоростной модели. Вы задаете увеличенную версию состояния constvel и cvmeas с учетом дополнительного постоянного состояния. Детали реализованы в функциях поддержки initLidarFilter, augmentedConstvel, augmentedConstvelJac, augmentedCVmeas, и augmentedCVMeasJac в конце примера.
lidarJPDA = trackerJPDA('TrackerIndex',2,... 'AssignmentThreshold',[70 150],... 'ClutterDensity',1e-16,... 'DetectionProbability',0.99,... 'DeletionThreshold',[10 10],... Delete lidar track if missed for 1 second 'ConfirmationThreshold',[4 5],... 'FilterInitializationFcn',@initLidarFilter)
lidarJPDA =
trackerJPDA with properties:
TrackerIndex: 2
FilterInitializationFcn: @initLidarFilter
MaxNumEvents: Inf
EventGenerationFcn: 'jpdaEvents'
MaxNumTracks: 100
MaxNumDetections: Inf
MaxNumSensors: 20
TimeTolerance: 1.0000e-05
OOSMHandling: 'Terminate'
AssignmentThreshold: [70 150]
InitializationThreshold: 0
DetectionProbability: 0.9900
ClutterDensity: 1.0000e-16
TrackLogic: 'History'
ConfirmationThreshold: [4 5]
DeletionThreshold: [10 10]
HitMissThreshold: 0.2000
HasCostMatrixInput: false
HasDetectableTrackIDsInput: false
StateParameters: [1×1 struct]
NumTracks: 0
NumConfirmedTracks: 0
Радарное средство отслеживания
В этом примере вы принимаете, что радарные возвраты предварительно обрабатываются таким образом, который только возвращается из перемещения объектов, сохраняются, который не является никаким, возвращается из земли или созданий. Радарное состояние измерения . Радарное разрешение достаточно прекрасно, чтобы сгенерировать несколько возвратов на цель UAV, и ее обнаружения не должны питаться непосредственно целевое средство отслеживания точки. Существует два возможных подхода, чтобы отследить с радарными обнаружениями с высоким разрешением. Одним путем можно кластеризировать обнаружения и увеличить состояние с размерностями и константами ориентации, как сделано ранее с кубоидами лидара. На другом пути можно накормить обнаружениями расширенное целевое средство отслеживания принятыми в этом примере при помощи средства отслеживания GGIW-PHD. Это средство отслеживания оценивает степень каждой цели с помощью инверсии распределение Уишарта, ожидание которого является 3х3 положительной определенной матрицей, представляя степень цели как 3D эллипс. Этот второй подход предпочтителен, потому что нет слишком многих обнаружений на объект, и кластеризация менее точна, чем расширено-целевое отслеживание
Чтобы создать средство отслеживания GGIW-PHD, вы сначала задаете настройку датчика отслеживания для каждого датчика, сообщающего средству отслеживания. В этом случае только необходимо задать настройку для одного радара. Когда радарная платформа монтирования перемещается, необходимо обновить эту настройку с текущим радарным положением перед каждым шагом средства отслеживания. Затем вы задаете функцию инициализации фильтра на основе настройки датчика. Наконец, вы создаете trackerPHD возразите и увеличьте порог разделения, чтобы получить размерности объектов, прослеженных в этом примере. Детали реализации показывают в конце примера в функции поддержки createRadarTracker.
radarPHD = createRadarTracker(radarSensor, egoUAV)
radarPHD =
trackerPHD with properties:
TrackerIndex: 1
SensorConfigurations: {[1×1 trackingSensorConfiguration]}
PartitioningFcn: @(dets)partitionDetections(dets,threshold(1),threshold(2),'Distance','euclidean')
MaxNumSensors: 20
MaxNumTracks: 1000
AssignmentThreshold: 50
BirthRate: 1.0000e-03
DeathRate: 1.0000e-06
ExtractionThreshold: 0.8000
ConfirmationThreshold: 0.9900
DeletionThreshold: 0.1000
MergingThreshold: 50
LabelingThresholds: [1.0100 0.0100 0]
StateParameters: [1×1 struct]
HasSensorConfigurationsInput: true
NumTracks: 0
NumConfirmedTracks: 0
Отследите сплав
Последний шаг в создании системы слежения должен задать объект термофиксатора дорожки плавить дорожки лидара и радарные дорожки. Вы используете 13-мерное состояние дорожек лидара как сплавленное определение состояния.
radarConfig = fuserSourceConfiguration('SourceIndex',1,... 'IsInitializingCentralTracks',true); lidarConfig = fuserSourceConfiguration('SourceIndex',2,... 'IsInitializingCentralTracks',true); fuser = trackFuser('SourceConfigurations',{radarConfig,lidarConfig},... 'ProcessNoise',blkdiag(2*eye(6),1*eye(3),0.2*eye(4)),... 'HasAdditiveProcessNoise',true,... 'AssignmentThreshold',200,... 'ConfirmationThreshold',[4 5],... 'DeletionThreshold',[5 5],... 'StateFusion','Cross',... 'StateTransitionFcn',@augmentedConstvel,... 'StateTransitionJacobianFcn',@augmentedConstvelJac);
Визуализация
Вы используете класс помощника, чтобы визуализировать сценарий. Этот класс помощника использует uavScenario возможности визуализации и theaterPlot плоттер, чтобы представлять обнаружение и информацию о дорожке.
Отображение разделено на 5 мозаик, показав соответственно, полную 3D сцену, три камеры преследования для трех БПЛА и легенду.
viewer = helperUAVDisplay(scene);
Симулируйте сценарий
Вы запускаете сценарий и визуализируете результаты системы слежения. Истинное положение каждой цели, а также радара, лидара и сплавленных дорожек сохранено для оффлайнового метрического анализа.
setup(scene); s = rng; rng(2021); numSteps = scene.StopTime*scene.UpdateRate; truthlog = cell(1,numSteps); radarlog = cell(1,numSteps); lidarlog = cell(1,numSteps); fusedlog = cell(1,numSteps); logCount = 0; while advance(scene) time = scene.CurrentTime; % Update sensor readings and read data. updateSensors(scene); egoPose = read(egoUAV); % Track with radar [radardets, radarTracks, inforadar] = updateRadarTracker(radar,radarPHD, egoPose, time); % Track with lidar [lidardets, lidarTracks, nonGroundCloud, groundCloud] = updateLidarTracker(lidar,lidarDetector, lidarJPDA, egoPose); % Fuse lidar and radar tracks rectRadarTracks = formatPHDTracks(radarTracks); if isLocked(fuser) || ~isempty(radarTracks) || ~isempty(lidarTracks) [fusedTracks,~,allfused,info] = fuser([lidarTracks;rectRadarTracks],time); else fusedTracks = objectTrack.empty; end % Save log logCount = logCount + 1; lidarlog{logCount} = lidarTracks; radarlog{logCount} = rectRadarTracks; fusedlog{logCount} = fusedTracks; truthlog{logCount} = logTargetTruth(scene.Platforms(1:3)); % Update figure viewer(radarcov, nonGroundCloud, groundCloud, lidardets, radardets, lidarTracks, radarTracks, fusedTracks ); end
На основе результатов визуализации вы выполняете начальную качественную оценку эффективности отслеживания. Отображение в конце сценария показывает, что все три БПЛА были хорошо прослежены эго. С настройкой набора датчика тока дорожки лидара были только установлены частично из-за ограниченного покрытия датчика лидара. Более широкое поле зрения радара позволило устанавливать радарные дорожки более последовательно в этом сценарии.
Три анимированных GIF выше частей показа представлений преследования. Вы видите, что качество дорожек лидара (оранжевое поле) затронуто геометрией сценария. UAV (слева) освещается лидаром (отображенный желтым) почти непосредственно сверху. Это позволяет средству отслеживания получать в полной мере беспилотник в зависимости от времени. Однако UAV C (справа) частично замечен радаром, который приводит к недооцениванию размера беспилотника. Кроме того, предполагаемый центроид периодически колеблется вокруг истинного центра беспилотника. Большее фиксированное крыло UAV (середина) генерирует много точек лидара. Таким образом средство отслеживания может обнаружить и отследить в полной мере цель, если это полностью ввело поле зрения лидара. Во всех трех случаях радар, отображенный синим, предоставляет более достоверную информацию целевой степени. В результате сплавленное поле дорожки (в фиолетовом) более тесно получает степень каждого UAV. Однако радарные возвраты менее точны в положении. Радарные дорожки показывают больше смещения положения и более плохой оценки ориентации.
Отслеживание метрик
В этом разделе вы анализируете эффективность системы слежения с помощью GOSPA отслеживание метрики. Вы сначала задаете функцию расстояния, которая определяет количество ошибки между дорожкой и истиной с помощью скалярного значения. Более низкое значение GOSPA означает полную лучшую эффективность.
gospaR = trackGOSPAMetric('Distance','custom','DistanceFcn',@metricDistance); gospaL = clone(gospaR); gospaF = clone(gospaR); gospaRadar = zeros(1,numSteps); gospaLidar = zeros(1,numSteps); gospaFused = zeros(1,numSteps); for i=1:numSteps truth = truthlog{i}; gospaRadar(i) = gospaR(radarlog{i},truth); gospaLidar(i) = gospaL(lidarlog{i},truth); gospaFused(i) = gospaF(fusedlog{i},truth); end figure plot(gospaRadar,'Color',viewer.RadarColor,'LineWidth',2); hold on grid on plot(gospaLidar,'Color',viewer.LidarColor,'LineWidth',2); plot(gospaFused,'Color',viewer.FusedColor,'LineWidth',2); legend('Radar','Lidar','Lidar + Radar'); xlabel('Steps') ylabel('GOSPA')
Вы анализируете полную производительность системы. Каждое средство отслеживания оштрафовано за то, что оно не отследило ни один из БПЛА, даже если целевой UAV находится вне покрытия датчика. Это показывает улучшенную производительность при плавлении лидара и радара из-за добавленной области наблюдения. Это особенно примечательно в конце симуляции, где две цели прослежены, один радаром и другим лидаром, но оба прослежены термофиксатором. Кроме того, вы видите, что сплавленным GOSPA является ниже минимума лидара и радара GOSPA, показывая, что сплавленная дорожка имеет лучшее качество, чем каждая отдельная дорожка.
% clean up removeCustomTerrain("southboulder"); rng(s);
Сводные данные
В этом примере вы изучили, как смоделировать перенесенную БПЛА систему слежения лидара и радара и протестировали ее на городском воздушном сценарии мобильности. Вы использовали uavScenario объект создать реалистическую городскую среду с ландшафтом и созданиями. Вы сгенерировали синтетические данные о датчике, чтобы протестировать полную цепь системы слежения, включающую обработку облака точек, цель точки и расширили целевое отслеживание и сплав дорожки.
Вспомогательные Функции
createScenario создает uavScenario использование ландшафта OpenStreetMap и создание данных о mesh
function scene = createScenario(dtedfile,buildingfile) try addCustomTerrain("southboulder",dtedfile); catch % custom terrain was already added. end load(buildingfile,'buildings'); minHeight = 1.6925e+03; latlonCenter = [39.9786 -105.2882 minHeight]; scene = uavScenario("UpdateRate",10,"StopTime",40,... "ReferenceLocation",latlonCenter); % Add terrain mesh sceneXLim = [1800 2000]; sceneYLim = [0 200]; scene.addMesh("terrain", {"southboulder", sceneXLim, sceneYLim},[0 0 0]); % Add buildings for idx = 1:numel(buildings)-1 v = buildings{idx}.Vertices; v(:,3) = v(:,3) - minHeight; rangeVMin = min(v); rangeVMax = max(v); if rangeVMin(1) > sceneXLim(1) && rangeVMax(1) < sceneXLim(2) &&... rangeVMin(2) > sceneYLim(1) && rangeVMax(2) < sceneYLim(2) scene.addMesh("custom", {v, buildings{idx}.Faces},[0 0 0]); end end end
createRadarTracker создает trackerPHD средство отслеживания, чтобы плавить радарные обнаружения.
function tracker = createRadarTracker(radar, egoUAV) % Create sensor configuration for trackerPHD fov = radar.FieldOfView; sensorLimits = [-fov(1)/2 fov(1)/2; -fov(2)/2 fov(2)/2; 0 inf]; sensorResolution = [radar.AzimuthResolution;radar.ElevationResolution; radar.RangeResolution]; Kc = radar.FalseAlarmRate/(radar.AzimuthResolution*radar.RangeResolution*radar.ElevationResolution); Pd = radar.DetectionProbability; sensorPos = radar.MountingLocation(:); sensorOrient = rotmat(quaternion(radar.MountingAngles, 'eulerd', 'ZYX', 'frame'),'frame'); % Specify frame info of radar with respect to UAV sensorTransformParameters(1) = struct('Frame','Spherical',... 'OriginPosition', sensorPos,... 'OriginVelocity', zeros(3,1),...% Sensor does not move relative to ego 'Orientation', sensorOrient,... 'IsParentToChild',true,...% Frame rotation is supplied as orientation 'HasElevation',true,... 'HasVelocity',false); % Specify frame info of UAV with respect to scene egoPose = read(egoUAV); sensorTransformParameters(2) = struct('Frame','Rectangular',... 'OriginPosition', egoPose(1:3),... 'OriginVelocity', egoPose(4:6),... 'Orientation', rotmat(quaternion(egoPose(10:13)),'Frame'),... 'IsParentToChild',true,... 'HasElevation',true,... 'HasVelocity',false); radarPHDconfig = trackingSensorConfiguration(radar.SensorIndex,... 'IsValidTime', true,... 'SensorLimits',sensorLimits,... 'SensorResolution', sensorResolution,... 'DetectionProbability',Pd,... 'ClutterDensity', Kc,... 'SensorTransformFcn',@cvmeas,... 'SensorTransformParameters', sensorTransformParameters); radarPHDconfig.FilterInitializationFcn = @initRadarFilter; radarPHDconfig.MinDetectionProbability = 0.4; % Threshold for partitioning threshold = [3 16]; tracker = trackerPHD('TrackerIndex',1,... 'HasSensorConfigurationsInput',true,... 'SensorConfigurations',{radarPHDconfig},... 'BirthRate',1e-3,... 'AssignmentThreshold',50,...% Minimum negative log-likelihood of a detection cell to add birth components 'ExtractionThreshold',0.80,...% Weight threshold of a filter component to be declared a track 'ConfirmationThreshold',0.99,...% Weight threshold of a filter component to be declared a confirmed track 'MergingThreshold',50,...% Threshold to merge components 'DeletionThreshold',0.1,...% Threshold to delete components 'LabelingThresholds',[1.01 0.01 0],...% This translates to no track-splitting. Read LabelingThresholds help 'PartitioningFcn',@(dets) partitionDetections(dets, threshold(1),threshold(2),'Distance','euclidean')); end
initRadarfilter реализует фильтр GGIW-PHD, используемый trackerPHD объект. Этот фильтр используется во время обновления средства отслеживания, чтобы 1) инициализировать компоненты второго рождения в плотности и 2) инициализировать новый компонент от разделов обнаружения.
function phd = initRadarFilter (detectionPartition) if nargin == 0 % Process noise sigP = 0.2; sigV = 1; Q = diag([sigP, sigV, sigP, sigV, sigP, sigV].^2); phd = ggiwphd(zeros(6,0),repmat(eye(6),[1 1 0]),... 'ScaleMatrices',zeros(3,3,0),... 'MaxNumComponents',1000,... 'ProcessNoise',Q,... 'HasAdditiveProcessNoise',true,... 'MeasurementFcn', @cvmeas,... 'MeasurementJacobianFcn', @cvmeasjac,... 'PositionIndex', [1 3 5],... 'ExtentRotationFcn', @(x,dT)eye(3,class(x)),... 'HasAdditiveMeasurementNoise', true,... 'StateTransitionFcn', @constvel,... 'StateTransitionJacobianFcn', @constveljac); else %nargin == 1 % ------------------ % 1) Configure Gaussian mixture % 2) Configure Inverse Wishart mixture % 3) Configure Gamma mixture % ----------------- %% 1) Configure Gaussian mixture meanDetection = detectionPartition{1}; n = numel(detectionPartition); % Collect all measurements and measurement noises. allDets = [detectionPartition{:}]; zAll = horzcat(allDets.Measurement); RAll = cat(3,allDets.MeasurementNoise); % Specify mean noise and measurement z = mean(zAll,2); R = mean(RAll,3); meanDetection.Measurement = z; meanDetection.MeasurementNoise = R; % Parse mean detection for position and velocity covariance. [posMeas,velMeas,posCov] = matlabshared.tracking.internal.fusion.parseDetectionForInitFcn(meanDetection,'initRadarFilter','double'); % Create a constant velocity state and covariance states = zeros(6,1); covariances = zeros(6,6); states(1:2:end) = posMeas; states(2:2:end) = velMeas; covariances(1:2:end,1:2:end) = posCov; covariances(2:2:end,2:2:end) = 10*eye(3); % process noise sigP = 0.2; sigV = 1; Q = diag([sigP, sigV, sigP, sigV, sigP, sigV].^2); %% 2) Configure Inverse Wishart mixture parameters % The extent is set to the spread of the measurements in positional-space. e = zAll - z; Z = e*e'/n + R; dof = 150; % Measurement Jacobian p = detectionPartition{1}.MeasurementParameters; H = cvmeasjac(states,p); Bk = H(:,1:2:end); Bk2 = eye(3)/Bk; V = (dof-4)*Bk2*Z*Bk2'; % Configure Gamma mixture parameters such that the standard deviation % of the number of detections is n/4 alpha = 16; % shape beta = 16/n; % rate phd = ggiwphd(... ... Gaussian parameters states,covariances,... 'HasAdditiveMeasurementNoise' ,true,... 'ProcessNoise',Q,... 'HasAdditiveProcessNoise',true,... 'MeasurementFcn', @cvmeas,... 'MeasurementJacobianFcn' , @cvmeasjac,... 'StateTransitionFcn', @constvel,... 'StateTransitionJacobianFcn' , @constveljac,... 'PositionIndex' ,[1 3 5],... 'ExtentRotationFcn' , @(x,dT) eye(3),... ... Inverse Wishart parameters 'DegreesOfFreedom',dof,... 'ScaleMatrices',V,... 'TemporalDecay',150,... ... Gamma parameters 'Shapes',alpha,'Rates',beta,... 'GammaForgettingFactors',1.05); end end
formatPHDTracks форматирует эллиптические дорожки GGIW-PHD в прямоугольные увеличенные дорожки состояния для сплава дорожки. convertExtendedTrack возвращает ковариацию состояния и состояния увеличенного прямоугольного состояния. Инверсия Уишарт случайные матричные собственные значения используется, чтобы вывести размерности прямоугольника. Собственные векторы предоставляют кватернион ориентации. В этом примере вы используете произвольную ковариацию для радарной размерности дорожки и ориентации, которая часто достаточна для отслеживания.
function tracksout = formatPHDTracks(tracksin) % Convert track struct from ggiwphd to objectTrack with state definition % [x y z vx vy vz L W H q0 q1 q2 q3] N = numel(tracksin); tracksout = repmat(objectTrack,N,1); for i=1:N tracksout(i) = objectTrack(tracksin(i)); [state, statecov] = convertExtendedTrack(tracksin(i)); tracksout(i).State = state; tracksout(i).StateCovariance = statecov; end end function [state, statecov] = convertExtendedTrack(track) % Augment the state with the extent information extent = track.Extent; [V,D] = eig(extent); % Choose L > W > H. Use 1.5 sigma as the dimension [dims, idx] = sort(1.5*sqrt(diag(D)),'descend'); V = V(:,idx); q = quaternion(V,'rotmat','frame'); q = q./norm(q); [q1, q2, q3, q4] = parts(q); state = [track.State; dims(:); q1 ; q2 ; q3 ; q4 ]; statecov = blkdiag(track.StateCovariance, 4*eye(3), 4*eye(4)); end
updateRadarTracker обновляет радарную цепь отслеживания. Функция сначала читает, текущий радар возвращается. Затем радарные возвраты передаются средству отслеживания GGIW-PHD после обновления его настройки датчика с текущим положением беспилотника эго.
function [radardets, radarTracks, inforadar] = updateRadarTracker(radar,radarPHD, egoPose, time) [~,~,radardets, ~, ~] = read(radar); % isUpdated and time outputs are not compatible with this workflow inforadar = []; if mod(time,1) ~= 0 radardets = {}; end if mod(time,1) == 0 && (isLocked(radarPHD) || ~isempty(radardets)) % Update radar sensor configuration for the tracker configs = radarPHD.SensorConfigurations; configs{1}.SensorTransformParameters(2).OriginPosition = egoPose(1:3); configs{1}.SensorTransformParameters(2).OriginVelocity = egoPose(4:6); configs{1}.SensorTransformParameters(2).Orientation = rotmat(quaternion(egoPose(10:13)),'frame'); [radarTracks,~,~,inforadar] = radarPHD(radardets,configs,time); elseif isLocked(radarPHD) radarTracks = predictTracksToTime(radarPHD,'confirmed', time); radarTracks = arrayfun(@(x) setfield(x,'UpdateTime',time), radarTracks); else radarTracks = objectTrack.empty; end end
updateLidarTracker обновляет цепь отслеживания лидара. Функция сначала читает облако текущей точки выход из датчика лидара. Затем облако точек обрабатывается, чтобы извлечь обнаружения объектов. Наконец, эти обнаружения передаются целевому средству отслеживания точки.
function [lidardets, lidarTracks,nonGroundCloud, groundCloud] = updateLidarTracker(lidar, lidarDetector,lidarJPDA, egoPose) [~, time, ptCloud] = read(lidar); % lidar is always updated [lidardets,nonGroundCloud, groundCloud] = lidarDetector(egoPose, ptCloud,time); if isLocked(lidarJPDA) || ~isempty(lidardets) lidarTracks = lidarJPDA(lidardets,time); else lidarTracks = objectTrack.empty; end end
initLidarFilter инициализирует фильтр для средства отслеживания лидара. Начальное состояние дорожки выведено из измерения положения обнаружения. Скорость собирается в 0 с большой ковариацией позволить будущим обнаружениям быть сопоставленными к дорожке. Увеличенная модель движения состояния, функции измерения и якобианы также заданы ниже.
function ekf = initLidarFilter(detection) % Lidar measurement: [x y z L W H q0 q1 q2 q3] meas = detection.Measurement; initState = [meas(1);0;meas(2);0;meas(3);0; meas(4:6);meas(7:10) ]; initStateCovariance = blkdiag(100*eye(6), 100*eye(3), eye(4)); % Process noise standard deviations sigP = 1; sigV = 2; sigD = 0.5; % Dimensions are constant but partially observed sigQ = 0.5; Q = diag([sigP, sigV, sigP, sigV, sigP, sigV, sigD, sigD, sigD, sigQ, sigQ, sigQ, sigQ].^2); ekf = trackingEKF('State',initState,... 'StateCovariance',initStateCovariance,... 'ProcessNoise',Q,... 'StateTransitionFcn',@augmentedConstvel,... 'StateTransitionJacobianFcn',@augmentedConstvelJac,... 'MeasurementFcn',@augmentedCVmeas,... 'MeasurementJacobianFcn',@augmentedCVmeasJac); end function stateOut = augmentedConstvel(state, dt) % Augmented state for constant velocity stateOut = constvel(state(1:6,:),dt); stateOut = vertcat(stateOut,state(7:end,:)); % Normalize quaternion in the prediction stage idx = 10:13; qparts = stateOut(idx); n = sqrt(sum(qparts.^2)); qparts = qparts./n; if qparts(1) < 0 stateOut(idx) = -qparts; else stateOut(idx) = qparts; end end function jacobian = augmentedConstvelJac(state,varargin) jacobian = constveljac(state(1:6,:),varargin{:}); jacobian = blkdiag(jacobian, eye(7)); end function measurements = augmentedCVmeas(state) measurements = cvmeas(state(1:6,:)); measurements = [measurements; state(7:9,:); state(10:13,:)]; end function jacobian = augmentedCVmeasJac(state,varargin) jacobian = cvmeasjac(state(1:6,:),varargin{:}); jacobian = blkdiag(jacobian, eye(7)); end
logTargetTruth журналы истинное положение и размерности в течение симуляции для анализа эффективности.
function logEntry = logTargetTruth(targets) n = numel(targets); targetPoses = repmat(struct('Position',[],'Velocity',[],'Dimension',[],'Orientation',[]),1,n); uavDimensions = [5 5 0.3 ; 9.8 8.8 2.8; 5 5 0.3]; for i=1:n pose = read(targets(i)); targetPoses(i).Position = pose(1:3); targetPoses(i).Velocity = pose(4:6); targetPoses(i).Dimension = uavDimensions(i,:); targetPoses(i).Orientation = pose(10:13); end logEntry = targetPoses; end
metricDistance задает пользовательское расстояние для GOSPA. Это расстояние включает ошибки в положение, скорость, размерность и ориентацию дорожек.
function out = metricDistance(track,truth) positionIdx = [1 3 5]; velIdx = [2 4 6]; dimIdx = 7:9; qIdx = 10:13; trackpos = track.State(positionIdx); trackvel = track.State(velIdx); trackdim = track.State(dimIdx); trackq = quaternion(track.State(qIdx)'); truepos = truth.Position; truevel = truth.Velocity; truedim = truth.Dimension; trueq = quaternion(truth.Orientation); errpos = truepos(:) - trackpos(:); errvel = truevel(:) - trackvel(:); errdim = truedim(:) - trackdim(:); % Weights expressed as inverse of the desired accuracy posw = 1/0.2; %m^-1 velw = 1/2; % (m/s) ^-1 dimw = 1/4; % m^-1 orw = 1/20; % deg^-1 distPos = sqrt(errpos'*errpos); distVel = sqrt(errvel'*errvel); distdim = sqrt(errdim'*errdim); distq = rad2deg(dist(trackq, trueq)); out = (distPos * posw + distVel * velw + distdim * dimw + distq * orw)/(posw + velw + dimw + orw); end
Ссылки
Шайба Лидара Velodyne: https://velodynelidar.com/products/puck/
Радар Echodyne UAV: https://www.echodyne.com/defense/uav-radar/