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