Обратная оценка сглаженного состояния 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 свойство фильтра.
trackingIMM ФильтрСгенерируйте истину, используя присоединенную 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','Foward estimates') plot3(smoothState(1,:),smoothState(3,:),smoothState(5,:),'g:','DisplayName','Smooth estimates') axis image;
Постройте график оценочных ошибок как для предполагаемых состояний, так и для сглаженных состояний. Из результатов процесс сглаживания уменьшает ошибки расчета.
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 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 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')
[1] Надараджа, Н., Р. Тармараса, Майк Макдональд и Т. Кирубараджан. IMM Filtering and Backward Smooting for Maneuvering Target Tracking (неопр.) (недоступная ссылка). Транзакции IEEE по аэрокосмическим и электронным системам 48, № 3 (июль 2012 года): 2673-78. https://doi.org/10.1109/TAES.2012.6237617.