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

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

В этом примере вы задаете единую цель, которая первоначально перемещается в постоянной скорости 200 м/с в течение 33 секунд, затем входит в постоянный поворот 10 градусов/с. Поворот длится в течение 33 секунд, затем цель ускоряется в прямой линии в 3 m/s^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.