Введение

Датчики, которые могут наблюдать только информацию о дальности, не могут обеспечить полное понимание состояния объекта из одного обнаружения. Кроме того, неопределенность измерения только в диапазоне, когда представлено в декартовом кадре координат, не является гауссовой и создает вогнутую форму. Для датчиков только диапазона с узким полем зрения (ФОВ) угловая неопределённость мала и может быть аппроксимирована эллипсоидом, то есть гауссовым распределением. Однако для датчиков, имеющих только дальность действия с широким ПТ, неопределенность, когда она представлена в декартовом кадре, описывается вогнутой формой кольца, которая не может быть легко аппроксимирована эллипсоидом. Это показано на рисунке ниже.

Это явление наблюдается и с дальнобойными радарами, которые обеспечивают как азимутальную, так и дальнобойную информацию. Когда FOV каждой ячейки разрешения азимута охватывает большую область в пространстве, распределение состояния становится негауссовым. Методы, такие как поправки на большие расстояния, часто используются для использования гауссовых фильтров, таких как расширенные фильтры Кальмана (trackingEKF) и фильтры Калмана без запаха (trackingUKF) в этих сценариях. Это подробно показано в примере мультиплексного радиолокационного обнаружения Fusion.

В этом примере вы узнаете, как использовать фильтр частиц и фильтр гауссовой суммы для представления негауссовой неопределенности в состоянии, вызванной измерениями диапазона от больших датчиков ФОВ.

Определение сценария

Сценарий моделирует один объект, движущийся с постоянной скоростью в плоскости X-Y. Объект пересекает зоны покрытия трех равноотстоящих датчиков с КТ 60 градусов по азимуту. Датчики FOV перекрываются, что повышает наблюдаемость, когда объект пересекает перекрывающиеся области. Каждый датчик сообщает об измерении дальности с точностью измерения 5 сантиметров.

s = rng;
rng(2018);
exPath = fullfile(matlabroot,'examples','fusion','main');
addpath(exPath);
[scene,theaterDisplay] = helperCreateRangeOnlySensorScenario;
showScenario(theaterDisplay)

showGrabs(theaterDisplay,[]);

Отслеживание с помощью фильтра частиц

В этом разделе фильтр частиц, trackingPF используется для отслеживания объекта. Отслеживание выполняется трекером с одной гипотезой с использованием trackerGNN. Трекер отвечает за поддержание трека при уменьшении количества ложных тревог.

Функция initRangeOnlyCVPF инициализирует фильтр частиц с постоянной скоростью. Функция аналогична функции initcvpf функция, но ограничивает углы от [-180 180] по азимуту и [-90 90] по отметке для initcvpf к известному датчику FOV в этой проблеме.

% Tracker
tracker = trackerGNN('FilterInitializationFcn',@initRangeOnlyCVPF,'MaxNumTracks',5);

% Update display to plot particle data
theaterDisplay.FilterType = 'trackingPF';

% Advance scenario and track object
while advance(scene)
    % Current time
    time = scene.SimulationTime;

    % Generate detections
    [detections, configs] = generateRangeDetections(scene);

    % Pass detections to tracker
    if ~isempty(detections)
        [confTracks,~,allTracks] = tracker(detections,time);
    elseif isLocked(tracker)
        confTracks = predictTracksToTime(tracker,'confirmed',time);
    end

    % Update display
    theaterDisplay(detections,configs,confTracks,tracker);
end

Обратите внимание, что частицы имеют негауссову неопределенность в состоянии по дуге измерения только диапазона, пока цель не будет обнаружена следующим датчиком. Когда мишень перемещается через границы зон покрытия датчика, вероятность частиц на границе увеличивается по сравнению с другими частицами. Это увеличение вероятности вызывает этап повторной дискретизации в фильтре частиц, и частицы разрушаются до истинного целевого местоположения.

showGrabs(theaterDisplay,[1 2]);

Используя фильтр частиц, трекер поддерживает трассу в течение всего сценария.

showGrabs(theaterDisplay,3);

Отслеживание с использованием фильтра гауссовой суммы

В этом разделе фильтр гауссовой суммы, trackingGSF, используется для отслеживания объекта. Для измерений только в диапазоне можно инициализировать trackingGSF использование initapekf. initapekf инициализирует параметризованный углом расширенный фильтр Калмана. Функция initRangeOnlyGSF , определенный в этом примере, изменяет initapekf функция для отслеживания медленно движущихся объектов.

% restart scene
restart(scene);
release(theaterDisplay);

% Tracker
tracker = trackerGNN('FilterInitializationFcn',@initRangeOnlyGSF);

% Update display to plot Gaussian-sum components
theaterDisplay.FilterType = 'trackingGSF';

while advance(scene)
    % Current time
    time = scene.SimulationTime;

    % Generate detections
    [detections, configs] = generateRangeDetections(scene);

    % Pass detections to tracker
    if ~isempty(detections)
        [confTracks,~,allTracks] = tracker(detections,time);
    elseif isLocked(tracker)
        confTracks = predictTracksToTime(tracker,'confirmed',time);
    end

    % Update display
    theaterDisplay(detections,configs,confTracks,tracker);
end

Вы можете использовать TrackingFilters имущества trackingGSF для просмотра состояния каждого расширенного фильтра Калмана. На следующем рисунке, представленном как «Отдельные фильтры», обратите внимание, как фильтры выравниваются по дуге, создаваемой измерением только диапазона, пока цель не достигнет перекрывающейся области. Сразу после пересечения границы вероятность фильтра на границе возрастает, и дорожка сходится к этому отдельному фильтру. Вес, ModelProbabilities, других отдельных фильтров падает по сравнению с ближайшим к границе, и их вклад в оценку состояния уменьшается.

showGrabs(theaterDisplay,[4 5]);

Используя фильтр гауссовой суммы, трекер поддерживает трассу в течение всего сценария.

showGrabs(theaterDisplay,6);
rng(s)
rmpath(exPath);

Резюме

В этом примере показано, как использовать фильтр частиц и фильтр гауссовой суммы для отслеживания объекта с помощью измерений только в диапазоне. Как фильтры частиц, так и фильтры гауссовой суммы предлагают возможности для отслеживания объектов, которые следуют за негауссовым распределением состояний. В то время как фильтр гауссовой суммы аппроксимирует распределение по взвешенной сумме гауссовых компонентов, фильтр частиц представляет это распределение по набору образцов. Фильтр частиц предлагает более естественный подход для представления любого произвольного распределения, пока из него могут быть получены образцы. Однако для точного представления распределения может потребоваться большое количество частиц, что увеличивает вычислительные требования к использованию фильтра. Поскольку состояние описывается образцами, архитектура фильтра частиц позволяет использовать любое распределение технологического шума, а также измерительный шум. Напротив, фильтр гауссовой суммы использует гауссовский процесс и шум измерения для каждого компонента. Одним из основных недостатков частиц является проблема «обнищания образцов». После повторной выборки частицы могут разрушаться в области высокой вероятности, не позволяя фильтру восстановиться после «неправильной» ассоциации. Поскольку повторная выборка в гауссовом фильтре не выполняется, и каждое состояние является гауссовым, гауссовы фильтры обеспечивают некоторую возможность восстановления после такой ассоциации. Однако это восстановление часто зависит от весов и ковариаций каждого гауссова компонента.

Вспомогательные функции

generateRangeDetections Эта функция генерирует обнаружение только диапазона

function [detections,config] = generateRangeDetections(scene)
detections = [];
config = [];
time = scene.SimulationTime;
for i = 1:numel(scene.Platforms)
    [thisDet, thisConfig] = detectOnlyRange(scene.Platforms{i},time);
    config = [config;thisConfig]; %#ok<AGROW>
    detections = [detections;thisDet]; %#ok<AGROW>
end
end

detectOnlyRange Эта функция удаляет az и el из сферического обнаружения

function [rangeDetections,config] = detectOnlyRange(observer,time)
    [fullDetections,numDetections,config] = detect(observer,time);
    for i = 1:numel(config)
        % Populate the config correctly for plotting
        config(i).FieldOfView = observer.Sensors{i}.FieldOfView;
        config(i).MeasurementParameters(1).OriginPosition = observer.Sensors{1}.MountingLocation(:);
        config(i).MeasurementParameters(1).Orientation = rotmat(quaternion(observer.Sensors{1}.MountingAngles(:)','eulerd','ZYX','frame'),'frame');
        config(i).MeasurementParameters(1).IsParentToChild = true;
    end
    % Remove all except range information
    rangeDetections = fullDetections;
    for i = 1:numDetections
        rangeDetections{i}.Measurement = rangeDetections{i}.Measurement(end);
        rangeDetections{i}.MeasurementNoise = rangeDetections{i}.MeasurementNoise(end);
        rangeDetections{i}.MeasurementParameters(1).HasAzimuth = false;
        rangeDetections{i}.MeasurementParameters(1).HasElevation = false;
    end
end

initRangeOnlyGSF Эта функция инициализирует параметризованный углом фильтр гауссовой суммы. Он использует угловые пределы [-30 30] для задания FOV и устанавливает число фильтров равным 10. Он также изменяет ковариацию состояния и шум процесса для медленно движущегося объекта в 2-D.

function filter = initRangeOnlyGSF(detection)
filter = initapekf(detection,10,[-30 30]);
for i = 1:numel(filter.TrackingFilters)
    filterK = filter.TrackingFilters{i};
    filterK.ProcessNoise = 0.01*eye(3);
    filterK.ProcessNoise(3,3) = 0;
    % Small covariance for slow moving targets
    filterK.StateCovariance(2,2) = 0.1;
    filterK.StateCovariance(4,4) = 0.1;
    % Low standard deviation in z as v = 0;
    filterK.StateCovariance(6,6) = 0.01;
end
end

initRangeOnlyCVPF Эта функция инициализирует фильтр частиц с постоянной скоростью с частицами, ограниченными FOV [-30 30].

function filter = initRangeOnlyCVPF(detection)
filter = initcvpf(detection);

% Uniform az in FOV
az = -pi/6 + pi/3*rand(1,filter.NumParticles);
% no elevation;
el = zeros(1,filter.NumParticles);
% Samples of range from Gaussian range measurement
r = detection.Measurement + sqrt(detection.MeasurementNoise)*randn(1,filter.NumParticles);

% x,y,z in sensor frame
[x,y,z] = sph2cart(az,el,r);

% Rotate from sensor to scenario frame.
senToPlat = detection.MeasurementParameters(1).Orientation';
senPosition = detection.MeasurementParameters(1).OriginPosition;
platToScenario = detection.MeasurementParameters(2).Orientation';
platPosition = detection.MeasurementParameters(2).OriginPosition;
posPlat = senToPlat*[x;y;z] + senPosition;
posScen = platToScenario*posPlat + platPosition;

% Position particles
filter.Particles(1,:) = posScen(1,:);
filter.Particles(3,:) = posScen(2,:);
filter.Particles(5,:) = posScen(3,:);

% Velocity particles
% uniform distribution
filter.Particles([2 4],:) = -1 + 2*rand(2,filter.NumParticles);

% Process Noise is set to a low number for slow moving targets. Larger than
% GSF to add more noise to particles preventing them from collapsing.
filter.ProcessNoise = 0.1*eye(3);

% Zero z-velocity
filter.Particles(6,:) = 0;
filter.ProcessNoise(3,3) = 0;
end