Используя отслеживание фильтров

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

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

В Отслеживании, Выводящем Целевой пример, вы изучаете, как использовать фильтр взаимодействующей многоуровневой модели (IMM), чтобы отследить цель, траектория которой может быть разделена на три сегмента: одна постоянная скорость, постоянный поворот и постоянный ускоряющий сегмент. Фильтр IMM является “гибридным” фильтром, который запускает ряд фильтров параллельно и возвращает результат, который взвешивает оценки каждого фильтра по его эффективности. Вы видите результат примера, воспроизведенного ниже:

n = 1000;
[trueState, time, fig1] = helperGenerateTruthLoop(n);
dt = diff(time(1:2));
numSteps = numel(time);

rng(2021); % for repeatable results

% Select position from 9-dimenstional state [x;vx;ax;y;vy;ay;z;vz;az]            
positionSelector = [1 0 0 0 0 0 0 0 0;
                    0 0 0 1 0 0 0 0 0;
                    0 0 0 0 0 0 1 0 0]; 
truePos = positionSelector*trueState;
sigma = 10; % Measurement noise standard deviation
measNoise = sigma* randn(size(truePos));
measPos = truePos + measNoise;
initialState = positionSelector'*measPos(:,1);
initialCovariance = diag([1 1e4 1e4 1 1e4 1e4 1 1e4 1e4]); % Velocity and acceleration are not measured

detection = objectDetection(0,[0; 0; 0],'MeasurementNoise',sigma^2 * eye(3));
f1 = initcaekf(detection); % Constant acceleration EKF
f1.ProcessNoise = 0.3*eye(3);
f2 = initctekf(detection); % Constant turn EKF
f2.ProcessNoise = diag([1 1 100 1]);
f3 = initcvekf(detection); % Constant velocity EKF
imm = trackingIMM({f1; f2; f3},'TransitionProbabilities',0.99, ...
    'ModelConversionFcn',@switchimm);
initialize(imm, initialState, initialCovariance);

estState = zeros(9,numSteps);
for i = 2:size(measPos,2)
    predict(imm,dt);
    estState(:,i) = correct(imm,measPos(:,i));
end

figure(fig1);
ax = gca(fig1);
line(ax,estState(1,:),estState(4,:),estState(7,:),'Color','k','LineStyle',':','DisplayName','Correction estimates');
axis(ax,'image');

Масштабируйте в области перехода между каждой моделью:

axis(ax,[6000 7000 -200 200])
legend(ax,'Location','bestoutside')

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

Сглаженные оценки траектории

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

imm = trackingIMM({f1; f2; f3},'TransitionProbabilities',0.99, ...
    'ModelConversionFcn',@switchimm, ...
    'EnableSmoothing',true, ...
    'MaxNumSmoothingSteps',size(measPos,2)-1);
initialize(imm, initialState, initialCovariance);

estState = zeros(9,numSteps);

for i = 2:size(measPos,2)
    predict(imm,dt);
    estState(:,i) = correct(imm,measPos(:,i));
end

smoothState = smooth(imm);

line(ax,smoothState(1,:),smoothState(4,:),smoothState(7,:),'Color','g','LineStyle','-','DisplayName','Smooth estimates')
axis(ax,'image');

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

axis(ax,[6000 7000 -200 200])

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

Используя полиномы младшего разряда для плотно выборочных данных

При сравнении отслеживания алгоритмов может быть выгодно представить результаты отслеживания в формат так, чтобы информация о положении могла быть запрошена в течение момента произвольного момента времени. Когда данные производятся на достаточно высоком показателе, можно использовать набор кусочных полиномов младшего разряда, чтобы аппроксимировать данные. Эти полиномы могут дифференцироваться, чтобы дать информация об ускорении и скорость. Кроме того, можно использовать набор алгоритмов, чтобы вывести ориентацию основанного на объектах на определенных предположениях. Например, примите, что объект всегда стоит перед направлением перемещения и банков, чтобы противодействовать центростремительным силам. Следующий код показывает, как использовать сглаживавшие положения и скорости, чтобы вывести общее ускорение объекта при помощи кубического сплайна.

smoothPos = smoothState([1 4 7],:);
smoothVel = smoothState([2 5 8],:);

% Smoothed state vector does not include acceleration
% Create piecewise polynomial from velocity information
velPP = pchip(time(2:end),smoothVel);

% Construct a new piecewise polynomial with the derivative
[breaks,coefs,~,k,dim]=unmkpp(velPP);
coefs = coefs(:,1:k-1).*(k-1:-1:1);
accPP=mkpp(breaks,coefs,dim);

% Evaluate the piecewise polynomial to get the acceleration
smoothAcc = ppval(accPP,time(2:end));

% Display the velocity and resulting acceleration
fig = figure;
ax1 = subplot(2,1,1,'Parent',fig);
plot(ax1,time(2:end),smoothVel(1,:),time(2:end),smoothVel(2,:),time(2:end),smoothVel(3,:));
xlabel('time [s]');
ylabel('velocity [m/s]')
title('Velocity Profile')
legend('v_x','v_y','v_z')

ax2 = subplot(2,1,2,'Parent',fig);
plot(ax2,time(2:end),smoothAcc(1,:),time(2:end),smoothAcc(2,:),time(2:end),smoothAcc(3,:));
xlabel('time [s]')
ylabel('acceleration [m/s^2]')
title('Acceleration Profile')
legend('a_x','a_y','a_z')

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

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

Используя старшие полиномы

В предыдущем разделе вы использовали кубические полиномы, чтобы вычислить ускорение в каждой точке. Для траекторий, которые состоят в основном из кругового и прямого движения, с помощью interpolant, который способствует постоянной кривизне, работает значительно лучше, чем полиномы младшего разряда. waypointTrajectory объект может смоделировать траектории с постоянным поворотом (постоянная кривизна). Это может также искренне представлять траектории постепенными изменениями и в искривлении и в ускорении.

Простой метод, чтобы уменьшать объем потребляемой памяти interpolant и уменьшить величину переходных процессов должен использовать changepoint оценку, чтобы определить местоположение существенных изменений в искривлении траектории. После этого используйте те точки, чтобы вычислить interpolant.

% Obtain curvature of trajectory projected into x-y plane
planarCurvature = cross(smoothVel,smoothAcc) ./ vecnorm(smoothVel,2).^3;
horizontalCurvature = planarCurvature(3,:);

% Obtain the estimates of the changepoints
threshold = var(horizontalCurvature);
[tf,m] = ischange(horizontalCurvature,'Threshold',threshold);

fig=figure;
hAx=gca(fig);
line(hAx,time(2:end),horizontalCurvature,'DisplayName','Computed');
[xx,yy]=stairs(time(2:end),m);
line(hAx,xx,yy,'DisplayName','Estimated','Color','k')
legend(hAx)
xlabel('time [s]');
ylabel('curvature [rad/m]')
title('Horizontal curvature')

Можно затем взять эти changepoints в качестве начальных точек для построения waypoints траектории:

idx = 1+[0 find(tf) numel(tf)-1];

waypoints = smoothPos(:,idx)';
wayvels = smoothVel(:,idx)'; 
timeOfArrival = time(idx);
traj = waypointTrajectory(waypoints,timeOfArrival,'Velocities',wayvels);
[wayPos, ~, wayVel] = lookupPose(traj,time(1:end-1));

clf
plot(trueState(1,:),trueState(4,:),'-');
hold on;
plot(wayPos(:,1),wayPos(:,2),'-')
plot(waypoints(:,1),waypoints(:,2),'o ');
grid on;
xlabel('X Position [m]');
ylabel('Y Position [m]');
axis equal;
legend('Truth','Waypoint Estimate', 'Waypoints', 'Location','southwest')

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

Если вы строите величину ошибки положения восстановленной траектории против сглаживавших оценок, вы видите локализованные области с большими ошибками:

figure;
plot(time(2:end),vecnorm(smoothPos-wayPos',2))
xlabel('time [s]')
ylabel('distance [m]')
title('Magnitude of Position Error');

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

objective = 10;  % use 10 m for objective maximal position error
maxError = Inf;  % pre-assign error to a maximal value
while maxError > objective
    [~,maxi] = max(vecnorm(smoothPos-wayPos',2));
    idx = unique(sort([maxi idx]));
    waypoints = smoothPos(:,idx)';
    wayvels = smoothVel(:,idx)'; 
    timeOfArrival = time(idx);
    traj = waypointTrajectory(waypoints,timeOfArrival,'Velocities',wayvels);
    [wayPos, ~, wayVel, wayAcc] = lookupPose(traj,time(1:end-1));
    maxError = max(vecnorm(smoothPos-wayPos',2));
end

figure;
plot(trueState(1,:),trueState(4,:),'-');
hold on;
plot(wayPos(:,1),wayPos(:,2),'-')
plot(waypoints(:,1),waypoints(:,2),'o ');
grid on;
xlabel('X Position [m]');
ylabel('Y Position [m]');
axis equal;
legend('Truth','Waypoint Estimate', 'Waypoints', 'Location','southwest')

Можно затем наблюдать уменьшаемую ошибку положения.

figure;
plot(time(2:end),vecnorm(smoothPos-wayPos',2))
xlabel('time [s]')
ylabel('distance [m]')
title('Magnitude of Position Error');

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

fig = figure;
ax1 = subplot(2,1,1,'Parent',fig);
plot(ax1,time(1:end-1),wayVel);
title('Velocity Profile')
xlabel('time');
ylabel('velocity [m/s]')
legend('v_x','v_y','v_z')

ax2 = subplot(2,1,2,'Parent',fig);
plot(ax2,time(1:end-1),wayAcc);
title('Acceleration Profile')
xlabel('time [s]');
ylabel('acceleration [m/s^2]')
legend('a_x','a_y','a_z')

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

В этом примере вы изучили, как восстановить траекторию основной истины с помощью взаимодействующей многоуровневой модели более сглаженные и полиномы низкоуровневые. Вы также использовали простую схему, которая использовала старший разряд interpolant (содержавшийся в waypointTrajectory объект) наряду с критической выборкой, чтобы восстановить производные высшего порядка, которые были бы в противном случае замаскированы шумом.