Сглаженная оценка траектории 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/s2 во время некоторых маневров. Вы используете 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

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

Поддержка

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