Пассивное ранжирование с помощью одного датчика маневрирования

Открыть сценарий

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

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

Введение

Отсутствие измерений дальности от цели подразумевает неполную наблюдаемость состояния цели. На следующем рисунке показано, что измерения только под углом, полученные наблюдателем, движущимся с постоянной скоростью, приводят к множеству возможных траекторий (предполагаемой постоянной скорости) цели.

Теоретические результаты предполагают, что целевое состояние является необозримым до тех пор, пока не будут выполнены следующие условия [1].

Датчик должен маневрировать вне цели, т.е. движение датчика должно быть не менее чем на 1 порядок выше цели. Например, если цель движется с постоянной скоростью, наблюдатель должен иметь по меньшей мере постоянное ускорение.
Составляющая маневра датчика перпендикулярно линии визирования должна быть ненулевой.

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

Вспомогательная функция helperCreatePassiveRangingScenario используется для определения одиночного инфракрасного датчика, установленного на платформе. Чувствительная платформа, часто называемая собственным кораблем, движется с постоянной скоростью в начале, а затем выполняет маневр для наблюдения дальности цели. Предполагается, что цель не маневрирует и движется с постоянной скоростью по сценарию.

% Setup
exPath = fullfile(matlabroot,'examples','fusion','main');
addpath(exPath);
[scene,ownship,theaterDisplay] = helperCreatePassiveRangingScenario;
showScenario(theaterDisplay);

Отслеживание с использованием EKF в декартовых координатах

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

% Set random seed for reproducible results
rng(50);

% Create a |trackerGNN| to track the targets using the
% FilterInitializationFcn as @initCartesianEKF.
tracker = trackerGNN('FilterInitializationFcn',@initCartesianEKF,...
    'AssignmentThreshold',50,...
    'MaxNumTracks',5);

[tem, tam] = helperPassiveRangingErrorMetrics(ownship,false);

theaterDisplay.ErrorMetrics = tem;

tracks = [];

% Advance scenario, simulate detections and track
while advance(scene)
    % Get time information from tracking scenario.
    truths = platformPoses(scene);
    time = scene.SimulationTime;

    % Generate detections from the ownship
    detections = detect(ownship,time);

    % Pass detections to tracker
    if ~isempty(detections)
        tracks = tracker(detections,time);
    elseif isLocked(tracker)
        tracks = predictTracksToTime(tracker,'confirmed',time);
    end
    % Update error and assignment metrics
    tam(tracks,truths);
    [trackIDs, truthIDs] = currentAssignment(tam);
    tem(tracks,trackIDs,truths,truthIDs);

    % Update display
    theaterDisplay(tracks,detections,tracker);
end

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

showGrabs(theaterDisplay,1);

На следующем рисунке показана производительность фильтра после начала маневрирования датчика в сценарии. Графики оценки диапазона и скорости диапазона создаются с использованием errorbar и показать $\sigma$ границы (стандартное отклонение) оценки дорожки.

showGrabs(theaterDisplay,2);

Устойчивость ЭКФ в декартовых координатах

Отслеживание в декартовых координатах с использованием расширенного фильтра Калмана привлекательно из-за простоты формулирования проблемы. Динамика состояния представлена набором линейных уравнений, а нелинейности встроены с использованием двух (относительно) простых уравнений для азимута и отметки.

При этом поведение расширенного фильтра Калмана показано неустойчивым и часто может быть нестабильным. Это происходит потому, что состояния (положение и скорость) и ненаблюдаемый диапазон тесно связаны между собой. Когда дальность является ненаблюдаемой, то есть во время фазы постоянной скорости движения чувствительной платформы, фильтр «ложно» оценивает дальность цели в зависимости от истории измерения и соответствующих значений шума. Это может привести к преждевременному коллапсу ковариационного эллипса, что может привести к тому, что фильтру потребуется очень много времени, чтобы сойтись (или даже расходиться) [2], даже после выполнения достаточных условий наблюдаемости.

Используйте другое случайное начальное число для наблюдения преждевременной сходимости расширенного фильтра Калмана в декартовых координатах.

% set random seed
rng(2015);

% Reset the theaterDisplay to capture snapshots with new scenario.
release(theaterDisplay);
theaterDisplay.GrabFigureFcn = @(fig,scene)helperGrabPassiveRangingDisplay(fig,scene,'CartEKF2');
helperRunPassiveRangingSimulation(scene,theaterDisplay,@initCartesianEKF);

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

showGrabs(theaterDisplay,3);

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

showGrabs(theaterDisplay,4);

Отслеживание с использованием EKF в измененных сферических координатах (MSC-EKF)

Модифицированные сферические координаты (MSC) представляют собой стабильную систему координат для отслеживания с использованием измерений только под углом. Разделяя состояние на наблюдаемые и необозримые части, фильтр преодолевает ограничения, налагаемые EKF в декартовых координатах. Состояние в измененных сферических координатах определяется относительным образом, т.е. [цель - наблюдатель], и, следовательно, для прогнозирования состояния в будущем требуется ввод от наблюдателя. Это можно видеть в следующих уравнениях, где члены более высокого порядка относятся к движению наблюдателя, не захваченному моделью постоянной скорости.

$x = x_{t} - x_{o}$

$\dot{x} = \dot{x_{t}} - \dot{x_{o}}$

$\dot{x} = Ax_{t} - (Ax_{0} + \mathcal{O}(t^{2}) + \mathcal{O}(t^{3}) + ..)$

$\dot{x} = A(x_{t} - x_{0}) + \mbox{ higher} \mbox{ order} \mbox{ terms}$

% Set the same random seed to compare with the same detections
rng(2015);

% restart the scene
restart(scene);
release(theaterDisplay);
theaterDisplay.GrabFigureFcn = @(fig,scene)helperGrabPassiveRangingDisplay(fig,scene,'MSCEKF');

[tem, tam] = helperPassiveRangingErrorMetrics(ownship,true);
theaterDisplay.ErrorMetrics = tem;

% Create a tracker using |trackerGNN| and MSC-EKF filter initialization.
tracker = trackerGNN('FilterInitializationFcn',@initMSCEKF,...
    'AssignmentThreshold',50);

% The tracks carry a state which is non-linearly dependent on position.
% Inform the theaterDisplay that tracks have non-linear state and position
% can be extracted using a function_handle.
theaterDisplay.HasNonLinearState = true;
theaterDisplay.NonLinearTrackPositionFcn = @getTrackPositionsMSC;

% Initialization for MSC-EKF.
prevPose = pose(ownship,'true');
lastCorrectionTime = 0;
allTracks = [];

% Advance scenario, simulate detections and track
while advance(scene)

    time = scene.SimulationTime;
    truths = platformPoses(scene);

    % Generate detections from ownship
    detections = detect(ownship,time);

    % Update the input from the ownship i.e. it's maneuver since last
    % correction time.
    currentPose = pose(ownship,'true');
    dT = time - lastCorrectionTime;
    observerManeuver = calculateManeuver(currentPose,prevPose,dT);

    for i = 1:numel(allTracks)
        % Set the ObserverInput property using |setTrackFilterProperties|
        % function of the tracker
        setTrackFilterProperties(tracker,allTracks(i).TrackID,'ObserverInput',observerManeuver);
    end

    % Pass detections to tracker
    if ~isempty(detections)
        lastCorrectionTime = time;
        % Store the previous pose to calculate maneuver
        prevPose = currentPose;
        [tracks,~,allTracks] = tracker(detections,time);
    elseif isLocked(tracker)
        tracks = predictTracksToTime(tracker,'confirmed',time);
    end

    % Update error and assignment metrics
    tam(tracks,truths);
    [trackIDs, truthIDs] = currentAssignment(tam);
    tem(tracks,trackIDs,truths,truthIDs);

    % Update display
    theaterDisplay.NonLinearStateInput = currentPose.Position(:);
    theaterDisplay(detections,tracks,tracker);
end

MSC-EKF пытается поддерживать ковариацию в диапазоне до тех пор, пока датчик не совершит маневр. Это, по существу, связано с разъединенными наблюдаемыми и ненаблюдаемыми частями в государстве.

showGrabs(theaterDisplay,5);

Фильтр сходится ближе к истинным значениям быстрее, чем EKF в декартовых координатах, и $\sigma$ границы обеспечивают истинную, несмещенную оценку ошибки.

showGrabs(theaterDisplay,6);

Отслеживание с использованием параметризованного диапазона MSC-EKF

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

В этом разделе показано использование фильтра гауссовой суммы, trackingGSF, чтобы описать состояние с банком фильтров, каждый инициализирован в различных допущениях диапазона. Этот метод обычно называют параметризацией диапазона [3].

% Set random seed
rng(2015);

% restart the scene
restart(scene);
release(theaterDisplay);
theaterDisplay.GrabFigureFcn = @(fig,scene)helperGrabPassiveRangingDisplay(fig,scene,'MSCRPEKF');

% Create error and assignment metrics.
[tem, tam] = helperPassiveRangingErrorMetrics(ownship,true);
theaterDisplay.ErrorMetrics = tem;

% Create a tracker using |trackerGNN| and range-parameterized MSC-EKF
% filter initialization.
tracker = trackerGNN('FilterInitializationFcn',@initMSCRPEKF,...
    'AssignmentThreshold',50);

theaterDisplay.HasNonLinearState = true;
theaterDisplay.NonLinearTrackPositionFcn = @getTrackPositionsMSC;

% Initialization for MSC-RPEKF
prevPose = pose(ownship,'true');
lastCorrectionTime = 0;
allTracks = [];

% Advance scenario, simulate detections and track
while advance(scene)

    time = scene.SimulationTime;
    truths = platformPoses(scene);

    % Generate detections from ownship
    detections = detect(ownship,time);

    % Update the input from the ownship i.e. it's maneuver since last
    % correction time.
    currentPose = pose(ownship,'true');
    dT = time - lastCorrectionTime;
    observerManeuver = calculateManeuver(currentPose,prevPose,dT);

    % Get each filter from the trackingGSF property TrackingFilters using
    % the |getTrackFilterProperties| function of the tracker.
    for i = 1:numel(allTracks)
        trackingFilters = getTrackFilterProperties(tracker,allTracks(i).TrackID,'TrackingFilters');
        % Set the ObserverInput for each tracking filter
        for m = 1:numel(trackingFilters{1})
            trackingFilters{1}{m}.ObserverInput = observerManeuver;
        end
    end

    % Pass detections to tracker
    if ~isempty(detections)
        lastCorrectionTime = time;
        % Store the previous pose to calculate maneuver
        prevPose = currentPose;
        [tracks,~,allTracks] = tracker(detections,time);
    elseif isLocked(tracker)
        tracks = predictTracksToTime(tracker,'confirmed',time);
    end

    % Update error and assignment metrics
    tam(tracks,truths);
    [trackIDs, truthIDs] = currentAssignment(tam);
    tem(tracks,trackIDs,truths,truthIDs);

    % Update display
    theaterDisplay.NonLinearStateInput = currentPose.Position(:);
    theaterDisplay(detections,tracks,tracker);
end

На следующих рисунках показан параметризованный по диапазону фильтр после инициализации дорожки и характеристики отслеживания фильтра. Процесс параметризации диапазона позволяет каждому фильтру нести относительно небольшую ковариацию в диапазоне и, следовательно, менее подвержен проблемам линеаризации. Время схождения для параметризованного диапазона фильтра в этом сценарии аналогично MSC-EKF. Однако фильтр демонстрировал менее переходное поведение при переходе датчика во вторую основную фазу маневрирования траектории, т.е. от 35 до 40 секунд при моделировании. Обратите внимание на приведенный выше график оценки дальности MSC-EKF, который привел к ошибке 10 + км в дальности. Погрешность оценки составила около 5 км для параметризованного по дальности фильтра.

showGrabs(theaterDisplay,[7 8]);

Закрыть дисплеи

showGrabs(theaterDisplay,[]);

Сценарии с несколькими целевыми объектами

Продемонстрированный подход с использованием MSC-EKF и параметризованного диапазона MSC-EKF применим для более чем 1 цели. Для наблюдения за каждой целью датчик должен маневрировать вне каждой из них. Поскольку такие фильтры, как MSC-EKF, могут поддерживать ковариацию дальности на этапах отсутствия маневрирования, оценка каждого пути должна сходиться ближе к целям, когда датчик совершает маневры относительно них.

MSC-EKF

% Set random seed
rng(2015);

% Create a two-target scenario and theater display.
[sceneTwo,~,theaterDisplayTwo] = helperCreatePassiveRangingScenario(2);

% Use the helper function to run the simulation using @initMSCEKF.
helperRunPassiveRangingSimulation(sceneTwo,theaterDisplayTwo,@initMSCEKF);

% Show results
showGrabs(theaterDisplayTwo,1);

Диапазон-параметризованный MSC-EKF

% Set random seed
rng(2015);
release(theaterDisplayTwo);

% Run the simulation using range-parameterized MSC-EKF
helperRunPassiveRangingSimulation(sceneTwo,theaterDisplayTwo,@initMSCRPEKF);

% Show results
showGrabs(theaterDisplayTwo,2);

rmpath(exPath);

Резюме

Этот пример иллюстрирует проблемы, связанные с отслеживанием только под углом одного датчика, и демонстрирует, как использовать различные алгоритмы слежения для оценки дальности и дальности цели. Вы научились пользоваться initcvekf, initcvmscekf и trackingGSF для определения настраиваемого декартова-EKF, MSC-EKF и диапазона-параметризованного MSC-EKF для пассивного ранжирования. Вы также научились использовать определенные фильтры с GNN-трекером, trackerGNN.

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

Функции инициализации фильтра

В следующем разделе перечислены все FilterInitializationFcn используется для trackerGNN объект в этом примере.

initCartesianEKF Инициализация trackingEKF с помощью обнаружения только под углом

function filter = initCartesianEKF(detection)

% Create a full detection with high covariance in range estimate.
rangeEstimate = 5e4;
rangeCov = 16e8;
fullDetection = detection;
fullDetection.Measurement = [fullDetection.Measurement;rangeEstimate];
fullDetection.MeasurementNoise = blkdiag(fullDetection.MeasurementNoise,rangeCov);

% Update the MeasurementParameters to include range.
fullDetection.MeasurementParameters(1).HasRange = true;

% Use the initcvekf function to initialize a trackingEKF using the
% fullDetection.
fullFilter = initcvekf(fullDetection);

% |initcvekf| defines the StateCovariance in velocity with 100. This
% defines a standard deviation uncertainty in velocity as 10 m/s. Scale
% the velocity covariance with 400 i.e. an equivalent velocity standard
% deviation of 200 m/s
velCov = fullFilter.StateCovariance(2:2:end,2:2:end);
fullFilter.StateCovariance(2:2:end,2:2:end) = 400*velCov;

% fullFilter can only be corrected with [az el r] measurements.
% Create a |trackingEKF| using the State and StateCovariance from
% fullFilter.
filter = trackingEKF(@constvel,@cvmeas,fullFilter.State,...
    'StateCovariance',fullFilter.StateCovariance,...
    'StateTransitionJacobianFcn',@constveljac,...
    'MeasurementJacobianFcn',@cvmeasjac,...
    'HasAdditiveProcessNoise',false);

% Unit standard deviation acceleration noise
filter.ProcessNoise = eye(3);

end

initMSCEKF Инициализация MSC-EKF с помощью функции обнаружения только под углом

function filter = initMSCEKF(detection)
% Use the second input of the |initcvmscekf| function to provide an
% estimate of range and standard deviation in range.
rangeEstimate = 5e4;
rangeSigma = 4e4;
filter = initcvmscekf(detection,[rangeEstimate,rangeSigma]);
% The initcvmscekf assumes a velocity standard deviation of 10 m/s, which
% is linearly transformed into azimuth rate, elevation rate and vr/r.
% Scale the velocity covariance by 400 to specify that target can move 20
% times faster.
filter.StateCovariance(2:2:end,2:2:end) = 400*filter.StateCovariance(2:2:end,2:2:end);
filter.ProcessNoise = eye(3);
end

initMSCRPEKF Инициализация параметризованного диапазона MSC-EKF с помощью обнаружения только под углом

function filter = initMSCRPEKF(detection)
% A range-parameterized MSC-EKF can be defined using a Gaussian-sum filter
% (trackingGSF) containing multiple |trackingMSCEKF| as TrackingFilters.

% Range-parametrization constants
rMin = 3e4;
rMax = 8e4;
numFilters = 10;
rho = (rMax/rMin)^(1/numFilters);
Cr = 2*(rho - 1)/(rho + 1)/sqrt(12);
indFilters = cell(numFilters,1);
for i = 1:numFilters
    range = rMin/2*(rho^i + rho^(i-1));
    rangeSigma = Cr*range;
    % Use initcvmscekf function to create a trackingMSCEKF with provided
    % range and rangeSigma.
    indFilters{i} = initcvmscekf(detection,[range rangeSigma]);
    % Update the velocity covariance of each filter.
    indFilters{i}.StateCovariance(2:2:end,2:2:end) = 400*indFilters{i}.StateCovariance(2:2:end,2:2:end);
end
filter = trackingGSF(indFilters);

end

Служебные функции calculateManeuver

function maneuver = calculateManeuver(currentPose,prevPose,dT)
% Calculate maneuver i.e. 1st order+ motion of the observer. This is
% typically obtained using sensors operating at much higher rate.
v = prevPose.Velocity;
prevPos = prevPose.Position;
prevVel = prevPose.Velocity;
currentPos = currentPose.Position;
currentVel = currentPose.Velocity;

% position change apart from constant velocity motion
deltaP = currentPos - prevPos - v*dT;
% Velocity change
deltaV = currentVel - prevVel;

maneuver = zeros(6,1);
maneuver(1:2:end) = deltaP;
maneuver(2:2:end) = deltaV;
end

getTrackPositionsMSC

function [pos,cov] = getTrackPositionsMSC(tracks,observerPosition)

if isstruct(tracks) || isa(tracks,'objectTrack')
    % Track struct
    state = [tracks.State];
    stateCov = cat(3,tracks.StateCovariance);
elseif isa(tracks,'trackingMSCEKF')
    % Tracking Filter
    state = tracks.State;
    stateCov = tracks.StateCovariance;
end

% Get relative position using measurement function.
relPos = cvmeasmsc(state,'rectangular');

% Add observer position
pos = relPos + observerPosition;
pos = pos';

if nargout > 1
    cov = zeros(3,3,numel(tracks));

    for i = 1:numel(tracks)
        % Jacobian of position measurement
        jac = cvmeasmscjac(state(:,i),'rectangular');
        cov(:,:,i) = jac*stateCov(:,:,i)*jac';
    end
end

end

Ссылки

[1] Фогель, Эли и Мотти Гавиш. «Наблюдаемость цели динамики N-го порядка из измерений угла». Сделки IEEE по аэрокосмическим и электронным системам 24.3 (1988): 305-308.

[2] Айдала, Винсент и Шерри Хаммель. «Использование измененных полярных координат для отслеживания только подшипников». Транзакции IEEE по автоматическому управлению 28.3 (1983): 283-294.

[3] Персик, Н. «Отслеживание только подшипников с использованием набора расширенных фильтров Калмана с диапазоном параметров». IEE Proceedings-Control Theory and Applications 142.1 (1995): 73-80.

Документация