retroCorrect

Правильный фильтр с OOSM, использующим retrodiction

Описание

retroCorrect функция корректирует оценку состояния и ковариацию с помощью измерения из последовательности (OOSM). Чтобы использовать эту функцию, задайте MaxNumOOSMSteps свойство фильтра как положительное целое число. Перед использованием этой функции необходимо использовать retrodict функционируйте к успешно retrodict текущее состояние ко времени, в которое был взят OOSM.

[retroCorrState,retroCorrCov] = retroCorrect(filter,z) корректирует фильтр измерением OOSM z и возвращает откорректированную ковариацию состояния и состояния. Функция изменяет значения State и StateCovariance свойства объекта фильтра к retroCorrState и retroCorrCov, соответственно.

пример

___ = retroCorrect(___,measparams) задает параметры измерения для измерения z.

Внимание

Можно использовать этот синтаксис только когда заданный filter trackingEKF объект.

Примеры

свернуть все

Сгенерируйте траекторию истины с помощью 3-D постоянной скоростной модели.

rng(2021) % For repeatable results
initialState = [1; 0.4; 2; 0.3; 1; -0.2]; % [x; vx; y; vy; z; vz]
dt = 1; % Time step
steps = 10;
sigmaQ = 0.2; % Standard deviation for process noise
states = NaN(6,steps);
states(:,1) = initialState;
for ii = 2:steps
    w = sigmaQ*randn(3,1);
    states(:,ii) = constvel(states(:,ii-1),w,dt);
end

Сгенерируйте измерения положения от истин.

positionSelector = [1 0 0 0 0 0; 0 0 1 0 0 0; 0 0 0 0 1 0];
sigmaR = 0.2; % Standard deviation for measurement noise
positions = positionSelector*states;
measures = positions + sigmaR*randn(3,steps); 

Покажите истины и измерения в графике x-y.

figure
plot(positions(1,:),positions(2,:),"ro","DisplayName","Truths");
hold on;
plot(measures(1,:),measures(2,:),"bx","DisplayName","Measures");
xlabel("x (m)")
ylabel("y (m)")
legend("Location","northwest")

Примите, что на девятом шаге измерение задерживается и поэтому недоступное.

delayedMeasure = measures(:,9);
measures(:,9) = NaN;

Создайте расширенный фильтр Калмана (EKF) на основе постоянной скоростной модели.

estimates = NaN(6,steps);
covariances = NaN(6,6,steps);

estimates(:,1) = positionSelector'*measures(:,1);
covariances(:,:,1) = 1*eye(6);
filter = trackingEKF(@constvel,@cvmeas,...
    "State",estimates(:,1),...
    "StateCovariance",covariances(:,:,1),...
    "ProcessNoise",eye(6),...
    "MeasurementNoise",sigmaR^2*eye(3),...
    "MaxNumOOSMSteps",3);

Продвиньтесь через EKF с измерениями.

for ii = 2:steps
    predict(filter);
    if ~any(isnan(measures(:,ii))) % Skip if unavailable
        correct(filter,measures(:,ii));
    end
    estimates(:,ii) = filter.State;
    covariances(:,:,ii) = filter.StateCovariance;  
end

Покажите предполагаемые результаты.

plot(estimates(1,:),estimates(3,:),"gd","DisplayName","Estimates");

Retrodict к девятому шагу, и правильный текущие оценки при помощи измерений из последовательности на девятом шаге.

[retroState,retroCov] = retrodict(filter,-1);
[retroCorrState,retroCorrCov] = retroCorrect(filter,delayedMeasure);

Постройте состояние retrodicted для девятого шага.

plot([retroState(1);retroCorrState(1)],...
     [retroState(3),retroCorrState(3)],...
     "kd","DisplayName","Retrodicted")

Figure contains an axes object. The axes object contains 4 objects of type line. These objects represent Truths, Measures, Estimates, Retrodicted.

Можно использовать определитель ковариации конечного состояния, чтобы видеть улучшения, сделанные retrodiction. Меньший определитель ковариации указывает на улучшенные оценки состояния.

detWithoutRetrodiciton = det(covariances(:,:,end))
detWithoutRetrodiciton = 3.2694e-04
detWithRetrodiciton = det(retroCorrCov)
detWithRetrodiciton = 2.6063e-04

Входные параметры

свернуть все

Отслеживание объекта фильтра в виде trackingKF или trackingEKF объект.

Измерение из последовательности в виде P-by-1 вектор с действительным знаком, где P является размером измерения.

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

Поле Описание
Frame

Перечислимый тип, указывающий на систему координат раньше, сообщал об измерениях. Когда об обнаружениях сообщают с помощью системы прямоугольной координаты, Frame установлен в 'rectangular'. Когда об обнаружениях сообщают в сферических координатах, Frame установлен в 'spherical' для первой структуры.

OriginPosition

Смещение положения источника дочерней системы координат относительно родительской системы координат, представленной как вектор 3 на 1.

OriginVelocity

Скоростное смещение источника дочерней системы координат относительно родительской системы координат, представленной как вектор 3 на 1.

Orientation

3х3 ортонормированная матрица вращения системы координат с действительным знаком. Направление вращения зависит от IsParentTochild поле .

IsParentToChild

Логический скаляр, указывающий, ли Orientation выполняет вращение системы координат от системы координат координаты вышестоящего элемента до системы координат координаты нижестоящего элемента. Если false, Orientation выполняет вращение системы координат от системы координат координаты нижестоящего элемента до системы координат координаты вышестоящего элемента вместо этого.

HasElevation

Логический скаляр, указывающий, включает ли измерение вертикальное изменение. Для измерений, о которых сообщают в прямоугольной системе координат, если HasElevation false, об измерениях сообщают, принимая 0 степени вертикального изменения.

HasAzimuthЛогический скаляр, указывающий, включает ли измерение азимут.
HasRangeЛогический скаляр, указывающий, включает ли измерение область значений.
HasVelocity

Логический скаляр, указывающий, включают ли обнаружения, о которых сообщают, скоростные измерения. Для измерений, о которых сообщают в прямоугольной системе координат, если HasVelocity false, об измерениях сообщают как [x y z]. Если HasVelocity true, об измерениях сообщают как [x y z vx vy vz].

Выходные аргументы

свернуть все

Состояние, откорректированное retrodiction, возвращенным как M-by-1 вектор с действительным знаком, где M является размером состояния фильтра.

Ковариация состояния откорректирована retrodiction, возвращенным как M-by-M положительно-определенная матрица с действительным знаком.

Больше о

свернуть все

Retrodiction и Retro-Correction

Примите, что шагом текущего времени фильтра является k. Во время k, состоянием posteriori и ковариацией состояния фильтра является x (k |k) и P (k |k), соответственно. Измерение из последовательности (OOSM), взятое во время β теперь, прибывает во время k. Найдите l таким образом, что β является временным шагом между этими двумя последовательными временными шагами:

klβ<kl+1,

где l является положительным целым числом и l <k.

На шаге retrodiction, текущем состоянии и ковариации состояния во время k предсказаны назад ко времени OOSM. Можно получить состояние retrodicted путем распространения функции изменения состояния назад вовремя. Для линейной функции изменения состояния состояние retrodicted описывается как:

x(β|k)=F(β,k)x(k|k),

где F (β, k) является обратной матрицей переходов от временного шага k к временному шагу β. retrodicted ковариация получена как:

P(β|k)=F(β,k)[P(k|k)+Q(k,β)Pxv(β|k)PxvT(β|k)]F(β,k)T,

где Q (k, β) является ковариационной матрицей для шума процесса и,

Pxv=Q(k,β)P(k|kl)S*(k)1Q(k,β).

Здесь, P (k |k-l) является ковариацией состояния монастыря во время k, предсказанный от информации о ковариации во время k–l, и

S*(k)1=P(k|kl)1P(k|kl)1P(k|k)P(k|kl)1.

На втором шаге ретро коррекция, текущее состояние и ковариация состояния корректируются с помощью OOSM. Откорректированное состояние получено как:

x(k|β)=x(k|k)+W(k,β)[z(β)z(β|k)],

где z (β) является OOSM во время, β и W (k, β), усиление фильтра, описываются как:

W(k,β)=Pxz(β|k)[H(β)P(β|k)HT(β)+R(β)]1.

Можно получить эквивалентное измерение во время β на основе оценки состояния в то время k, z (β |k), как

z(β|k)=H(β)x(β|k).

В этих выражениях R (β) является ковариационной матрицей измерения для OOSM и:

Pxz(β|k)=[P(k|k)Pxv(β|k)]F(β,k)TH(β)T,

где H (β) является матрицей якобиана измерения. Откорректированная ковариация получена как:

P(k|β)=P(k|k)Pxz(β|k)S(β)1Pxz(β|k)T.

Ссылки

[1] Панель шалом, Y., Хуимин Чен и М. Маллик. “Решение с одним шагом для Многоступенчатого проблемы Измерения последовательности в Отслеживании”. Транзакции IEEE на Космических и Электронных системах 40, № 1 (январь 2004): 27–37. https://doi.org/10.1109/TAES.2004.1292140.

Расширенные возможности

Смотрите также

| |

Введенный в R2021b