Kalman Filter

👩🏻‍💻💭 칼만필터 .. 이 기회에 제대로 공부해보고 싶어서 적어봅니다 !

 

자율주행: 기계가 이동하면서 스스로 장애물을 피하고(변수 감지), 최적의 경로를 찾아가는(주행 제어) 기능

⇨ 이를 위해서는 정확한 지도(Map) 즉, 센서와 데이터를 통한 맵핑(Mapping)이 필요하다. (위치 측정(Localization))

 

지도(Map)가 주어지면, 로봇은 장착된 센서(ex. 초음파, 라이더, 카메라)들을 통해 확률기반의 방법으로 "예측"한다.

⇨ 모든 관측값은 "오차(Error)"를 가지게 된다.

⇨ 이를 줄이기 위해 필요한 것이 "칼만 필터(Kalman Filter)"이다.

 

✔️정의

칼만 필터의 기본개념은, 과거와 현재의 값을 기준으로 재귀적 연산을 통해 최적값을 추적하는 것이다.

필터란, 측정 데이터에 포함된 "노이즈"를 필터링 하는 것 = 대상으로부터 얻는 데이터가 완벽할 수 없기에 부족한 성질을 보충하는 것

즉, 측정 데이터/신호는 반드시 노이즈를 지니므로 여기서 원하는 신호나 정보를 골라내는 알고리즘이다.

(확률에 기반한 예측 시스템이므로 이것의 대상은 정규분포(가우시안분포)를 가지는 대상이 된다.)

 

두 가지 모델을 고려해야한다.

① 모션 모델(motion model)

② 측정 모델(measurement model)

 

칼만필터 알고리즘은 두 가지 가정이 갖추어져야한다.

① 모션 모델과 측정 모델이 "Linear" 하다

② 모션 모델과 측정 모델이 "Gaussian 분포"를 따른다

⇨ 실제 시스템은 대부분 비선형이고 가우시안 분포를 따르지 않음 → Extended Kalman Filter(EKF)

⇨ 선형 시스템에서는 엄청난 도구

 

상태 예측(state prediction)과 측정 업데이트(measurement update)를 반복적으로 수행하며 로봇의 현재 위치를 계산한다.

 

상태 예측 

↳ 이전 측정 업데이트에서 계산한 확률분포와 로봇 모션 입력의 확률분포를 통해 현재 상태의 분포를 예측한다.

 

측정 업데이트

↳ 상태 예측에서 예측된 현재 로봇 위치에 대한 확률분포와 현재 로봇의 위치에서 측정한 관찰값의 확률분포를 통해 사후 확률분포를 업데이트한다.

 

Ex. 1차원에서의 현재 위치 계산

- 상태 예측

이전 측정 업데이트에서 계산한 확률분포와 로봇 모션 입력의 확률분포를 통해 현재 상태의 분포를 예측한다.

이 때, 확률분포의 평균은 두 평균을 더한 것이고 확률분포의 분산은 두 분산을 더한 것이다.

 

- 측정 업데이트

상태 예측에서 예측된 현재 로봇 위치에 대한 확률분포와 현재 로봇의 위치에서 측정한 관찰값의 확률분포를 통해 사후 확률분포를 업데이트한다.

예를 들어, 로봇이 이전에 20미터 거리에 있었고 분산이 9미터였다.

현재 로봇의 위치에서 측정한 센서의 값은 30미터이고 분산은 3미터이다.

업데이트 된 새로운 가우시간 확률분포 = prior 가우시안분포와 measurement 가우시안분포의 곱

계산 결과, 새로운 가우시안 분포의 평균과 분산은 다음과 같다.

이를 통해 측정 업데이트를 수행하면 다음과 같은 사후 확률분포를 얻을 수 있다.

이것이 업데이트된 로봇의 위치에 대한 확률분포이다. 

 

 

 

✔️알고리즘

** 각 알파벳 위에 '-'가 있다면 "추정값"이라는 의미, k는 현재스텝, k-1은 직전 과거스텝이라는 의미이다.

Zk	측정값
^Xk	추정값
Pk	^Xk(추정값)의 오차 공분산
^X-k	^Xk(추정값)의 예측값
P-k	Pk의 예측값

A	X^k-1(이전 추정값)을 통해 추정값을 예측할 때 사용되는 행렬
H	X^-k을 측정값의 형태로 변환할 때 필요한 행렬
Q	시스템 노이즈
R	측정값 노이즈

 

① 초기값 선정

이전 스텝에서의 결과값이 그 다음 스텝에서의 초기값이다. (=recursive)

 

② 추정값(^X-k)과 오차 공분산(P-k) 예측

A, Q는 이미 주어진 값이므로 "P"값만 구해주면 된다.

이전 상태에서 구한 P값에 A를 곱하고 Q를 더해주어 현재상태의 P를 추측하는 것이다.

 

이전 단계에서 계산한 추정값(^Xk-1)을 사용하여 시스템모델(A)과의 계산을 통해 새로운 추정값(^X-k)을 예측한다.

이때, 예측한 값이 평균을 기준으로 어느 정도로 분포되어 있는지 이전 오차 공분산(Pk-1)을 사용하여 새로운 오차 공분산(P-k)도 함께 예측한다.

 

③ 칼만 이득(Kk) 계산

H, R도 이미 정해진 값이므로 앞서 구한 P만 있으면 된다.

 

Kk(③)가 커지면 (Zk - ^X-k)가 ^Xk(④)에 많은 영향을, 

Kk(③)가 작아지면 (Zk - ^X-k)가 ^Xk(④)에 적은 영향을 미친다.

 

측정값의 노이즈(R)이 0에 수렴하면 Kk 값이 커짐 → 추정값 계산(④)에서 (Zk - ^X-k) 즉 Zk가 많은 영향을 미치게 된다.

오차 공분산의 예측값(P-k)이 0에 수렴하면 Kk 값이 작아짐 → 추정값 계산(④)에서 Kk 값의 영향이 미미해지고 ^X-k에 의해 결정된다.

 

⇨ 측정값(Zk)의 노이즈(R)가 작아 측정값(Zk)이 더 신뢰적이면 추정값 계산(④)에 측정값(Zk)을 더 믿고,

오차공분산의 예측값(P-k)이 작아 추정값의 예측값(^X-k)이 더 신뢰적이면 추정값 계산(④)에 추정값의 예측값(^X-k)을 더 믿는다.

 

센서노이즈(R) 증가 → 칼만 이득 감소 → 최종 추정값에서 센서에 대한 비중 축소

시스템노이즈(Q) 증가 → 칼만 이득 증가 → 최종 추정값에서 센서에 대한 비중 증대(추정값에 대한 비중 감소)

 

⇨ 센서 노이즈가 크면 센서값을 덜 보고, 시스템 노이즈가 크면 시스템을 덜 보겠다!

 

④ 추정값(^Xk) 계산

앞서 구한 값들과 센서로 측정한 값인 Zk를 통해 현재상태의 값을 추청한다.

 

예측과정에서 계산했던 새로운 추정값(^X-k)에, 입력으로 받은 Zk(측정값)와 새로운 추정값(^X-k)의 차이를 더하여 계산한다.

이 때, 칼만 이득이 사용되고 입력 받은 측정값과 새로운 추정값의 차이(Zk - ^X-k)의 결과에 영향을 미친다.

 

⑤ 오차공분산(Pk) 계산

앞서 구한 값들로 오차공분산 Pk를 구한다.

여기서 얻은 Pk의 값은 다음 스텝의 ①로 돌아가 Pk-1이 된다.

 

+) 칼만 필터를 사용할 때 가장 중요한 것은 시스템 모델(A)이다. 실제 시스템과 최대한 비슷해야 한다.

시스템 모델은 칼만 필터를 수행하면서 칼만 이득(Kk)처럼 변하는 값이 아니라, 동작 전에 미리 설정하는 고정적인 값이므로 얼마나 최적의 값으로 이를 설정하느냐에 따라 칼만 필터의 성능이 달라질 수 있다.