Введение

В системе слежения, когда несколько датчиков сообщают тому же средству отслеживания, измерения могут прибыть в средство отслеживания с задержкой относительно времени, когда они сгенерированы от датчика. Задержка может быть вызвана любой из следующих причин:

Следующая фигура изображает общий случай, в котором измерение ожидается во время t, но получено во время t+k, где k является количеством обновлений фильтра. После того, как фильтр обновляет свое состояние во время t+k, прибывшее измерение со времени t является измерением из последовательности (OOSM) для фильтра. Количество шагов, k, называется задержкой.

Существует несколько методов в литературе по тому, как обработать OOSM:

Существует несколько способов реализовать retrodiction метод. Метод, который вы используете в этом примере, известен как алгоритм Bl1, где алгоритм B является аппроксимированным алгоритмом, и фильтр включает OOSM с задержкой l-шагов и 1 "гигантским прыжком" [1].

Инициализируйте фильтр и включите Retrodiction

В этом примере вы следуете примеру, показанному в разделе IX из [1]. Полагайте, что объект проходит ось X с почти постоянной скоростной моделью.

q = 0.5; % The power spectral density of the continuous time process noise.

Датчик измеряет и положение и скорость объекта вдоль оси X со следующей ковариационной матрицей измерения.

R = diag([1,0.1]);

Вы задаете это, объект следует за постоянной скоростью на скорости 10 м/с вдоль оси X.

dt = 1; % The time step, in seconds.
v = 10; % The speed along the x axis, in meters per second.

Следующий код инициализирует расширенный Фильтр Калмана 1D постоянной скорости, используемый в этом примере. Смотрите служебные функции oneDmotion, oneDmeas, oneDmotionJac, и oneDmeasJac если в конце этого скрипта.

ekf = trackingEKF(@oneDmotion, @oneDmeas, ...
    'StateTransitionJacobianFcn', @oneDmotionJac, ...
    'MeasurementJacobianFcn', @oneDmeasJac, ...
    'HasAdditiveProcessNoise', false, ...
    'ProcessNoise', q, ...
    'State', [0;10], ... % x=0, v=10
    'StateCovariance', R,...
    'MeasurementNoise', R);

Чтобы включить retrodiction, необходимо определить номер шагов OOSM с помощью MaxNumOOSMSteps свойство фильтра так, чтобы это подготовило историю фильтра, используемую retrodiction алгоритмом.

ekf.MaxNumOOSMSteps = 5;

Сравните OOSM обработка методов

В этом разделе вы сравниваете результаты повторной обработки, пренебрежения и retrodiction для задержки измерения с 1 задержкой.

Измерения в последовательности получены в такты 1, 2, 3, и 4. OOSM получен в такт 3.5, который находится в пределах первого интервала задержки между тактами 3 и 4.

t = [1, 2, 3, 4, 3.5];
x = v * t;
allStates = [x; repmat(v, 1, numel(t))];
allMeasurements = oneDmeasWithNoise(allStates, R);

Вы используете метод пренебрежения. Чтобы сделать это, вы запускаете фильтр только с измерениями в последовательности с тактов 1, 2, 3, и 4, и вы игнорируете OOSM во время 3.5.

neglectEKF = clone(ekf); % Clone the EKF to preserve its initial state
for i = 1:4
    predict(neglectEKF, dt); % Predict
    correct(neglectEKF, allMeasurements(:,i)); % Correct the filter
end

Чтобы сравнить различные методы, вы наблюдаете ковариацию состояния. Ковариация состояния представляет уровень неопределенности по поводу оценки состояния. Более высокие значения в ковариации состояния означают более высокую неопределенность или меньше уверенности об оценке состояния. Общий метод, чтобы сравнить величину значений в ковариации состояния при помощи трассировки или определителя матрицы. Вы используете трассировку здесь.

disp(neglectEKF.StateCovariance);

    0.3142    0.0370
    0.0370    0.0834

disp(trace(neglectEKF.StateCovariance));

    0.3976

Для метода повторной обработки вы используете OOSM в такт 3.5, как будто это было дано в правильном порядке с остальной частью измерений в последовательности.

reprocessingEKF = clone(ekf); % Clone the EKF to preserve its initial state
indices = [1 2 3 5 4]; % Reorder the measurements
for i = 1:numel(indices)
    if i <= 3 % Before t=3
        dt = 1;
    else % For 3 -> 3.5 and 3.5 -> 4
        dt = 0.5;
    end
    predict(reprocessingEKF, dt);
    correct(reprocessingEKF, allMeasurements(:,indices(i)));
end
disp(reprocessingEKF.StateCovariance);

    0.2287    0.0225
    0.0225    0.0759

disp(trace(reprocessingEKF.StateCovariance));

    0.3046

Вы замечаете, что метод повторной обработки обеспечивает намного меньшую ковариацию состояния, что означает более определенную оценку состояния. Результат ожидается, потому что OOSM в t=3.5 был повторно обработан в правильной последовательности и новой информации, которую это содержит, помог уменьшать неопределенность.

Вы теперь используете retrodiction метод. Во-первых, вы обрабатываете все измерения в последовательности. Затем вы retrodict фильтр ко времени OOSM и затем ретро правильный фильтр с OOSM.

retroEKF = clone(ekf); % Clone the EKF to preserve its initial state
dt = 1;
for i = 1:4
    predict(retroEKF, dt); % Predict
    correct(retroEKF, allMeasurements(:,i)); % Correct the filter
end
retrodict(retroEKF,-0.5); % Retrodict from t=4 to t=3.5
retroCorrect(retroEKF, allMeasurements(:,5)); % The measurement at t=3.5
disp(retroEKF.StateCovariance);

    0.2330    0.0254
    0.0254    0.0779

disp(trace(retroEKF.StateCovariance));

    0.3109

Как ожидалось retrodiction метод обеспечивает ковариационную матрицу состояния, которая является о той же величине, как тот получил использование идеального метода повторной обработки. Матричная трассировка для retrodiction метода только на 2% больше трассировки ковариации состояния повторной обработки. Это значительно меньше, чем трассировка ковариации состояния, полученная при помощи метода пренебрежения.

Сравните результаты для различных значений задержки

Чтобы изучить удар задержки на обработке OOSM, вы задаете четыре уровня задержки от 1 шага до задержки с 4 шагами. Эти задержки соответствуют генерации OOSM время от времени 3.5, 2.5, 1.5, и 0.5, соответственно.

Вы организуете результаты в табличной форме как показано в коде ниже.

for lag = 1:4
    timestamps = [0, 1, 2, 3, 4, 4.5-lag];
    allStates = [v*timestamps, repmat(v, 1, numel(timestamps))];
    allMeasurements = oneDmeasWithNoise(allStates, R);
    oneLagStruct(lag) = runOneLagValue(ekf, allMeasurements, timestamps); %#ok<SAGROW> 
end
displayTable(oneLagStruct)

   Lag        Neglect          Reprocessing          Retrodiction         
════════════════════════════════════════════════════════════════════════
    1    ⎡ 0.3142 0.0370 ⎤     ⎡ 0.2287 0.0225 ⎤     ⎡ 0.2330 0.0254 ⎤ 
         ⎣ 0.0370 0.0834 ⎦     ⎣ 0.0225 0.0759 ⎦     ⎣ 0.0254 0.0779 ⎦
────────────────────────────────────────────────────────────────────────
    2    ⎡ 0.3142 0.0370 ⎤     ⎡ 0.2597 0.0381 ⎤     ⎡ 0.2667 0.0389 ⎤ 
         ⎣ 0.0370 0.0834 ⎦     ⎣ 0.0381 0.0832 ⎦     ⎣ 0.0389 0.0830 ⎦
────────────────────────────────────────────────────────────────────────
    3    ⎡ 0.3142 0.0370 ⎤     ⎡ 0.2854 0.0387 ⎤     ⎡ 0.2955 0.0403 ⎤ 
         ⎣ 0.0370 0.0834 ⎦     ⎣ 0.0387 0.0833 ⎦     ⎣ 0.0403 0.0828 ⎦
────────────────────────────────────────────────────────────────────────
    4    ⎡ 0.3142 0.0370 ⎤     ⎡ 0.2983 0.0381 ⎤     ⎡ 0.3070 0.0393 ⎤ 
         ⎣ 0.0370 0.0834 ⎦     ⎣ 0.0381 0.0833 ⎦     ⎣ 0.0393 0.0826 ⎦
────────────────────────────────────────────────────────────────────────

Первые три строки в приведенной выше таблице равны результатам, показанным в Таблице I в [1] для этих трех методов. Вы делаете следующие наблюдения:

Сводные данные

Этот пример ввел тему измерений из последовательности, часто известных сокращением OOSM. Пример показал три общих метода обработки OOSM на уровне фильтра: пренебрежение его, повторная обработка его со всеми измерениями, сохраненными в буфере или retrodicting фильтр и представление OOSM, чтобы улучшить текущую оценку. Из этих трех методов пренебрежение является самым эффективным в памяти и обработке, но обеспечивает худшую оценку состояния. Метод повторной обработки является самым дорогим в терминах памяти и обработки, но обеспечивает самый точный результат. retrodiction метод является хорошим компромиссом обработки и памяти по сравнению с точностью. Вы также видели, что максимальное количество шагов OOSM должно быть ограничено 3 или 4, потому что преимущество представления OOSM становится меньшим, когда количество шагов увеличивается.

Ссылки

[1] Панель шалом Яакова, Хуимин Чен и Махендра Маллик, "Решение с одним шагом для многоступенчатого проблемы измерения последовательности в отслеживании", транзакции IEEE на космических и электронных системах, издании 40, № 1, январь 2004.

Вспомогательные Функции

oneDmotion

1D постоянная функция изменения состояния скорости.

function state = oneDmotion(state, ~, dt)
state = [1 dt;0 1]*state;
end

oneDmotionJac

1D постоянное скоростное изменение состояния функционирует якобиан. Это обеспечивает шум процесса, используемый в [1].

function [dfdx,dfdv] = oneDmotionJac(~, ~, dt)
dfdx = [1 dt;0 1];
dfdv = chol([dt^3/3 dt^2/2; dt^2/2 dt],'lower');
end

oneDmeas

1D постоянная функция измерений скорости. Это обеспечивает измерение включая положение и скорость без шума.

function z = oneDmeas(state)
z = state;
end

oneDmeasJac

1D постоянные скоростные измерения функционируют якобиан.

function H = oneDmeasJac(state)
H = eye(size(state,1));
end

oneDmeasWithNoise

1D постоянная функция измерений скорости. Это предоставляет измерению включая положение и скорость с Гауссовым шумом и ковариацией R.

function z = oneDmeasWithNoise(state,R)
z = state + R * randn(size(R,1), size(state,2));
end

runNeglect

Запускает метод пренебрежения для различных значений задержек.

function [x,P] = runNeglect(ekf, allMeasurements, timestamps)
neglectEKF = clone(ekf); % Clone the EKF to preserve its initial state
dt = diff(timestamps);
for i = 1:numel(dt)-1
    predict(neglectEKF, dt(i)); % Predict
    correct(neglectEKF, allMeasurements(:,i)); % Correct the filter
end
x = neglectEKF.State;
P = neglectEKF.StateCovariance;
end

runReprocessing

Запускает метод повторной обработки для различных значений задержек.

function [x, P] = runReprocessing(ekf, allMeasurements, timestamps)
reprocessingEKF = clone(ekf); % Clone the EKF to preserve its initial state
[timestamps, indices] = sort(timestamps); % Reorder the timestamps
allMeasurements = allMeasurements(:, indices(2:end)-1); % Reorder the measurements
dt = diff(timestamps);
for i = 1:numel(dt)
    predict(reprocessingEKF, dt(i));
    correct(reprocessingEKF, allMeasurements(:,i));
end
x = reprocessingEKF.State;
P = reprocessingEKF.StateCovariance;
end

runRetrodiction

Запускает retrodiction метод для различных значений задержек.

function [x, P] = runRetrodiction(ekf, allMeasurements, timestamps)
retrodictionEKF = clone(ekf); % Clone the EKF to preserve its initial state
dt = diff(timestamps);
for i = 1:numel(dt)-1
    predict(retrodictionEKF, dt(i));
    correct(retrodictionEKF, allMeasurements(:,i));
end
retrodict(retrodictionEKF, dt(end));
retroCorrect(retrodictionEKF, allMeasurements(:,end));
x = retrodictionEKF.State;
P = retrodictionEKF.StateCovariance;
end

runOneLagValue

Запуски и собирают результаты этих трех методов: пропустите, повторная обработка и retrodiction.

function oneLagStruct = runOneLagValue(ekf, allMeasurements, timestamps)
oneLagStruct = struct('Lag',ceil(timestamps(end-1)-timestamps(end)),...
    'Neglect',zeros(2,2),...
    'Reprocessing',zeros(2,2),...
    'Retrodiction',zeros(2,2));

% Neglect the OOSM
[~, P] = runNeglect(ekf, allMeasurements, timestamps);
oneLagStruct.Neglect = P;

% Reprocess all the measurements according to time
[~, P] = runReprocessing(ekf, allMeasurements, timestamps);
oneLagStruct.Reprocessing = P;
% 
% Use retrodiction 
[~, P] = runRetrodiction(ekf, allMeasurements, timestamps);
oneLagStruct.Retrodiction = P;
end

displayTable

Отображает результаты в легком, чтобы считать табличную форму.

function displayTable(t)
varNames = fieldnames(t);
fprintf('<strong>%6s  </strong>', 'Lag');
fprintf('<strong>%13s         </strong>', string(varNames(2:4)));
fprintf('\n════════════════════════════════════════════════════════════════════════\n');
for i = 1:numel(t)
    fprintf('    %d', t(i).Lag);
    for j = 2:numel(varNames)
        fprintf('    %c %1.4f %1.4f %c ', 9121, t(i).(varNames{j})(1,1:2), 9124);
    end
    fprintf('\n    ');
    for j = 2:numel(varNames)
        fprintf('     %c %1.4f %1.4f %c', 9123, t(i).(varNames{j})(2,1:2), 9126);
    end
    fprintf('\n');
    fprintf('────────────────────────────────────────────────────────────────────────\n');
end
end