smooth

Назад сглаживайте оценки состояния trackingIMM фильтр

Описание

пример

[smoothX,smoothP,modelProbability] = smooth(imm) запускает обратную рекурсию, чтобы получить сглаживавшие состояния, ковариации и вероятности модели на предыдущих шагах для trackingIMM фильтр, imm. Функция определяет количество обратных шагов на основе количества выполняемых прямых шагов F и максимальное количество обратных шагов MB, заданный MaxNumSmoothingSteps свойство filter. Если F <MB, количеством обратных шагов является F – 1. В противном случае количеством обратных шагов является MB.

Количество прямых шагов равно количеству вызовов predict объектная функция фильтра. Обратные шаги не включают шаг текущего времени фильтра.

[smoothX,smoothP,modelProbability] = smooth(imm,numBackSteps) задает количество обратных шагов сглаживания numBackSteps. Значение numBackSteps должно быть меньше чем или равно меньшему из F – 1 и MB, где F является количеством выполняемых прямых шагов, и MB является максимальным количеством обратных шагов, заданных MaxNumSmoothingSteps свойство фильтра.

Примеры

свернуть все

Сгенерируйте истину с помощью присоединенного helperGenerateTruth функция. Траектория истины состоит из постоянной скорости, постоянного ускорения и постоянных сегментов траектории поворота.

n = 1000;
[trueState, time, fig1] = helperGenerateTruth(n);
dt = diff(time(1:2));
numSteps = numel(time);

Настройте начальные условия для симуляции.

rng(2021); % for repeatable results
positionSelector = [1 0 0 0 0 0;
                    0 0 1 0 0 0;
                    0 0 0 0 1 0]; % Select position from 6-dimenstional state [x;vx;y;vy;z;vz]            
truePos = positionSelector*trueState;
sigma = 10; % Measurement noise standard deviation
measNoise = sigma* randn(size(truePos));
measPos = truePos + measNoise;
initialState = positionSelector'*measPos(:,1);
initialCovariance = diag([1 1e4 1 1e4 1 1e4]); % Velocity is not measured

Создайте фильтр IMM на основе трех фильтров EKF и инициализируйте фильтр IMM.

detection = objectDetection(0,[0; 0; 0],'MeasurementNoise',sigma^2 * eye(3));
f1 = initcvekf(detection); % constant velocity EKF
f2 = initcaekf(detection); % constant acceleration EKF
f2.ProcessNoise = 3*eye(3);
f3 = initctekf(detection); % Constant turn EKF
f3.ProcessNoise(3,3) = 100;
imm = trackingIMM({f1; f2; f3},'TransitionProbabilities',0.99, ...
    'ModelConversionFcn',@switchimm, ...
    'EnableSmoothing',true, ...
    'MaxNumSmoothingSteps',size(measPos,2)-1);
initialize(imm, initialState, initialCovariance);

Предварительно выделите переменные для фильтра выходные параметры.

estState = zeros(6,numSteps);
modelProbs = zeros(numel(imm.TrackingFilters),numSteps);
modelProbs(:,1) = imm.ModelProbabilities;

Запустите фильтр.

for i = 2:size(measPos,2)
    predict(imm,dt);
    estState(:,i) = correct(imm,measPos(:,i));
    modelProbs(:,i) = imm.ModelProbabilities;
end

Сглаживайте фильтр.

[smoothState,~,smoothProbs] = smooth(imm);

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

fig1
fig1 = 
  Figure (1) with properties:

      Number: 1
        Name: ''
       Color: [1 1 1]
    Position: [360 502 560 420]
       Units: 'pixels'

  Show all properties

hold on;
plot3(estState(1,:),estState(3,:),estState(5,:),'k:','DisplayName','Forward estimates')
plot3(smoothState(1,:),smoothState(3,:),smoothState(5,:),'g:','DisplayName','Smooth estimates')
axis image;

Figure contains an axes object. The axes object contains 5 objects of type line. These objects represent Truth: Constant Velocity, Truth: Constant Turn, Truth: Constant Acceleration, Forward estimates, Smooth estimates.

Постройте оценочные ошибки и для прямых предполагаемых состояний и для сглаживавших состояний. От результатов процесс сглаживания уменьшает ошибки расчета.

figure;
errorTruth = abs(estState - trueState);
errorSmooth = abs(smoothState - trueState(:,1:end-1));

subplot(3,1,1)
plot(time,errorTruth(1,:),'k')
hold on
plot(time(1:end-1),errorSmooth(1,:),'g')
ylabel('x-error (m)')
legend('Forward estimate error','Smooth error')
title('Absolute Error of Forward and Smooth Estimates')

subplot(3,1,2)
plot(time,errorTruth(2,:),'k')
hold on;
plot(time(1:end-1),errorSmooth(2,:),'g')
ylim([0 30])
ylabel('y-error (m)')

subplot(3,1,3)
plot(time,errorTruth(3,:),'k')
hold on
plot(time(1:end-1),errorSmooth(3,:),'g')
xlabel('time (sec)')
ylabel('z-error (m)')

Figure contains 3 axes objects. Axes object 1 with title Absolute Error of Forward and Smooth Estimates contains 2 objects of type line. These objects represent Forward estimate error, Smooth error. Axes object 2 contains 2 objects of type line. Axes object 3 contains 2 objects of type line.

Покажите вероятности модели на основе прямой фильтрации и обратного сглаживания.

figure
plot(time,modelProbs(1,:))
hold on
plot(time,modelProbs(2,:))
plot(time,modelProbs(3,:))
xlabel('Time (sec)')
title('Model Probabilities in Forward Filtering')
legend('Constant velocity','Constant turn','Constant acceleration')

Figure contains an axes object. The axes object with title Model Probabilities in Forward Filtering contains 3 objects of type line. These objects represent Constant velocity, Constant turn, Constant acceleration.

figure
plot(time(1:end-1),smoothProbs(1,:))
hold on
plot(time(1:end-1),smoothProbs(2,:))
plot(time(1:end-1),smoothProbs(3,:))
xlabel('Time (sec)');
title('Model Probabilities in Backward Smoothing')
legend('Constant velocity','Constant turn','Constant acceleration')

Figure contains an axes object. The axes object with title Model Probabilities in Backward Smoothing contains 3 objects of type line. These objects represent Constant velocity, Constant turn, Constant acceleration.

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

свернуть все

Отфильтруйте для отслеживания объекта в виде trackingIMM объект.

Количество обратных шагов в виде положительного целого числа. Значение должно быть меньше чем или равно меньшему из F – 1 и MB, где F является количеством выполняемых прямых шагов, и MB является максимальным количеством обратных шагов, заданных MaxNumSmoothingSteps свойство фильтра, imm.

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

свернуть все

Сглаживавшие состояния, возвращенные как N-by-K матрица. N является размерностью состояния, и K является количеством обратных шагов. Первый столбец представляет состояние в конце обратной рекурсии, которая является самым ранним состоянием во временном интервале сглаживания. Последний столбец представляет состояние в начале обратной рекурсии, которая является последним состоянием во временном интервале сглаживания.

Типы данных: single | double

Сглаживавшие ковариации, возвращенные как N-by-N-by-K массив. N является размерностью состояния, и K является количеством обратных шагов. Каждая страница (N-by-N матрица) массива является сглаживавшей ковариационной матрицей для соответствующего сглаживавшего состояния в smoothX вывод .

Типы данных: single | double

Вероятность каждой модели, возвращенной как K-by-M массив неотрицательных скаляров. K является количеством обратных шагов, и M является количеством моделей, используемых в фильтре.

Типы данных: single | double

Ссылки

[1] Nadarajah, N., Р. Тармараса, Майк Макдональд и Т. Кирубараджэн. “IMM, Вперед Фильтрующий и Обратное Сглаживание для Маневрирования Целевого Отслеживания”. Транзакции IEEE на Космических и Электронных системах 48, № 3 (июль 2012): 2673–78. https://doi.org/10.1109/TAES.2012.6237617.

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

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

Введенный в R2021a
Для просмотра документации необходимо авторизоваться на сайте