Отслеживание маневрирующих целей

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

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

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

В этом примере вы задаете одну цель, которая первоначально перемещается с постоянной скоростью 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')

The 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

Документация по Sensor Fusion and Tracking Toolbox

Поддержка

Для просмотра документации необходимо авторизоваться на сайте