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.

Примеры

свернуть все

Фиксированный интервал сглаживая для `trackingEKF`

Скрипт 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 object. The axes object 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 objects. Axes object 1 with title Position Errors contains 2 objects of type line. These objects represent Forward filtering, Backward smoothing. Axes object 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 objects. Axes object 1 with title Velocity Errors contains 2 objects of type line. These objects represent Forward filtering, Backward smoothing. Axes object 2 contains 2 objects of type line. Axes object 3 contains 2 objects of type line.

Сглаживание фиксированной задержки `trackingEKF`

Скрипт Open Live Script

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 object. The axes object 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()
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)')

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

свернуть все

`filter` — Отфильтруйте для объектного отслеживания
отслеживание объекта фильтра

Отфильтруйте для отслеживания объекта в виде одного из этих объектов:

trackingABF — Фильтр альфы - беты
trackingKF — Линейный Фильтр Калмана
trackingEKF — Расширенный Фильтр Калмана
trackingUKF — Сигма-точечный фильтр Калмана
trackingCKF — Фильтр Калмана кубатуры
trackingMSCEKF — Расширенный Фильтр Калмана с помощью модифицированных сферических координат (MSC)

`numBackSteps` — Количество обратных шагов
положительное целое число

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

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

свернуть все

`smoothX` — Сглаживавшие состояния
N-by-K матрица

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

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

`smoothP` — Сглаживавшие ковариации
N-by-N-by-K массив

Сглаживавшие ковариации, возвращенные как 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.

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

Генерация кода C/C++
Генерация кода C и C++ с помощью MATLAB® Coder™.

В генерации кода, после клонирования фильтра, вы не можете изменить его EnableSmoothing свойство.

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

smooth(for trackingIMM)

Введенный в R2021a

Документация

smooth

Синтаксис

Описание

Примеры

Фиксированный интервал сглаживая для `trackingEKF`

Сглаживание фиксированной задержки `trackingEKF`

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

`filter` — Отфильтруйте для объектного отслеживания
отслеживание объекта фильтра

`numBackSteps` — Количество обратных шагов
положительное целое число

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

`smoothX` — Сглаживавшие состояния
N-by-K матрица

`smoothP` — Сглаживавшие ковариации
N-by-N-by-K массив

Ссылки

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

Генерация кода C/C++
Генерация кода C и C++ с помощью MATLAB® Coder™.

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

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

Поддержка

Документация

smooth

Синтаксис

Описание

Примеры

Фиксированный интервал сглаживая для trackingEKF

Сглаживание фиксированной задержки trackingEKF

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

filter — Отфильтруйте для объектного отслеживания отслеживание объекта фильтра

numBackSteps — Количество обратных шагов положительное целое число

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

smoothX — Сглаживавшие состояния N-by-K матрица

smoothP — Сглаживавшие ковариации N-by-N-by-K массив

Ссылки

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

Генерация кода C/C++ Генерация кода C и C++ с помощью MATLAB® Coder™.

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

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

Поддержка

Фиксированный интервал сглаживая для `trackingEKF`

Сглаживание фиксированной задержки `trackingEKF`

`filter` — Отфильтруйте для объектного отслеживания
отслеживание объекта фильтра

`numBackSteps` — Количество обратных шагов
положительное целое число

`smoothX` — Сглаживавшие состояния
N-by-K матрица

`smoothP` — Сглаживавшие ковариации
N-by-N-by-K массив

Генерация кода C/C++
Генерация кода C и C++ с помощью MATLAB® Coder™.