smooth

Обратная оценка сглаженного состояния отслеживающего фильтра

Свернуть все на странице

Синтаксис

[smoothX,smoothP] = smooth(filter)
[smoothX,smoothP] = smooth(filter,numBackSteps)

Описание

пример

[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.

Примеры

свернуть все

Открыть Live Script

Создайте траекторию истинности на основе модели движения с постоянным поворотом и сгенерируйте измерения 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')

Figure contains an axes. The axes contains 4 objects of type line. These objects represent Truth, Measurements, Forward filtering, Backward smoothing.

Получите ошибки расчета. 

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 contains 2 axes. Axes 1 with title Position Errors contains 2 objects of type line. These objects represent Forward filtering, Backward smoothing. Axes 2 contains 2 objects of type line.

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)')

Figure contains 3 axes. Axes 1 with title Velocity Errors contains 2 objects of type line. These objects represent Forward filtering, Backward smoothing. Axes 2 contains 2 objects of type line. Axes 3 contains 2 objects of type line.

Открыть Live Script

Создайте траекторию истинности на основе модели движения с постоянным поворотом и сгенерируйте измерения 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')

Figure contains an axes. The axes contains 4 objects of type line. These objects represent Truth, Measurements, Forward filtering, Backward smoothing.

Получите ошибки расчета.

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 contains 2 axes. Axes 1 with title Position Errors contains 2 objects of type line. These objects represent Forward filtering, Backward smoothing. Axes 2 contains 2 objects of type line.

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)')

Figure contains 3 axes. Axes 1 with title Velocity Errors contains 2 objects of type line. These objects represent Forward filtering, Backward smoothing. Axes 2 contains 2 objects of type line. Axes 3 contains 2 objects of type line.

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

свернуть все

Фильтр для отслеживания объектов, заданный как один из следующих объектов:

  • trackingABF - Альфа-бета фильтр

  • trackingKF - Линейный фильтр Калмана

  • trackingEKF - Расширенный фильтр Калмана

  • trackingUKF - Сигма-точечный фильтр Калмана

  • trackingCKF - Кубатурный фильтр Калмана

  • trackingMSCEKF - Расширенный фильтр Калмана с использованием измененных сферических координат (MSC)

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

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

свернуть все

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

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

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

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

Ссылки

[1] СЯрккА, Симо. «Нескромный Rauch--Tung--Striebel Smooter». Транзакции IEEE по автоматическому контролю, 53, № 3 (апрель 2008 года): 845-49. https://doi.org/10.1109/TAC.2008.919531.

[2] Rauch, H. E., F. Tung, and C. T. Striebel. «Максимальные оценки правдоподобия линейных динамических систем». AIAA Journal 3, № 8 (август 1965): 1445-50. https://doi.org/10.2514/3.3166.

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

.

См. также

Введенный в R2021a

Документация по Sensor Fusion and Tracking Toolbox

Поддержка

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