Оценки плавного состояния фильтра слежения в обратном направлении
[ выполняет обратную рекурсию для получения сглаженных состояний и ковариаций на предыдущих этапах для фильтра слежения, smoothX,smoothP] = smooth(filter)filter. Функция определяет количество обратных шагов на основе количества выполненных прямых шагов F и максимального количества обратных шагов MB, заданного MaxNumSmoothingSteps имущества filter. Если F < MB, число обратных шагов равно F-1. В противном случае число обратных шагов равно МБ.
Количество шагов переадресации равно количеству вызовов predict объектная функция filter. Обратные шаги не включают текущий временной шаг фильтра.
[ определяет количество шагов обратного сглаживания smoothX,smoothP] = smooth(filter,numBackSteps)numBackSteps. Значение numBackSteps должно быть меньше или равно меньшему из F-1 и MB, где F - количество выполненных прямых шагов, а MB - максимальное количество обратных шагов, заданное MaxNumSmoothingSteps имущества filter.
trackingEKFСоздайте истинную траекторию на основе модели движения с постоянным поворотом и создайте измерения положения 2-D.
rng(2020); % For repeatable results % Initialization dt = 1; simTime = 50; tspan = 0:dt:simTime; trueInitialState = [0; 1; 0; 1; 5]; % [x;vx;y;vy;omega] processNoise = diag([0.5; 0.5; 0.1]); % process noise matrix measureNoise = diag([4 4 1]); % measurement noise matrix numSteps = length(tspan); trueStates = NaN(5,numSteps); trueStates(:,1) = trueInitialState; % Propagate the constant turn model and generate the measurements with % noise. for i = 2:length(tspan) trueStates(:,i) = constturn(trueStates(:,i-1),chol(processNoise)*randn(3,1),dt); end measurements = ctmeas(trueStates) + chol(measureNoise)*randn(3,numSteps);
Постройте график истинной траектории и измерений.
figure plot(trueStates(1,1),trueStates(3,1),'r*','HandleVisibility','off') hold on plot(trueStates(1,:),trueStates(3,:),'r','DisplayName','Truth') plot(measurements(1,:),measurements(2,:),'ko','DisplayName','Measurements') xlabel('x (m)') ylabel('y (m)') axis image
Создать trackingEKF фильтрация объекта на основе модели движения с постоянным поворотом.
initialGuess = [measurements(1,1);- 1; measurements(2,1); -1; 0]; filter = trackingEKF(@constturn,@ctmeas,initialGuess, ... 'StateCovariance', diag([1,1,1,1,10]), ... 'StateTransitionJacobianFcn',@constturnjac, ... 'MeasurementNoise',measureNoise, ... 'MeasurementJacobianFcn',@ctmeasjac, ... 'EnableSmoothing',true, ... 'MaxNumSmoothingSteps',numSteps); estimateStates = NaN(size(trueStates)); estimateStates(:,1) = filter.State;
Распространите фильтр и обновите оценочное состояние с помощью измерений.
for i=2:length(tspan) predict(filter,dt) estimateStates(:,i) = correct(filter,measurements(:,i)); end
Визуализация расчетных результатов.
plot(estimateStates(1,:),estimateStates(3,:),'b','DisplayName','Forward filtering')
Выровняйте предполагаемые состояния назад.
smoothStates = smooth(filter);
Визуализируйте сглаженную траекторию.
plot(smoothStates(1,:),smoothStates(3,:),'g','DisplayName','Backward smoothing') legend('Location','best')
Получение ошибок оценки.
forwardError = abs(estimateStates - trueStates); smoothError = abs(smoothStates - trueStates(:,1:end-1)); rmsForward = sqrt(mean(forwardError'.^2))
rmsForward = 1×5
1.6492 1.2326 1.6138 1.1619 5.0195
rmsSmooth = sqrt(mean(smoothError'.^2))
rmsSmooth = 1×5
0.9201 0.6587 1.2122 0.6139 2.2426
Визуализация ошибок оценки. Из результатов процесс сглаживания уменьшает ошибки оценки.
figure subplot(2,1,1) plot(tspan,forwardError(1,:),'b') hold on; plot(tspan(1:end-1),smoothError(1,:),'g') title('Position Errors') legend('Forward filtering','Backward smoothing') ylabel('x error (m)') subplot(2,1,2) plot(tspan,forwardError(3,:),'b') hold on plot(tspan(1:end-1),smoothError(3,:),'g') xlabel('time (sec)') ylabel('y error (m)')
figure subplot(3,1,1) plot(tspan,forwardError(2,:),'b') hold on; plot(tspan(1:end-1),smoothError(2,:),'g') title('Velocity Errors') legend('Forward filtering','Backward smoothing') ylabel('v_x error (m/s)') subplot(3,1,2) plot(tspan,forwardError(4,:),'b') hold on; plot(tspan(1:end-1),smoothError(4,:),'g') xlabel('time (sec)') ylabel('v_y error (m/s)') subplot(3,1,3) plot(tspan,forwardError(5,:),'b') hold on; plot(tspan(1:end-1),smoothError(5,:),'g') xlabel('time (sec)') ylabel('\omega-error (deg/s)')
trackingEKFСоздайте истинную траекторию на основе модели движения с постоянным поворотом и создайте измерения положения 2-D.
rng(2020); % For repeatable results % Initialization dt = 1; simTime = 50; tspan = 0:dt:simTime; trueInitialState = [0; 1; 0; 1; 5]; % [x;vx;y;vy;omega] processNoise = diag([0.5; 0.5; 0.1]); % process noise matrix measureNoise = diag([4 4 1]); % measurement noise matrix numSteps = length(tspan); trueStates = NaN(5,numSteps); trueStates(:,1) = trueInitialState; % Propagate the constant turn model and generate the measurements with % noise. for i = 2:length(tspan) trueStates(:,i) = constturn(trueStates(:,i-1),chol(processNoise)*randn(3,1),dt); end measurements = ctmeas(trueStates) + chol(measureNoise)*randn(3,numSteps);
Постройте график истинной траектории и измерений.
figure plot(trueStates(1,1),trueStates(3,1),'r*','HandleVisibility','off') hold on; plot(trueStates(1,:),trueStates(3,:),'r','DisplayName','Truth') plot(measurements(1,:),measurements(2,:),'ko','DisplayName','Measurements') xlabel('x (m)') ylabel('y (m)') axis image;
Создать trackingEKF фильтрация объекта на основе модели движения с постоянным поворотом. Установите интервал сглаживания в три шага.
initialGuess = [measurements(1,1);-1;measurements(2,1);-1;0]; filter = trackingEKF(@constturn,@ctmeas,initialGuess,... 'StateCovariance', diag([1,1,1,1,10]),... 'StateTransitionJacobianFcn',@constturnjac,... 'MeasurementNoise', measureNoise,... 'MeasurementJacobianFcn',@ctmeasjac,... 'EnableSmoothing',true,... 'MaxNumSmoothingSteps',4); estimateStates = NaN(size(trueStates)); estimateStates(:,1) = filter.State; stepLag = 3; % Smoothing lag steps smoothStates = NaN(5,numSteps-stepLag);
Распространите фильтр и обновите оценочное состояние с помощью измерений.
for i = 2:length(tspan) predict(filter,dt); estimateStates(:,i) = correct(filter,measurements(:,i)); if i > 3 smoothSegment = smooth(filter,stepLag); smoothStates(:,i-3) = smoothSegment(:,1); end end
Визуализируйте оцененные вперед и сглаженные траектории.
plot(estimateStates(1,:),estimateStates(3,:),'b','DisplayName','Forward filtering') plot(smoothStates(1,:),smoothStates(3,:),'g','DisplayName','Backward smoothing') legend('Location','best')
Получение ошибок оценки.
forwardError = abs(estimateStates - trueStates); smoothError = abs(smoothStates - trueStates(:,1:end-stepLag)); rmsForward = sqrt(mean(forwardError'.^2))
rmsForward = 1×5
1.6492 1.2326 1.6138 1.1619 5.0195
Визуализация ошибок оценки. Из результатов процесс сглаживания уменьшает ошибки оценки.
figure subplot(2,1,1) plot(tspan,forwardError(1,:),'b') hold on plot(tspan(1:end-stepLag),smoothError(1,:),'g') title('Position Errors') legend('Forward filtering','Backward smoothing') ylabel('x error (m)') subplot(2,1,2) plot(tspan,forwardError(3,:),'b') hold on plot(tspan(1:end-stepLag),smoothError(3,:),'g') xlabel('time (sec)') ylabel('y error (m)')
figure() subplot(3,1,1) plot(tspan,forwardError(2,:),'b') hold on; plot(tspan(1:end-stepLag),smoothError(2,:),'g') title('Velocity Errors') legend('Forward filtering','Backward smoothing') ylabel('v_x error (m/s)') subplot(3,1,2) plot(tspan,forwardError(4,:),'b') hold on; plot(tspan(1:end-stepLag),smoothError(4,:),'g') xlabel('time (sec)') ylabel('v_y error (m/s)') subplot(3,1,3) plot(tspan,forwardError(5,:),'b') hold on; plot(tspan(1:end-stepLag),smoothError(5,:),'g') xlabel('time (sec)') ylabel('\omega-error (deg/s)')
[1] СярккЯ, Симо. «Незараженный Рауч - Тунг - Штрибель более гладкий». IEEE Transactions on Automatic Control, 53, No 3 (апрель 2008): 845-49. https://doi.org/10.1109/TAC.2008.919531.
[2] Раух, Х. Э., Ф. Тунг и К. Т. Штрибель. «Оценки максимального правдоподобия линейных динамических систем». AIAA Journal 3, № 8 (август 1965): 1445-50. https://doi.org/10.2514/3.3166.