Назад сглаживайте оценки состояния отслеживания фильтра
[ запускает обратную рекурсию, чтобы получить сглаживавшие состояния и ковариации на предыдущих шагах для фильтра отслеживания, smoothX,smoothP] = smooth(filter)filter. Функция определяет количество обратных шагов на основе количества выполняемых прямых шагов F и максимальное количество обратных шагов MB, заданный MaxNumSmoothingSteps свойство filter. Если F <MB, количеством обратных шагов является F – 1. В противном случае количеством обратных шагов является MB.
Количество прямых шагов равно количеству вызовов predict объектная функция filter. Обратные шаги не включают шаг текущего времени фильтра.
[ задает количество обратных шагов сглаживания smoothX,smoothP] = smooth(filter,numBackSteps)numBackSteps. Значение numBackSteps должно быть меньше чем или равно меньшему из F – 1 и MB, где F является количеством выполняемых прямых шагов, и MB является максимальным количеством обратных шагов, заданных MaxNumSmoothingSteps свойство filter.
trackingEKFСоздайте траекторию истины на основе постоянной модели движения поворота и сгенерируйте 2D измерения положения.
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Создайте траекторию истины на основе постоянной модели движения поворота и сгенерируйте 2D измерения положения.
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] SÄrkkÄ, Simo. “Недушистый Раух - Тунговый - Более сглаженный Striebel”. Транзакции IEEE на Автоматическом управлении, 53, № 3 (апрель 2008): 845–49. https://doi.org/10.1109/TAC.2008.919531.
[2] Раух, H. E., F. Тунговый, и К. Т. Стрибель. “Оценки Наибольшего правдоподобия Линейных Динамических систем”. Журнал 3 AIAA, № 8 (август 1965): 1445–50. https://doi.org/10.2514/3.3166.