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

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

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

Задайте сценарий

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

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

Поддержка

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