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