Сопровождение маневренных целей

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

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

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

В этом примере определяется одна цель, которая первоначально движется с постоянной скоростью 200 м/с в течение 33 секунд, а затем входит в постоянный поворот 10 град/с. Поворот длится 33 секунды, затем мишень разгоняется по прямой при 3 м/с ^ 2.

[trueState, time, fig1] = helperGenerateTruthData;
dt = diff(time(1:2));
numSteps = numel(time);
figure(fig1)

Определите измерения как положение и добавьте к измерениям нормальный случайный шум со стандартным отклонением 1.

% Set the RNG seed for repeatable results
s = rng;
rng(2018);
positionSelector = [1 0 0 0 0 0;0 0 1 0 0 0;0 0 0 0 1 0]; % Position from state
truePos = positionSelector * trueState;
measNoise = randn(size(truePos));
measPos = truePos + measNoise;

Отслеживание с использованием фильтра постоянной скорости

Вы определяете trackingEKF с моделью движения с постоянной скоростью. Первое измерение используется для определения начального состояния и ковариации состояния, а также для определения шума процесса в виде неизвестного ускорения в компонентах x, y и z. Это определение аналогично тому, как функция initcvekf работает.

initialState = positionSelector' * measPos(:,1);
initialCovariance = diag([1,1e4,1,1e4,1,1e4]); % Velocity is not measured
cvekf = trackingEKF(@constvel, @cvmeas, initialState, ...
    'StateTransitionJacobianFcn', @constveljac, ...
    'MeasurementJacobianFcn', @cvmeasjac, ...
    'StateCovariance', initialCovariance, ...
    'HasAdditiveProcessNoise', false, ...
    'ProcessNoise', eye(3));

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

dist = zeros(1,numSteps);
estPos = zeros(3,numSteps);
for i = 2:size(measPos,2)
    predict(cvekf, dt);
    dist(i) = distance(cvekf,truePos(:,i)); % Distance from true position
    estPos(:,i) = positionSelector * correct(cvekf, measPos(:,i));
end
figure(fig1);
plot(estPos(1,:),estPos(2,:),'.g','DisplayName','CV Low PN')
title('True and Estimated Positions')
axis([5000 8000 -500 2500])

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

fig2 = figure;
hold on
plot((1:numSteps)*dt,dist,'g','DisplayName', 'CV Low PN')
title('Normalized Distance From Estimated Position to True Position')
xlabel('Time (s)')
ylabel('Normalized Distance')
legend

Увеличение шума процесса

Одним из возможных решений является увеличение шума процесса. Шум процесса представляет немодулированные члены в модели движения. Для модели постоянной скорости это неизвестные члены ускорения. Увеличивая шум процесса, вы допускаете большую неопределенность в модели движения, что заставляет фильтр больше полагаться на измерения, чем на модель. Следующие строки создают фильтр постоянной скорости с высокими значениями технологического шума, которые соответствуют примерно 5-G повороту.

cvekf2 = trackingEKF(@constvel, @cvmeas, initialState, ...
    'StateTransitionJacobianFcn', @constveljac, ...
    'MeasurementJacobianFcn', @cvmeasjac, ...
    'StateCovariance', initialCovariance, ...
    'HasAdditiveProcessNoise', false, ...
    'ProcessNoise', diag([50,50,1])); % Large uncertainty in the horizontal acceleration
dist = zeros(1,numSteps);
estPos = zeros(3,numSteps);
for i = 2:size(measPos,2)
    predict(cvekf2, dt);
    dist(i) = distance(cvekf2,truePos(:,i)); % Distance from true position
    estPos(:,i) = positionSelector * correct(cvekf2, measPos(:,i));
end
figure(fig1)
plot(estPos(1,:),estPos(2,:),'.c','DisplayName','CV High PN')
axis([5000 8000 -500 2500])

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

figure(fig2)
plot((1:numSteps)*dt,dist,'c','DisplayName', 'CV High PN')
axis([0 100 0 50])

Использование взаимодействующего фильтра модели движения

Другим решением является использование фильтра, который может рассматривать все модели движения одновременно, называемого взаимодействующим фильтром множественной модели (IMM). Фильтр IMM может поддерживать любое количество моделей движения, но обычно используется с 2-5 моделями движения. Для этого примера достаточно трёх моделей: модели постоянной скорости, модели постоянного поворота и модели постоянного ускорения.

imm = trackingIMM('TransitionProbabilities', 0.99); % The default IMM has all three models
% Initialize the state and state covariance in terms of the first model
initialize(imm, initialState, initialCovariance);

Фильтр IMM используется так же, как и EKF.

dist = zeros(1,numSteps);
estPos = zeros(3,numSteps);
modelProbs = zeros(3,numSteps);
modelProbs(:,1) = imm.ModelProbabilities;
for i = 2:size(measPos,2)
    predict(imm, dt);
    dist(i) = distance(imm,truePos(:,i)); % Distance from true position
    estPos(:,i) = positionSelector * correct(imm, measPos(:,i));
    modelProbs(:,i) = imm.ModelProbabilities;
end
figure(fig1)
plot(estPos(1,:),estPos(2,:),'.m','DisplayName','IMM')

trackingIMM фильтр способен отслеживать маневрирующую цель во всех трех частях движения.

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

figure(fig2)
hold on
plot((1:numSteps)*dt,dist,'m','DisplayName', 'IMM')
axis([0 100 0 50])

Чтобы лучше понять, как работает фильтр IMM, постройте график вероятностей модели как функции времени. На рисунке показано, что фильтр инициализируется тремя моделями, имеющими одинаковую вероятность. По мере обновления фильтра он быстро сходится к очень высокой вероятности того, что модель является моделью постоянной скорости. После 33 секунд движения модель постоянной скорости больше не верна, и вероятность модели постоянного поворота становится очень высокой на время поворота. На последнем участке движения во время манёвра постоянного ускорения фильтр IMM присваивает высокую вероятность того, что движение является постоянным ускорением, но фильтр менее определён относительно правильной модели движения, и существует около 0,3 вероятности движения с постоянной скоростью.

figure
plot((1:numSteps)*dt, modelProbs)
title('Model Probabilities vs. Time')
xlabel('Time (s)')
ylabel('Model Probabilities')
legend('IMM-CV','IMM-CA','IMM-CT')
% Return the RNG to its previous state
rng(s)

Резюме

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

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

helperGenerateTruthData

Эта функция генерирует траекторию истинности земли.

function [Xgt, tt, figs] = helperGenerateTruthData
% Generate ground truth
vx = 200; % m/s
omega = 10; % deg/s
acc = 3; % m/s/s
dt = 0.1;
tt = (0:dt:floor(1000*dt));
figs = [];

Xgt = NaN(9,numel(tt));
Xgt(:,1) = 0;

% Constant velocity
seg1 = floor(numel(tt)/3);
Xgt(2,1) = vx;
slct = eye(9);
slct(3:3:end,:) = [];
for m = 2:seg1
    X0 = slct*Xgt(:,m-1);
    X1 = constvel(X0, dt);
    X1 = slct'*X1;
    Xgt(:,m) = X1;
end

% Constant turn
seg2 = floor(2*numel(tt)/3);
slct = eye(9);
slct(3:3:end,:) = [];
for m = seg1+1:seg2
    X0 = slct*Xgt(:,m-1);
    X0 = [X0(1:4);omega];
    X1 = constturn(X0, dt);
    X1 = X1(1:4);
    X1 = [X1(1:2);0;X1(3:4);0;zeros(3,1)];
    Xgt(:,m) = X1;
end

% Constant acceleration
first = true;
for m = seg2+1:numel(tt)
    X0 = Xgt(:,m-1);
    if first
        vel = X0(2:3:end);
        ua = vel/norm(vel);
        va = acc*ua;
        X0(3:3:end) = va;
        first = false;
    end
    X1 = constacc(X0, dt);
    Xgt(:,m) = X1;
end

% Drop acceleration dimension
slct = eye(9);
slct(3:3:end,:) = [];
Xgt = slct*Xgt;

figs = [figs figure];
plot(Xgt(1,1:seg1),Xgt(3,1:seg1),'.-');
hold on;
plot(Xgt(1,seg1+1:seg2),Xgt(3,seg1+1:seg2),'.-');
plot(Xgt(1,seg2+1:end),Xgt(3,seg2+1:end),'.-');
grid on;
xlabel('X Position (m)');
ylabel('Y Position (m)');
title('True Position')
axis equal;
legend('Constant Velocity', 'Constant Turn', 'Constant Acceleration')
end

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