샘플링된 데이터에서 필터링, 평활화, 보간을 사용하여 ground truth 궤적 복원하기

R2021b 이후

이 예제에서는 대화형 방식의 모션 모델 기반 필터를 스무더와 함께 사용하여 보간 기법을 기반으로 ground truth 궤적을 복원하는 방법을 보여줍니다.

추적 필터 사용하기

많은 궤적은 시간에 따른 일련의 개별 기동으로 나눌 수 있습니다. 이때 각 기동은 시간 경과에 따라 객체의 자세가 어떻게 변화하는지 제어하는 작은 파라미터 세트에 의해 결정됩니다. 이러한 기동은 매우 단순한 모델(예: 등속도 모델, 등가속도 모델 또는 선회가 일정한 모델)로 구성되는 경우가 많습니다. 이러한 단순 모델은 파라미터의 작은 섭동에 대해 내성이 있는 추적 필터와 함께 사용할 수 있습니다.

이러한 섭동은 작아야 하지만 반드시 무작위일 필요는 없습니다. 예를 들어 추적 결과에서 약간의 편향이 발생하는 것을 감안한다면 등속도 필터로 작은 일정 가속도를 갖는 객체를 추적할 수 있는 경우가 많습니다. 이러한 섭동이 상당한 경우에는 필터 성능이 심각하게 저하될 수 있습니다.

Tracking Maneuvering Targets 예제에서는 IMM(Interacting Multiple Model) 필터를 사용하여, 궤적이 3가지 세그먼트(등속도, 일정 선회, 등가속도)로 나뉘는 타깃을 추적하는 방법을 알아봅니다. 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');

위와 동일한 영역을 확대해 보면 평활화된 추정값이 ground truth 궤적 복구에 훨씬 더 성공적임을 알 수 있습니다.

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

결과를 보면, 평활화는 한 모션 모델에서 다른 모션 모델로의 천이 시 갑작스러운 변화가 있을 때 궤적 데이터를 복구하는 데 효과적인 툴임을 확인할 수 있습니다. 정방향-역방향 평활화는 ground truth 궤적 추정 시 대체로 잘 작동합니다.

조밀하게 샘플링된 데이터에 대해 낮은 차수의 다항식 사용하기

추적 알고리즘 비교 시, 자세 정보를 임의의 시점에서 쿼리할 수 있도록 추적 결과를 형식으로 나타내는 것이 유용할 수 있습니다. 데이터가 충분히 높은 레이트로 샘플링되는 경우, 일련의 낮은 차수의 조각별 다항식을 사용하여 데이터를 근사할 수 있습니다. 속도 정보와 가속도 정보를 위해 이러한 다항식을 미분할 수 있습니다. 또한 일련의 알고리즘을 사용하여 특정 가정을 기반으로 객체의 방향을 유추할 수 있습니다. 예를 들어 객체는 항상 이동 방향을 향하고 구심력을 상쇄하기 위해 기울어진다고 가정합니다. 다음 코드는 3차 스플라인을 사용하여 객체의 총 가속도를 유추하기 위해 평활화된 위치와 속도를 사용하는 방법을 보여줍니다.

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')

계산된 가속도는 일정한 선회 기동 중에 순수한 정현파가 아니며, 이는 평활화된 출력의 작은 변동으로 인한 것입니다. 그럼에도 불구하고 결과는 각 기동의 고유한 영역을 식별할 수 있는 정보를 제공합니다.

3차 다항식은 위치를 보간하는 데 수치적으로 효율적이지만 원호를 정확히 나타내기 위해 조밀한 샘플링에 의존합니다. 더 우수한 정확도가 필요한 경우에는 더 높은 레이트로 샘플링된 데이터를 사용해야 합니다. 그러기 위해서는 메모리 리소스를 늘려야 하고 더 큰 룩업 테이블이 필요합니다.

높은 차수의 다항식 사용하기

이전 섹션에서는 3차 다항식을 사용해 각 점에서 가속도를 계산했습니다. 주로 원형 운동과 직선 운동으로 구성된 궤적의 경우, 일정한 곡률을 선호하는 보간을 사용하면 낮은 차수의 다항식보다 훨씬 더 효과적입니다. waypointTrajectory 객체는 일정한 선회(일정 곡률)를 가진 궤적을 모델링할 수 있습니다. 또한 곡률과 가속도가 점진적으로 변화하는 궤적을 정확히 표현할 수 있습니다.

보간에 대한 메모리 사용량을 줄이고 과도의 크기를 줄이는 간단한 기법은 변화 지점 추정을 사용하여 궤적의 곡률에서 유의미한 변화를 찾는 것입니다. 그런 다음, 그러한 점을 사용하여 보간을 계산합니다.

% 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')

그러면 이러한 변화 지점을, 궤적의 웨이포인트를 생성하기 위한 시작점으로 사용할 수 있습니다.

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')

결과를 보면, 단 몇 개의 웨이포인트와 그에 대응하는 속도를 사용하여 원형 궤적을 복구했습니다. 한편, 반경은 추적 필터가 예측하기에 어려운 영역일 수 있는 천이 영역 내에서 추정된 속도의 탄젠트 각도에 민감합니다.

평활화된 추정값에 대해 복원된 궤적의 위치 오차의 크기를 플로팅하는 경우, 큰 오차가 있는 국소화된 영역들을 볼 수 있습니다.

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

웨이포인트가 평활화된 대응 웨이포인트에 가까운 영역에서는 밀접하게 일치하고 웨이포인트에서 더 멀리 떨어진 영역에서는 편차가 더 커진다는 것을 확인할 수 있습니다. 오차를 줄이는 간단한 방법은 원하는 목표를 달성할 때까지 최대 오차 위치에 추가 웨이포인트를 반복적으로 추가하는 것입니다. 이 예제에서 최대 오차에 대해 10미터를 목표로 설정하는 경우 웨이포인트는 몇 개만 더 필요합니다.

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')

요약

이 예제에서는 IMM(Interacting Multiple Model) 스무더와 낮은 차수의 다항식을 사용하여 ground truth 궤적을 복원하는 방법을 알아보았습니다. 또한 잡음으로 마스크 처리될 수 있는 높은 차수의 도함수를 복구하기 위해 중요한 샘플링과 함께 높은 차수의 보간(waypointTrajectory 객체에 포함되어 있음)을 사용하는 간단한 방식을 사용했습니다.