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

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

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

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

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