Сглаженная оценка траектории `trackingIMM` Фильтр

Скрипт Open Live Script

В этом примере показано, как сглаживать оценки состояния цели с помощью smooth объектная функция. Сглаживание является методом, чтобы совершенствовать предыдущие оценки состояния с помощью актуальных измерений и информации об оценке состояния. В этом примере вы изучите, как улучшить ранее откорректированные оценки от фильтра Взаимодействующей мультимодели (IMM) путем выполнения обратной рекурсии, которая производит сглаживавшие и более точные оценки состояния. В первом разделе вы реализуете сглаженный алгоритм, чтобы сглаживать траекторию поворачивающегося автомобиля. В оставшейся части этого примера вы выполняете сглаживание на нескольких высоко маневрирующих траекториях самолета, взятых из Траекторий Сравнительного теста для примера Отслеживания Мультиобъекта.

Фиксированный интервал сглаживая, чтобы отследить превращение автомобиля

В этом разделе вы сглаживаете оценку траектории поворачивающегося автомобиля. Во-первых, вы используете helperGenerateCarMeasurements функция, чтобы сгенерировать измерения положения автомобиля и соответствующей истины.

rng(2021); % For repeatable results

% Generate measurements for a car taking a turn
[measPositionCar, trueStateCar, timeCar] = helperGenerateCarMeasurements();

% Define the initial detection in the format of objectDetection
detection = objectDetection(0, [0;0;0], 'MeasurementNoise', eye(3,3));

Используя initekfimm функция инициализации, вы создаете фильтр IMM на основе постоянной скорости, постоянного ускорения и постоянных моделей движения поворота. Чтобы установить фильтр для обратного сглаживания, вы устанавливаете EnableSmoothing свойство фильтра к true. Поскольку вы хотите выполнить фиксированный интервал, сглаживая (оффлайн сглаживающий) на оценках состояния, который использует все откорректированные и предсказанные шаги, вы устанавливаете MaxNumSmoothingSteps свойство к количеству шагов измерения.

defaultIMMCar = initekfimm(detection);
% Enable Smoothing
defaultIMMCar.EnableSmoothing = true;
defaultIMMCar.MaxNumSmoothingSteps = size(measPositionCar,1);
% Initilaize IMM
positionSelector = [1 0 0 0 0 0;0 0 1 0 0 0;0 0 0 0 1 0]; % Select position from state
initialState = positionSelector' * measPositionCar(1,:)';
initialCovariance = diag([1,1e4,1,1e4,1,1e4]);
initialize(defaultIMMCar, initialState, initialCovariance);

Чтобы получить прямые оценки автомобиля, вы запускаете фильтр путем итеративного предсказания и исправления оценок состояния.

% Initialize the corrected states
numSteps = numel(timeCar);
correctState = zeros(6,numSteps);
correctStateCovariance = zeros(6,6,numSteps);
correcState(:,1) = defaultIMMCar.State;
correctStateCovariance(:,:,1) = defaultIMMCar.StateCovariance;

% Forward tracking with prediction and correction
for i = 2:numSteps
    dt = timeCar(i) - timeCar(i-1);
    predict(defaultIMMCar, dt);
    [correctState(:,i), correctStateCovariance(:,:,i)] = correct(defaultIMMCar, measPositionCar(i,:));
end

Чтобы выполнить сглаживание, просто вызовите smooth объектная функция фильтра. Функция возвращает сглаживавшие состояния, ковариацию состояния и вероятности модели.

[smoothState, smoothStateCovariance, modelProbabilities] = smooth(defaultIMMCar);

Затем используйте helperTrajectoryViewer функция, чтобы визуализировать сглаженные результаты и ошибки RMS. Результаты показывают, что использование оффлайнового сглаживания фильтра IMM позволяет вам уменьшать ошибки в оценках.

trajNum = 0;
helperTrajectoryViewer(trajNum,timeCar,correctState, smoothState, trueStateCar, measPositionCar);

Сглаженные высоко маневрирующие траектории самолета с настройкой IMM по умолчанию

В этом разделе вы сглаживаете траектории шести высоко маневрирующих самолетов. Траектории, используемые в этом разделе, совпадают с теми в Траекториях Сравнительного теста для примера Отслеживания Мультиобъекта. В примере ускорение самолета изменяет целых 35 $m / s^{2}$ во время некоторых маневров. Вы используете helperGenerateAircraftTrajMeasurements функция, чтобы сгенерировать данные об измерении и истину.

[measPosition, trueState, time] = helperGenerateAircraftTrajMeasurements;

Конфигурируйте фильтр IMM, подобный как предыдущий раздел.

% Define initial detection
detection = objectDetection(0, [0;0;0], 'MeasurementNoise', eye(3,3));

defaultIMMAircraft = initekfimm(detection);
% Enable Smoothing
defaultIMMAircraft.EnableSmoothing = true;
defaultIMMAircraft.MaxNumSmoothingSteps = size(measPosition,1);

Используя helperGenerateSmoothData функция, вы запускаете созданный фильтр IMM, получаете откорректированные оценки состояния и генерируете сглаживавшие оценки состояния для первой траектории.

% Only estimate the first trajectory
trajNum = 1;

% Extract measurement data
measPosTraj = measPosition(:,:,trajNum);

% Estimate and smooth the trajectory using the helperGenerateSmoothData function
[correctState, correctStateCovariance, smoothState, smoothStateCovariance] = helperGenerateSmoothData(defaultIMMAircraft,measPosTraj,time);

Визуализируйте прямую оценку и сглаживающие результаты с помощью helperTrajectoryViewer funciton.

helperTrajectoryViewer(trajNum,time,correctState, smoothState, trueState(:,:,trajNum), measPosTraj);

Как замечено по результатам, процесс сглаживания выполняет плохо и становится нестабильными близкими 120 секундами. Это происходит в основном из-за недостаточного шума процесса в фильтре IMM. Быстрые изменения на ускорении самолета и крутые повороты в коротких интервалах требуют более высокого шума процесса для постоянного ускорения и постоянных моделей движения поворота фильтра IMM.

Корректировка шума процесса для лучшей эффективности сглаживания

В этом разделе вы настраиваете шум процесса фильтра для лучшей оценки и сглаживающих результатов. Во-первых, создайте постоянную скорость, постоянное ускорение и постоянную модель угловой скорости вращения, которая будет использоваться в фильтре IMM.

constVelocityEKF = initcvekf(detection);
constantAccelerationEKF = initcaekf(detection);
constantTurnEKF = initctekf(detection);

Чтобы компенсировать скоростную неопределенность, увеличьте шум процесса постоянной скоростной модели во всех трех осях на основе оценок изменения в скоростных значениях маневрирующей цели. Точно так же, чтобы компенсировать ускоряющую неопределенность, увеличьте шум процесса для постоянной ускоряющей модели во всех трех осях на основе оценки изменения в ускоряющих значениях цели manuvering. Чтобы компенсировать поворачивающуюся неопределенность уровня, увеличьте шум процесса для угловой скорости вращения модели постоянного поворота.

constantVelocityEKF.ProcessNoise = 36*eye(3,3);
constantAccelerationEKF.ProcessNoise = 100*eye(3,3);
constantTurnEKF.ProcessNoise = 36*eye(4,4);

Создайте фильтр IMM с помощью трех заданных фильтров оценки и включите возможность сглаживания.

filters = {constVelocityEKF;constantAccelerationEKF;constantTurnEKF};
imm = trackingIMM(filters,'TransitionProbabilities', 0.99);

imm.EnableSmoothing = true;
imm.MaxNumSmoothingSteps = size(measPosition,1);

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

for i = 1:6
    trajNum = i;
    measPosTraj = measPosition(:,:,trajNum);
    [correctState, correctStateCovariance, smoothState, smoothStateCovariance] = helperGenerateSmoothData(clone(imm),measPosTraj,time);
    
    % Visualize the results for each trajectory
    helperTrajectoryViewer(trajNum, time, correctState, smoothState, trueState(:,:,trajNum), measPosTraj)
end

От результатов использование настроенного шума процесса значительно уменьшает ошибки расчета по сравнению с предыдущими результатами сглаживания, не настраивая шум процесса.

Сводные данные

В этом примере вы изучили, как сглаживать результаты фильтра получить лучшие оценки состояния. Вы также изучаете важность настраивающего шума фильтра правильно для ваших определенных приложений.

Вспомогательные Функции

`helperGenerateCarMeasurements`

Сгенерируйте входные параметры измерения для траектории поворачивающегося автомобиля

function [measPosition, trueState, time] = helperGenerateCarMeasurements()

% Create a trajectory of a car taking a turn.
carWayPoints = [6 2 0; 18 4 0; 25 7 0; 28 10 0; 31 15 0; 33 22 0];
timeSteps = [0 2 4 6 8 10];

% Scenario Generation.

scene = trackingScenario('UpdateRate',20);

plat = platform(scene);
plat.Trajectory = waypointTrajectory(carWayPoints, timeSteps);

% Create trajectory using waypoints from the scenario.
numSteps = 0;
truePosition = [];
trueVelocity = [];
trueAcceleration = [];
time = [];
while advance(scene)
    poses = platformPoses(scene);
    t = scene.SimulationTime;
    numSteps = numSteps + 1;
    truePosition(numSteps,1:3) = poses.Position;
    trueVelocity(numSteps,1:3) = poses.Velocity;
    trueAcceleration(numSteps,1:3) = poses.Acceleration;
    time(numSteps) = t;
end

trueState = [truePosition(:,1,:),trueVelocity(:,1,:),truePosition(:,2,:),trueVelocity(:,2,:),truePosition(:,3,:),trueVelocity(:,3,:)];

% Add measurement noise to true position.
measNoise = 1* randn(size(truePosition));
measPosition = truePosition + [measNoise(:,1:2), zeros(numSteps,1)];

end

helperGenerateAircraftTrajMeasurements

Сгенерируйте входные параметры измерения для траекторий нескольких самолетов

function [measPosition, trueState, time] = helperGenerateAircraftTrajMeasurements()

% Load trajectory waypoints and velocities. The file contains tables of waypoints and 
% velocities (in units of meters and meters per second) that are used to reconstruct six aircraft trajectories.
load('benchmarkTrajectoryTables.mat', 'trajTable');

% Scenario generation.
scene = trackingScenario('UpdateRate',10);

% Assign each platform with a trajectory
for n=1:6
    plat = platform(scene);
    traj = trajTable{n};
    plat.Trajectory = waypointTrajectory(traj.Waypoints, traj.Time, 'Velocities', traj.Velocities);
end

% Create trajectory using waypoints from the scenario.
numSteps = 0;
truePosition = [];
trueVelocity = [];
trueAcceleration = [];
time = [];
while advance(scene)
    poses = platformPoses(scene);
    t = scene.SimulationTime;
    numSteps = numSteps + 1;
    position = vertcat(poses.Position);
    velocity = vertcat(poses.Velocity);
    acceleration = vertcat(poses.Acceleration);
    truePosition(numSteps,1:3,1:6) = permute(position,[3 2 1]);
    trueVelocity(numSteps,1:3,1:6) = permute(velocity,[3 2 1]);
    trueAcceleration(numSteps,1:3,1:6) = permute(acceleration,[3 2 1]);
    time(numSteps) = t;
end
trueState = [truePosition(:,1,:),trueVelocity(:,1,:),truePosition(:,2,:),trueVelocity(:,2,:),truePosition(:,3,:),trueVelocity(:,3,:)];

% Define the measurements to be the position and add normal random noise with a standard deviation of 0.1 to the measurements.
measNoise = 0.1* randn(size(truePosition));
measPosition = truePosition + measNoise;
end

helperGenerateSmoothData

Сгенерируйте откорректированные и сглаживавшие состояния

function [correctState, correctStateCovariance, smoothState, smoothStateCovariance] = helperGenerateSmoothData(imm, measPosTraj, time)

% Output correct and smooth states

numSteps = numel(time);

positionSelector = [1 0 0 0 0 0;0 0 1 0 0 0;0 0 0 0 1 0];
initialState = positionSelector' * measPosTraj(1,:)';
initialCovariance = diag([1,16e4,1,16e4,1,16e4]); % Velocity is not measured
initialize(imm, initialState, initialCovariance);

correctState = zeros(6,numSteps);
correctStateCovariance = zeros(6,6,numSteps);
correctModelProb = zeros(3,numSteps);
correctState(1,1) = measPosTraj(1,1);
correctState(3,1) = measPosTraj(1,2);
correctState(5,1) = measPosTraj(1,3);
correctStateCovariance(:,:,1) = imm.StateCovariance;
correctModelProb(:,1) = imm.ModelProbabilities;

for i = 2:numSteps
    dt = time(i) - time(i-1);
    predict(imm, dt);
    [correctState(:,i), correctStateCovariance(:,:,i)] = correct(imm, measPosTraj(i,:));
    correctModelProb(:,i) = imm.ModelProbabilities;
end

[smoothState, smoothStateCovariance, smoothModelProb] = smooth(imm);

end

`helperTrajectoryViewer`

Визуализируйте результаты сглаживания и сравните ошибку RMS

function helperTrajectoryViewer(n, time, correctState, smoothState, trueStateTraj, measPosTraj)

% Create figure and two panels, first panel for position and second panel
% for error visualization

truePosition = trueStateTraj(:,[1 3 5])';
correctPosition = correctState([1 3 5],:);
smoothPosition = smoothState([1 3 5],:);

correctPosError = correctState([1,3,5],1:end-1) - trueStateTraj(1:end-1,[1,3,5])';
smoothPosError = smoothState([1,3,5],:) - trueStateTraj(1:end-1,[1,3,5])';

correctVelError = correctState([2,4,6],1:end-1) - trueStateTraj(1:end-1,[2,4,6])';
smoothVelError = smoothState([2,4,6],:) - trueStateTraj(1:end-1,[2,4,6])';

numSteps = numel(time);

correctPosRMS = zeros(1,numSteps-1);
smoothPosRMS = zeros(1,numSteps - 1);

correctVelRMS = zeros(1,numSteps-1);
smoothVelRMS = zeros(1,numSteps - 1);

for i = 1:numSteps -1
    deltaPc = correctPosError(:,i);
    correctPosRMS(:,i) = sqrt((deltaPc')*deltaPc);
    
    deltaPs = smoothPosError(:,i);
    smoothPosRMS(:,i) = sqrt((deltaPs')*deltaPs);

    
    deltaVc = correctVelError(:,i);
    correctVelRMS(:,i) = sqrt((deltaVc')*deltaVc);
    
    deltaVs = smoothVelError(:,i);
    smoothVelRMS(:,i) = sqrt((deltaVs')*deltaVs);
end

hfig = figure('Name',"Trajectory " + n,'NumberTitle','off','Units','normalized','Position',[0.1 0.1 0.8 0.8]);
hLeftPanel = uipanel(hfig,'Position',[0 0 1/2 1]);
hRightPanel = uipanel(hfig,'Position',[1/2 0 1/2 1]);

xMin = 10*floor(min(truePosition(:,1))/10e3)-5;
xMax = 10*ceil(max(truePosition(:,1))/10e3)+5;
yMin = 10*floor(min(truePosition(:,2))/10e3)-5;
yMax = 10*ceil(max(truePosition(:,2))/10e3)+5;
zMin = 10*floor(min(truePosition(:,3))/10e3)-5;
zMax = 10*ceil(max(truePosition(:,3))/10e3)+5;

% Place x-y plot in left panel for plotting true trajectory, corrected
% trajectory, and smoothed trajectory
hAx1 = axes('Parent',hLeftPanel);
axis(hAx1,[xMin xMax yMin yMax zMin zMax]);
plot3(hAx1,truePosition(1,:),truePosition(2,:),truePosition(3,:),'-','lineWidth',2);
hold on
plot3(hAx1,measPosTraj(:,1),measPosTraj(:,2),measPosTraj(:,3),'.','MarkerSize',3,'lineWidth',2);
plot3(hAx1,correctPosition(1,:),correctPosition(2,:),correctPosition(3,:),'-','lineWidth',.05);
plot3(hAx1,smoothPosition(1,:),smoothPosition(2,:),smoothPosition(3,:),'-','lineWidth',2);

title("Trajectory " + n);
xlabel('X (m)');
ylabel('Y (m)');
zlabel('Z (m)');
lObj = legend('True Position', 'Measured Position', 'Corrected Position', 'smoothed Position','location',"northeast");
axis(hAx1,'square');
grid(hAx1,'minor');
% Switch view to X-Y if Z 0
viewSwitch = mean(truePosition(3,:));
if viewSwitch == 0
    view(90,90);
else
    view(60,10);
end
% Set legend position
legendPos = lObj.Position;
set(lObj,'Position',[legendPos(1)*1.1 legendPos(2) legendPos(3) legendPos(4)])
set(gca,'ZDir','reverse');

% Position RMS plot
hAx2 = subplot(2,1,1,'Parent',hRightPanel);
line(hAx2, time(20:end-1),correctPosRMS(20:end),'color','m');
hold on;
line(hAx2, time(20:end-1),smoothPosRMS(20:end),'color','b');
grid(hAx2,'on');
grid(hAx2,'minor');
xlim([1 inf]);
xlabel('time (s)');
ylabel('Position Error');
legend('Correct Error', 'Smooth Error');

% Velocity RMS plot
hAx3 = subplot(2,1,2,'Parent',hRightPanel);
line(hAx3, time(20:end-1),correctVelRMS(20:end),'color','m');
hold on;
line(hAx3, time(20:end-1),smoothVelRMS(20:end),'color','b');
grid(hAx3,'on');
grid(hAx3,'minor');
xlim([1 inf]);
xlabel('time (s)');
ylabel('Velocity Error');
legend('Correct Error', 'Smooth Error');
end

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

Сглаженная оценка траектории `trackingIMM` Фильтр

Фиксированный интервал сглаживая, чтобы отследить превращение автомобиля

Сглаженные высоко маневрирующие траектории самолета с настройкой IMM по умолчанию

Корректировка шума процесса для лучшей эффективности сглаживания

Сводные данные

Вспомогательные Функции

`helperGenerateCarMeasurements`

helperGenerateAircraftTrajMeasurements

helperGenerateSmoothData

`helperTrajectoryViewer`

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

Поддержка

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

Сглаженная оценка траектории trackingIMM Фильтр

Фиксированный интервал сглаживая, чтобы отследить превращение автомобиля

Сглаженные высоко маневрирующие траектории самолета с настройкой IMM по умолчанию

Корректировка шума процесса для лучшей эффективности сглаживания

Сводные данные

Вспомогательные Функции

helperGenerateCarMeasurements

helperGenerateAircraftTrajMeasurements

helperGenerateSmoothData

helperTrajectoryViewer

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

Поддержка

Сглаженная оценка траектории `trackingIMM` Фильтр

`helperGenerateCarMeasurements`

`helperTrajectoryViewer`