확장 칼만 필터

객체를 추적하기 위해 필터를 사용하는 경우, 검출 또는 측정값의 시퀀스를 사용하여 객체의 모션 모델을 기반으로 객체의 상태를 추정합니다. 모션 모델에서, 상태는 위치, 속도, 가속도와 같은 객체의 상태 정보를 나타내는 수량의 모음입니다. 객체 모션이 비선형 상태 방정식을 따르거나 측정값이 상태에 대한 비선형 함수라면 확장 칼만 필터(trackingEKF)를 사용합니다. 예를 들어 방위각, 고도, 거리와 같은 객체의 측정값이 구면 좌표로 표현되지만 타깃의 상태는 카테시안 좌표로 표현되는 경우, 확장 칼만 필터를 사용하는 것을 고려하십시오.

확장 칼만의 정식화는 상태 방정식과 측정 방정식의 선형화를 기반으로 합니다. 선형화를 사용하면 상태와 상태 공분산을 근사적으로 선형인 형식으로 전파할 수 있으며, 선형화를 위해 상태 방정식과 측정 방정식의 야코비 행렬이 필요합니다.

참고

추정 시스템이 선형인 경우 선형 칼만 필터(trackingKF) 또는 확장 칼만 필터(trackingEKF)를 사용하여 타깃 상태를 추정할 수 있습니다. 시스템이 비선형인 경우에는 확장 칼만 필터나 무향 칼만 필터(trackingUKF)와 같은 비선형 필터를 사용해야 합니다.

상태 업데이트 모델

예측 상태에 대한 닫힌 형식의 표현식을 이전 상태 x_k, 컨트롤 u_k, 잡음 w_k, 시간 t의 함수로 가정합니다.

$x_{k + 1} = f (x_{k}, u_{k}, w_{k}, t)$

이전 상태에 대한 예측 상태의 야코비 행렬은 편도함수를 통해 다음과 같이 구합니다.

$F^{(x)} = \frac{\partial f}{\partial x} .$

잡음에 대한 예측 상태의 야코비 행렬은 다음과 같습니다.

$F^{(w)} = \frac{\partial f}{\partial w} .$

이러한 함수는 잡음이 상태 업데이트 방정식에서 가산성 잡음인 경우 더 간단한 형식을 취합니다.

$x_{k + 1} = f (x_{k}, u_{k}, t) + w_{k}$

이 경우 F^(w)는 단위 행렬입니다.

trackingEKF 객체의 StateTransitionJacobianFcn 속성을 사용하여 상태 야코비 행렬을 지정할 수 있습니다. 이 속성을 지정하지 않으면 객체는 수치적 차분을 사용하여 야코비 행렬을 계산합니다. 이런 경우 정확도가 약간 떨어지고 계산 시간이 늘어날 수 있습니다.

측정값 모델

확장 칼만 필터에서 측정값은 상태와 측정 잡음에 대한 비선형 함수일 수도 있습니다.

$z_{k} = h (x_{k}, v_{k}, t)$

상태에 대한 측정값의 야코비 행렬은 다음과 같습니다.

$H^{(x)} = \frac{\partial h}{\partial x} .$

측정 잡음에 대한 측정값의 야코비 행렬은 다음과 같습니다.

$H^{(v)} = \frac{\partial h}{\partial v} .$

이러한 함수는 잡음이 측정값 방정식에서 가산성 잡음인 경우 더 간단한 형식을 취합니다.

$z_{k} = h (x_{k}, t) + v_{k}$

이 경우 H^(v)는 단위 행렬입니다.

trackingEKF에서, MeasurementJacobianFcn 속성을 사용하여 측정값 야코비 행렬을 지정할 수 있습니다. 이 속성을 지정하지 않으면 객체는 수치적 차분을 사용하여 야코비 행렬을 계산합니다. 이런 경우 정확도가 약간 떨어지고 계산 시간이 늘어날 수 있습니다.

확장 칼만 필터 루프

확장 칼만 필터 루프는 다음 사항을 제외하면 Linear Kalman Filters의 루프와 거의 동일합니다.

이 필터는 가능한 경우 항상 정확한 비선형 상태 업데이트 함수와 측정 함수를 사용합니다.
상태 야코비 행렬이 상태 천이 행렬을 대체합니다.
측정값 야코비 행렬이 측정값 행렬을 대체합니다.

미리 정의된 확장 칼만 필터 함수

툴박스는 trackingEKF에서 사용할 수 있는 미리 정의된 상태 업데이트 함수와 측정 함수를 제공합니다.

모션 모델	함수 이름	함수 용도	상태 표현
일정 속도	`constvel`	일정 속도 상태 업데이트 모델	1차원 — `[x;vx]` 2차원 — `[x;vx;y;vy]` 3차원 — `[x;vx;y;vy;z;vz]` 여기서 `x`, `y`, `z`는 각각 x 방향, y 방향, z 방향에서의 위치를 나타냅니다. `vx`, `vy`, `vz`는 각각 x 방향, y 방향, z 방향에서의 속도를 나타냅니다.
	`constveljac`	일정 속도 상태 업데이트 야코비 행렬
	`cvmeas`	일정 속도 측정값 모델
	`cvmeasjac`	일정 속도 측정값 야코비 행렬
일정 가속도	`constacc`	일정 가속도 상태 업데이트 모델	1차원 — `[x;vx;ax]` 2차원 — `[x;vx;ax;y;vy;ay]` 3차원 — `[x;vx;ax;y;vy;ay;z;vz;az]` 여기서 `ax`, `ay`, `az`는 각각 x 방향, y 방향, z 방향의 가속도를 나타냅니다.
	`constaccjac`	일정 가속도 상태 업데이트 야코비 행렬
	`cameas`	일정 가속도 측정값 모델
	`cameasjac`	일정 가속도 측정값 야코비 행렬
등선회율	`constturn`	등선회율 상태 업데이트 모델	2차원 — `[x;vx;y;vy;omega]` 3차원 — `[x;vx;y;vy;omega;z;vz]` 여기서 `omega`는 선회율을 나타냅니다.
	`constturnjac`	등선회율 상태 업데이트 야코비 행렬
	`ctmeas`	등선회율 측정값 모델
	`ctmeasjac`	등선회율 측정값 야코비 행렬

예: `trackingEKF`를 사용하여 각도 측정값과 거리 측정값으로 2차원 타깃 상태 추정하기

라이브 스크립트 열기

추정 모델 초기화하기

타깃이 다음과 같은 초기 위치와 속도로 2차원적으로 움직인다고 가정합니다. 시뮬레이션은 0.2초의 샘플 시간으로 20초 동안 지속됩니다.

rng(2022);    % For repeatable results
dt = 0.2;     % seconds
simTime = 20; % seconds
tspan = 0:dt:simTime;
trueInitialState = [30; 1; 40; 1]; % [x;vx;y;vy]
initialCovariance = diag([100,1e3,100,1e3]);
processNoise = diag([0; .01; 0; .01]); % Process noise matrix

측정값은 x축의 양의 방향에 대한 방위각과 원점에서 타깃까지의 거리라고 가정합니다. 측정 잡음 공분산 행렬은 다음과 같습니다.

measureNoise = diag([2e-6;1]); % Measurement noise matrix. Units are m^2 and rad^2.

결과를 저장할 변수를 사전할당합니다.

numSteps = length(tspan);
trueStates = NaN(4,numSteps);
trueStates(:,1) = trueInitialState;
estimateStates = NaN(size(trueStates));
measurements = NaN(2,numSteps);

실제 상태 및 측정값 구하기

일정 속도 모델을 전파하고 잡음이 있는 측정값을 생성합니다.

for i = 2:length(tspan)
    if i ~= 1
        trueStates(:,i) = stateModel(trueStates(:,i-1),dt) + sqrt(processNoise)*randn(4,1);  
    end
    measurements(:,i) = measureModel(trueStates(:,i)) + sqrt(measureNoise)*randn(2,1);
end

실제 궤적과 측정값을 플로팅합니다.

figure(1)
plot(trueStates(1,1),trueStates(3,1),"r*",DisplayName="Initial Truth")
hold on
plot(trueStates(1,:),trueStates(3,:),"r",DisplayName="True Trajectory")
xlabel("x (m)")
ylabel("y (m)")
title("True Trajectory")
axis square

Figure contains an axes object. The axes object with title True Trajectory, xlabel x (m), ylabel y (m) contains 2 objects of type line. One or more of the lines displays its values using only markers These objects represent Initial Truth, True Trajectory.

figure(2)
subplot(2,1,1)
plot(tspan,measurements(1,:)*180/pi)
xlabel("time (s)")
ylabel("angle (deg)")
title("Angle and Range")
subplot(2,1,2)
plot(tspan,measurements(2,:))
xlabel("time (s)")
ylabel("range (m)")

Figure contains 2 axes objects. Axes object 1 with title Angle and Range, xlabel time (s), ylabel angle (deg) contains an object of type line. Axes object 2 with xlabel time (s), ylabel range (m) contains an object of type line.

확장 칼만 필터 초기화하기

[35; 0; 45; 0]의 초기 상태 추정값으로 필터를 초기화합니다.

filter = trackingEKF(State=[35; 0; 45; 0],StateCovariance=initialCovariance, ...
    StateTransitionFcn=@stateModel,ProcessNoise=processNoise, ...
    MeasurementFcn=@measureModel,MeasurementNoise=measureNoise);
estimateStates(:,1) = filter.State;

확장 칼만 필터를 실행하고 결과 표시하기

predict 객체 함수와 correct 객체 함수를 재귀적으로 호출하여 필터를 실행합니다.

for i=2:length(tspan)
    predict(filter,dt);
    estimateStates(:,i) = correct(filter,measurements(:,i));
end
figure(1)
plot(estimateStates(1,1),estimateStates(3,1),"g*",DisplayName="Initial Estimate")
plot(estimateStates(1,:),estimateStates(3,:),"g",DisplayName="Estimated Trajectory")
legend(Location="northwest")
title("True Trajectory vs Estimated Trajectory")

Figure contains an axes object. The axes object with title True Trajectory vs Estimated Trajectory, xlabel x (m), ylabel y (m) contains 4 objects of type line. One or more of the lines displays its values using only markers These objects represent Initial Truth, True Trajectory, Initial Estimate, Estimated Trajectory.

헬퍼 함수

stateModel은 공정 잡음 없이 일정 속도 모션을 모델링합니다.

function stateNext = stateModel(state,dt)
    F = [1 dt 0  0; 
         0  1 0  0;
         0  0 1 dt;
         0  0 0  1];
    stateNext = F*state;
end

meausreModel은 잡음 없이 거리 측정값과 방위각 측정값을 모델링합니다.

function z = measureModel(state)
    angle = atan(state(3)/state(1));
    range = norm([state(1) state(3)]);
    z = [angle;range];
end

참고 항목

trackingKF | trackingEKF | trackingUKF | Linear Kalman Filters