Введение

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

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

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

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

Сценарий моделирует один объект, перемещающийся с постоянной скоростью в плоскости X-Y. Объект пересекает зоны покрытия трех равномерно расположенных датчиков с FOV 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. The 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