Пассивный диапазон с использованием одного датчика маневрирования

Открыть скрипт

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

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

Введение

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

Теоретические результаты подразумевают, что целевое состояние является ненаблюдаемым, пока не будут выполнены следующие условия [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] даже после достижения достаточных условий наблюдаемости.

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

% 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 в Декартовых координатах. Состояние в измененных сферических координатах определяется относительным образом, то есть [target - observer], и, следовательно, требуется вход от наблюдателя, чтобы предсказать состояние в будущем. Это можно увидеть в следующих уравнениях, где члены более высокого порядка относятся к движению наблюдателя, не полученному моделью постоянной скорости.

$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 Processions-Control Theory and Applications 142.1 (1995): 73-80.

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