Exponenta Banner
exponenta event banner

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.

Примеры

свернуть все

Скрипт Open Live Script

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

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.

Скрипт Open Live Script

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

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

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

Смотрите также

Введенный в R2021a

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

Поддержка