Main Content

이 페이지의 최신 내용은 아직 번역되지 않았습니다. 최신 내용은 영문으로 볼 수 있습니다.

시변 칼만 필터를 사용한 상태 추정

이 예제에서는 Simulink에서 시변 칼만 필터를 사용하여 선형 시스템의 상태를 추정하는 방법을 보여줍니다. Control System Toolbox 라이브러리에서 Kalman Filter 블록을 사용하여 잡음이 있는 위치 측정값(예: GPS 센서 측정값)을 기반으로 지상 차량의 위치와 속도를 추정합니다. 칼만 필터의 플랜트 모델은 시변 잡음 특성을 갖습니다.

소개

북쪽과 동쪽 방향에서 지상 차량의 위치와 속도를 추정하려고 합니다. 차량은 2차원 공간에서 아무 제약 없이 자유롭게 이동할 수 있습니다. 차량뿐 아니라 모든 물체에 사용할 수 있는 다목적 내비게이션 및 위치 추적 시스템을 설계합니다.

$x_e(t)$$x_n(t)$는 원점으로부터 차량의 동쪽 및 북쪽 위치이고 $\theta(t)$는 동쪽에서 차량 방향이며 $u_\psi(t)$는 차량의 조향각입니다. $t$는 연속시간 변수입니다.

Simulink 모델은 두 가지 주요 부분, 즉 차량 모델과 칼만 필터로 구성됩니다. 이에 대해서는 다음 섹션에서 자세히 설명합니다.

open_system('ctrlKalmanNavigationExample');

차량 모델

추적하는 차량은 다음과 같은 간단한 점질량(point-mass) 모델로 표현됩니다.

$$ \frac{d}{dt} \left[
\begin{array} {c}
 x_e(t) \\
 x_n(t) \\
 s(t) \\
 \theta(t)
\end{array} \right] = \left[
\begin{array} {c}
 s(t)\cos(\theta(t)) \\
 s(t)\sin(\theta(t)) \\
 (P \; \frac{u_T(t)}{s(t)} - A \; C_d \; s(t)^2) / m \\
 s(t) \tan(u_\psi(t)) / L
\end{array} \right]
$$

여기서 차량 상태는 다음과 같습니다.

$$ \begin{array} {ll}
x_e(t) \; & \textnormal{East position} \; [m] \\
x_n(t) \; & \textnormal{North position} \; [m] \\
s(t) \; & \textnormal{Speed} \; [m/s] \\
\theta(t) \; & \textnormal{Orientation from east} \; [deg] \\
\end{array} $$

차량 파라미터는 다음과 같습니다.

$$ \begin{array} {ll}
P=100000 \; & \textnormal{Peak engine power} \; [W] \\
A=1 \; & \textnormal{Frontal area} \; [m^2] \\
C_d=0.3 \; & \textnormal{Drag coefficient} \; [Unitless] \\
m=1250 \; & \textnormal{Vehicle mass} \; [kg] \\
L=2.5 \; & \textnormal{Wheelbase length} \; [m] \\
\end{array} $$

제어 입력은 다음과 같습니다.

$$ \begin{array} {ll}
u_T(t) \; & \textnormal{Throttle position in the range of -1 and 1} \; [Unitless] \\
u_\psi(t) \; & \textnormal{Steering angle} \; [deg] \\
\end{array} $$

이 모델의 종방향 역학에서는 구름 저항(rolling resistance)을 무시합니다. 이 모델의 횡방향 역학에서는 원하는 조향각에 즉시 도달할 수 있는 것으로 가정하며 요(yaw) 관성 모멘트를 무시합니다.

차량 모델은 ctrlKalmanNavigationExample/Vehicle Model 서브시스템에 구현되어 있습니다. 이 Simulink 모델의 ctrlKalmanNavigationExample/Speed And Orientation Tracking 서브시스템에는 차량의 원하는 방향과 속도를 추종하기 위한 두 개의 PI 제어기가 포함되어 있습니다. 따라서 차량에 다양한 기동 조건을 지정하여 칼만 필터의 성능을 테스트할 수 있습니다.

칼만 필터 설계

칼만 필터는 선형 모델을 기반으로 미지의 관심 변수를 추정하는 알고리즘입니다. 이 선형 모델은 모델 초기 조건뿐 아니라 알려진 모델 입력과 알 수 없는 모델 입력에 대한 응답으로 시간에 따른 추정된 변수의 변화를 설명합니다. 이 예제에서는 다음과 같은 파라미터/변수를 추정합니다.

$$ \hat{x}[n] = \left[
 \begin{array}{c}
 \hat{x}_e[n] \\
 \hat{x}_n[n] \\
 \hat{\dot{x}}_e[n] \\
 \hat{\dot{x}}_n[n]
 \end{array}
 \right]
$$

여기서 각각은 다음을 나타냅니다.

$$ \begin{array} {ll}
\hat{x}_e[n] \; & \textnormal{East position estimate} \; [m] \\
\hat{x}_n[n] \; & \textnormal{North position estimate} \; [m] \\
\hat{\dot{x}}_e[n] \; & \textnormal{East velocity estimate} \; [m/s] \\
\hat{\dot{x}}_n[n] \; & \textnormal{North velocity estimate} \; [m/s] \\
\end{array} $$

$\dot{x}$ 항은 미분 연산자가 아니라 속도를 나타냅니다. $n$은 이산시간 인덱스입니다. 칼만 필터에 사용되는 모델의 형식은 다음과 같습니다.

$$ \begin{array} {rl}
\hat{x}[n+1] &= A\hat{x}[n] + Gw[n] \\
y[n] &= C\hat{x}[n] + v[n]
\end{array} $$

여기서 $\hat{x}$는 상태 벡터, $y$는 측정값, $w$는 공정 잡음, $v$는 측정 잡음입니다. 칼만 필터는 $w$$v$가 평균 0, 알려진 분산 $E[ww^T]=Q$, $E[vv^T]=R$, $E[wv^T]=N$인 독립적인 확률 변수라고 가정합니다. 여기서 A, G, C 행렬은 다음과 같으며

$$ A = \left[
 \begin{array}{c c c c}
 1 & 0 & T_s & 0 \\
 0 & 1 & 0 & T_s \\
 0 & 0 & 1 & 0 \\
 0 & 0 & 0 & 1
 \end{array}
 \right]
$$

$$ G = \left[
 \begin{array}{c c}
 T_s/2 & 0 \\
 0 & T_s/2 \\
 1 & 0 \\
 0 & 1
 \end{array}
 \right]
$$

$$ C = \left[
 \begin{array}{c c c c}
 1 & 0 & 0 & 0 \\
 0 & 1 & 0 & 0
 \end{array}
 \right]
$$

$T_s=1\;[s]$입니다.

A와 G의 세 번째 행은 동쪽 속도를 임의 보행으로 모델링합니다($\hat{\dot{x}}_e[n+1]=\hat{\dot{x}}_e[n]+w_1[n]$). 실세계에서 위치는 연속시간 변수이며 시간에 따른 속도의 적분입니다($\frac{d}{dt}\hat{x}_e=\hat{\dot{x}}_e$). A와 G의 첫 번째 행은 이 운동학적 관계에 대한 이산 근삿값을 나타냅니다($(\hat{x}_e[n+1]-\hat{x}_e[n])/Ts=(\hat{\dot{x}}_e[n+1]+\hat{\dot{x}}_e[n])/2$). A와 G의 두 번째 및 네 번째 행은 북쪽 속도와 위치 간의 동일한 관계를 나타냅니다.

C 행렬은 위치 측정값만 사용할 수 있음을 나타냅니다. GPS와 같은 위치 센서는 1Hz의 샘플 레이트에서 이러한 측정값을 제공합니다. 측정 잡음 $v$의 분산인 R 행렬은 $R=50$으로 지정됩니다. R은 스칼라로 지정되므로 Kalman Filter 블록은 행렬 R이 대각 행렬이고 대각선 요소는 50이며 y와 호환되는 차원을 갖는 것으로 가정합니다. 측정 잡음이 가우스 분포이면 R=50은 위치 측정값이 동쪽과 북쪽 방향의 실제 위치로부터 $\pm\sqrt{50}\;m$ 이내의 거리에 속할 확률이 68%라는 의미입니다. 그러나 칼만 필터의 경우 이 가정은 불필요합니다.

$w$의 요소는 차량 속도가 하나의 샘플 시간 Ts에 얼마나 변화할 수 있는지를 나타냅니다. 공정 잡음 w의 분산인 Q 행렬은 시변으로 선택되었습니다. 이것은 속도가 크면 $w[n]$의 일반적인 값은 더 작다는 직관을 반영합니다. 예를 들어, 0m/s에서 10m/s로 올리는 것이 10m/s에서 20m/s로 올리는 것보다 쉽습니다. 구체적으로, 다음과 같이 추정된 북쪽 및 동쪽 속도와 포화 함수를 사용하여 Q[n]을 생성합니다.

$$f_{sat}(z)=min(max(z,25),625)$$

$$ Q[n] = \left[
 \begin{array}{ c c }
 \displaystyle 1+\frac{250}{f_{sat}(\hat{\dot{x}}_e^2)} & 0 \\
 0 & \displaystyle 1+\frac{250}{f_{sat}(\hat{\dot{x}}_n^2)}
 \end{array}
 \right]
$$

Q의 대각선 요소는 추정된 속도의 제곱에 반비례하는 w의 분산을 모델링합니다. 포화 함수는 Q가 너무 크거나 너무 작지 않도록 방지합니다. 계수 250은 일반 차량의 0~5, 5~10, 10~15, 15~20, 20~25m/s 가속 시간 데이터에 최소제곱 피팅을 사용하여 구한 것입니다. 대각선 Q에 대한 선택은 북쪽과 동쪽 방향의 속도 변화가 상관 관계가 없다고 단순하게 가정했음을 나타냅니다.

Kalman Filter 블록 입력 및 설정

'Kalman Filter' 블록은 Simulink의 Control System Toolbox 라이브러리에 있습니다. 이 블록은 또한 System Identification Toolbox/Estimators 라이브러리에도 있습니다. 이산시간 상태 추정에 대해 블록 파라미터를 구성합니다. 다음 필터 설정 파라미터를 지정합니다.

  • 시간 영역: 이산시간. 이 옵션을 선택하면 이산시간 상태를 추정할 수 있습니다.

  • 현재 측정값 y[n]을 사용하여 xhat[n] 향상 체크박스를 선택합니다. 그러면 이산시간 칼만 필터의 "현재 추정기" 변형이 구현됩니다. 이 옵션은 추정 정확도를 향상시키며 느린 샘플 시간에 더 유용합니다. 반면, 계산 비용이 증가합니다. 또한 이 칼만 필터 변형에는 직접 피드스루가 있으며 이는 칼만 필터가 어떤 지연도 포함되지 않은 피드백 루프에 사용되는 경우 대수 루프를 발생시킵니다(피드백 루프 자체에도 직접 피드스루가 있음). 대수 루프는 시뮬레이션 속도에도 영향을 미칠 수 있습니다.

옵션 탭을 클릭하여 블록 인포트 및 아웃포트 옵션을 설정합니다.

  • 입력 포트 u 추가 체크박스를 선택 취소합니다. 플랜트 모델에 알려진 입력이 없습니다.

  • 상태 추정 오차의 공분산 Z 출력 체크박스를 선택합니다. Z 행렬은 필터의 상태 추정값에 대한 신뢰성 정보를 제공합니다.

모델 파라미터를 클릭하여 플랜트 모델 및 잡음 특성을 지정합니다.

  • 모델 소스: 개별 A, B, C, D 행렬.

  • A: A. A 행렬은 이 예제의 앞부분에서 정의되었습니다.

  • C: C. C 행렬은 이 예제의 앞부분에서 정의되었습니다.

  • 초기 추정값 소스: 대화 상자

  • 초기 상태 x[0]: 0. 이 값은 t=0s에서 위치 및 속도 추정값의 초기 추측값이 0임을 나타냅니다.

  • 상태 추정 오차의 공분산 P[0]: 10. 초기 추측값 x[0]과 실제 값 간의 오차는 표준편차가 $\sqrt{10}$인 확률 변수라고 가정합니다.

  • G 행렬 및 H 행렬 사용(디폴트 값: G=I 및 H=0) 체크박스를 선택하여 디폴트가 아닌 G 행렬을 지정합니다.

  • G: G. G 행렬은 이 예제의 앞부분에서 정의되었습니다.

  • H: 0. 공정 잡음은 Kalman Filter 블록에 들어가는 측정값 y에 영향을 미치지 않습니다.

  • 시불변 Q 체크박스를 선택 취소합니다. Q 행렬은 시변 행렬이며 블록 인포트 Q를 통해 제공됩니다. 이 설정으로 인해 블록은 시변 칼만 필터를 사용합니다. 이 옵션을 선택하면 시불변 칼만 필터를 사용할 수 있습니다. 시불변 칼만 필터는 이 문제에 대해 성능은 약간 떨어지지만 설계가 더 쉽고 계산 비용이 낮습니다.

  • R: R. 측정 잡음 $v[n]$의 공분산입니다. R 행렬은 이 예제의 앞부분에서 정의되었습니다.

  • N: 0. 공정 잡음과 측정 잡음 간에 상관관계가 없다고 가정합니다.

  • 샘플 시간(상속된 경우 -1): Ts. 이 예제의 앞부분에서 정의되었습니다.

결과

차량이 다음과 같이 기동하는 시나리오를 시뮬레이션하여 칼만 필터의 성능을 테스트합니다.

  • t = 0에서 차량 위치는 $x_e(0)=0$, $x_n(0)=0$이고 정지 상태입니다.

  • 동쪽 방향으로 25m/s로 가속합니다. t = 50s에서 5m/s로 감속합니다.

  • t = 100s에서 북쪽으로 방향을 돌려 20m/s로 가속합니다.

  • t = 200s에서 다시 서쪽으로 방향을 돌립니다. 25m/s로 가속합니다.

  • t = 260s에서 15m/s로 감속하고 일정한 속도로 180도 방향을 돌립니다.

Simulink 모델을 시뮬레이션합니다. 차량 위치의 실제 값, 측정된 값, 칼만 필터 추정값을 플로팅합니다.

sim('ctrlKalmanNavigationExample');

figure;
% Plot results and connect data points with a solid line.
plot(x(:,1),x(:,2),'bx',...
    y(:,1),y(:,2),'gd',...
    xhat(:,1),xhat(:,2),'ro',...
    'LineStyle','-');
title('Position');
xlabel('East [m]');
ylabel('North [m]');
legend('Actual','Measured','Kalman filter estimate','Location','Best');
axis tight;

측정된 위치와 실제 위치 간의 오차 및 칼만 필터 추정값과 실제 위치 간의 오차는 다음과 같습니다.

% East position measurement error [m]
n_xe = y(:,1)-x(:,1);
% North position measurement error [m]
n_xn = y(:,2)-x(:,2);
% Kalman filter east position error [m]
e_xe = xhat(:,1)-x(:,1);
% Kalman filter north position error [m]
e_xn = xhat(:,2)-x(:,2);

figure;
% East Position Errors
subplot(2,1,1);
plot(t,n_xe,'g',t,e_xe,'r');
ylabel('Position Error - East [m]');
xlabel('Time [s]');
legend(sprintf('Meas: %.3f',norm(n_xe,1)/numel(n_xe)),sprintf('Kalman f.: %.3f',norm(e_xe,1)/numel(e_xe)));
axis tight;
% North Position Errors
subplot(2,1,2);
plot(t,y(:,2)-x(:,2),'g',t,xhat(:,2)-x(:,2),'r');
ylabel('Position Error - North [m]');
xlabel('Time [s]');
legend(sprintf('Meas: %.3f',norm(n_xn,1)/numel(n_xn)),sprintf('Kalman f: %.3f',norm(e_xn,1)/numel(e_xn)));
axis tight;

플롯 범례는 데이터 점의 개수로 정규화된 위치 측정값과 추정 오차($||x_e-\hat{x}_e||_1$$||x_n-\hat{x}_n||_1$)를 표시합니다. 칼만 필터 추정값이 원시 측정값보다 오차가 약 25% 적습니다.

동쪽 방향의 실제 속도와 칼만 필터 추정값은 아래 그림의 위쪽 플롯에 표시되어 있습니다. 아래쪽 플롯은 추정 오차를 표시합니다.

e_ve = xhat(:,3)-x(:,3); % [m/s] Kalman filter east velocity error
e_vn = xhat(:,4)-x(:,4); % [m/s] Kalman filter north velocity error
figure;
% Velocity in east direction and its estimate
subplot(2,1,1);
plot(t,x(:,3),'b',t,xhat(:,3),'r');
ylabel('Velocity - East [m]');
xlabel('Time [s]');
legend('Actual','Kalman filter','Location','Best');
axis tight;
subplot(2,1,2);
% Estimation error
plot(t,e_ve,'r');
ylabel('Velocity Error - East [m]');
xlabel('Time [s]');
legend(sprintf('Kalman filter: %.3f',norm(e_ve,1)/numel(e_ve)));
axis tight;

오차 플롯의 범례는 데이터 점의 개수로 정규화된 동쪽 속도 추정 오차 $||\dot{x}_e-\hat{\dot{x}}_e||_1$을 표시합니다.

칼만 필터의 속도 추정값이 실제 속도의 추세를 정확하게 추종합니다. 차량이 높은 속도로 이동하면 잡음 수준이 감소합니다. 이는 Q 행렬의 설계와 부합합니다. t=50s 및 t=200s에 두 개의 큰 스파이크가 있습니다. 이 시점은 각각 자동차가 갑자기 감속하고 급히 방향을 돌린 시간입니다. 이 순간의 속도 변화는 Q 행렬 입력을 기반으로 하는 칼만 필터의 예측값보다 훨씬 큽니다. 몇 번의 시간 스텝 후에는 필터의 추정값이 실제 속도를 따라잡습니다.

요약

Simulink에서 Kalman Filter 블록을 사용하여 차량의 위치와 속도를 추정했습니다. 이 모델의 공정 잡음 동특성은 시간에 따라 변화했습니다. 다양한 차량 기동과 무작위로 생성된 측정 잡음을 시뮬레이션하여 필터 성능을 확인했습니다. 칼만 필터를 통해 차량의 위치 측정값을 향상시키고 속도 추정값을 얻을 수 있었습니다.

bdclose('ctrlKalmanNavigationExample');

관련 항목