[1회차] Kalman Filter

Kalman Filter

칼만 필터, Kalman Filter: 잡음이 포함되어 있는 측정치를 바탕으로 선형 역학계의 상태를 추정하는 재귀 필터.

• 루돌프 칼만이 개발
• 컴퓨터 비전, 로봇 공학, 레이다 등의 여러 분야에서 사용
• 과거에 수행한 측정값을 바탕으로 현재의 상태 변수의 결합분포를 추정

알고리즘 구성

1. 예측
   현재 상태 변수의 값과 정확도를 예측
2. 업데이트
   이전에 추정한 상태 변수를 기반으로 예측한 측정치와 실제 측정치의 차이를 반영해 현재의 상태 변수를 업데이트

칼만 필터의 적용 분야

물체의 측정값에 확률적인 오차가 포함되고, 또한 물체의 특정 시점에서의 상태가 이전 시점의 상태와 선형적인 관계를 가지고 있는 경우 적용 가능.

 

ex) 레이다 추적

• 특정 물체의 위치, 속도, 가속도 등을 측정할 수 있지만 이 측정값에 오차가 포함되어 있을 수 있음.
• 이 경우 연속적으로 측정하는 값들을 칼만 필터를 이용해 물체의 위치를 추정할 수 있음.

칼만 필터의 동적 시스템 모델

이산 시간 선형 동적 시스템을 기반으로 동작

각 시간에서의 상태 벡터는 이전 시간의 상태 벡터를 통해서 결정된다는 마르코프 연쇄를 가정

• 특정 시간 $k$에서의 상태 벡터를 $x_k$라고 정의하고, 그 시간에서의 사용자 입력을 $u_k$라고 정의할 때, 칼만 필터에서는 다음과 같은 관계식을 가정한다.

$$x_k=F_k{x}_{(k-1)}+B_k{u}_k+w_k$$

$$F_k=(해당시간에서이전상태에기반한상태전이행렬)$$

$$B_k=(사용자입력에의한상태전이행렬)$$

$$w_k=(공분산행렬Q_k를가지는다변량정규분포w_k\sim N(0,Q_k)잡음변수)$$

 

• 상태 벡터 $x_k$와 그 벡터를 측정했을 때 실제로 얻어진 벡터 $z_k$는 다음과 같은 관계식을 가지고 있다.

$$z_k=H_k{x}_k+v_k$$

$$H_k=(해당시간의상태에서측정값을도출하는행렬)$$

$$v_k=(공분산행렬R_k를가지는다변량정규분포v_k\sim N(0,R_k)잡음변수)$$

 

초기 상태와 각 잡음 변수 ${x_0, w_1,\cdots, w_k,v_1,\cdots, v_k}$ 는 모두 상호 독립이라는 가정이 필요.

칼만 필터의 구조

칼만 필터는 이전 시간에 추정한 값을 토대로 현재의 값을 추정하며, 바로 이전 시간 외의 측정값이나 추정값은 사용하지 않는다.

각 추정 시간은 두 단계로 이루어진다.

1. 예측$prediction$ 단계: 이전 시간에 추정된 상태에 대해 사용자가 입력을 가했을 때 예상되는 측정값 계산
2. 보정$update$ 단계: 앞서 예측된 측정값과 실제 측정값을 토대로 현재의 상태를 추정

각 시간의 추정 상태는 평균과 분산의 두 개의 변수로 표현된다.

• $\hat{x}_{n|m}$: m 시점의 측정값을 토대로 한 n 시점의 상태 추정값
• $P_{n|m}$: m 시점의 측정값을 토대로 한 n 시점의 상태 공분산행렬

예측 단계

• 연역적 상태 예측: $\hat{x}{k|k-1} = F_k\hat{x}{k-1|k-1} + B_ku_k$
• 연역적 공분산 예측: $P_{k|k-1} = F_kP_{k-1|k-1}F_k^T+Q_{k-1}$

보정 단계

• 예측 단계와 실제 측정간의 잔차: $\tilde{y}k = z_k - H_k\hat{x}{k|k-1}$
• 잔차의 공분산: $S_k = H_kP_{k|k-1}H_k^T+R_k$
• 최적 칼만 이득, Kalman gain: $K_k = P_{k|k-1}H_k^TS_k^{-1}$
• 귀납적 상태 보정: $\hat{x}{k|k} = \hat{x}{k|k-1} + K_k\tilde{y}_k$
• 귀납적 상태 공분산 보정: $P_{k|k} = (I-K_kH_k)P_{k|k-1}$

불변량

모델이 정확하고, $\hat{x}{0|0}$와$P{0|0}$의 값이 정확하게 초기 상태 값의 몫을 반영한다면, 다음 불변량은 보존된다.

• $E[x_k-\hat{x}{k|k}] = E[x_k-\hat{x}{k|k-1}] = 0$
• $E[\tilde{y}_k] = 0$

여기서 $E[\xi]$은 $\xi$의 기대값이고, 공분산 행렬은 정확하게 추정의 공분산을 반영한다.

• $P_{k|k} = cov(x_k -\hat{x}_{k|k})$
• $P_{k|k-1} = cov(x_k -\hat{x}_{k|k-1})$
• $S_k = cov(\tilde{y}_k)$