Chapter 09 위치로 속도 추정하기

예제 9-1

80m/s로 움직이는 물제의 위치 정보를 0.1초 간격으로 알고 있을 때 속도를 추정

9.1 시스템 모델

속도를 구하기 위해 거리에 대해 미분을 하면 분모 대비 잡음이 커서 들쭉날쭉 할 수 있다.

이 시스템 모델에서 상태 변수는 위치와 속도이다. 즉,
$x=\begin{pmatrix}position \\ velocity\end{pmatrix}$

이다. 따라서,

$x_{k+1}=Ax_k+w_k$
$z_k=Hx_k+v_k$

$A=\begin{pmatrix}1 & \Delta t \\ 0 & 1\end{pmatrix}$
$H=\begin{pmatrix}1 & 0\end{pmatrix}$

이 된다. $\Delta t$는 위치를 측정하는 시간 간격을 의미한다. 여기서 $w_k=\begin{pmatrix}0 \\ 1\end{pmatrix}$이 된다.

또한,

$z_k=Hx_k+v_k=\begin{pmatrix}1 & 0\end{pmatrix}\begin{pmatrix}position \\ velocity\end{pmatrix}+v_k=position+v_k$

이 된다.

위에서 알 수 있듯이 행렬 $A$와 $H$는 임의로 선정한 것이 아니라 시스템의 물리적인 관계를 모델링한 결과물이다.

마지막으로 공분산 행렬을 결정하자. 측정 잡음인 $v_k$는 센서의 제작사에서 제공하는 경우가 많지만, 없다면 실험과 경험을 통해 결정해야 한다. 마찬가지로 $Q$는 시스템에 대한 경험에 의존해야 한다. 만약 논리적으로 공분산 행렬을 구하기 어렵다면, 칼만 필터의 설계 인자로 간주해 시행착오를 거쳐야 한다. 우선 아래와 같이 정의하자.

$Q=\begin{pmatrix}1 & 0 \\ 0 & 3\end{pmatrix}$
$R=10$

9.2 칼만 필터 함수

(생략)

9.3 테스트 프로그램

(생략)

9.4 속도로 위치 추정하기

위 수식에서 $H$를 아래와 같이 바꿔주면 위치 추정이 가능하다.
$H=\begin{pmatrix}0 & 1\end{pmatrix}$

9.5 초음파 거리계로 속도 추정하기

(생략)

9.6 효율적인 칼만 필터 함수

칼만 이득을 구할 때 역행렬 연산이 필요한데, 행렬 차원이 낮은 경우 직접 손계산으로 넣어주면 계산 속도가 증가한다.

9.7 시스템 모델의 힘

저주파 통과 필터의 경우에는 새 측정값과 추정값에 가중치를 주고 더하기만 한다. 따라서 측정하지 않은 물리량을 추정하는 것은 불가능하다.

하지만 칼만 필터는 시스템의 수학적 모델을 알고 있다고 가정한다. 즉 시스템이 어떤 법칙에 따라 측정값을 출력하는지 알고 있다는 전제를 깔고 있다.