파이썬 로보틱스 - 4. EKF

-ㅁ너ㅐㅔ채ㅔㄷㅂ퍼ㅐㅔㅐㅔㅓㄷ패ㅔㅍㄷ재ㅔㅏㅜㅇ류라ㅡㄷㅍ자ㅡ

 

 

이번에 정리하고자 하는 내용은 확장 칼만필터

 

확장 칼만 필터 EKF

확장 칼만 필터는 기존의 칼만필터의 상태 전이와 측정이 선형 함수 형태로 이루어져 있었는데

대부분 비선형적인 동작을 하는 현실 문제들에는 부적합함

 

칼만 필터에서 선형함수를 사용하여 다음 상태, 관측치를 표현한 것 대신

비선형 함수 g와 h를 사용해서 계산하는데

 

 

잠깐 가우시안 분포가 선형 변환을 하면 어떻게 되나 보자

(칼만 필터에서는 다음 상태를 구할때 하던 그거)

선형 변환때는 문제 없이 가우시안 분포를 따르는 다음 상태 p(y)를 구할수 있다.

 

 

하지만 문제는 비선형 변환을 할때이다.

가우시안 분포 p(x)를 비선형 함수 g(x)에 넣었더니

지금까지 계속 사용했던 가우시안 분포의 형태가 나오지 않아서 사용할수 없다.

* 파란 가우시안 분포는 p(y)의 평균과 분산을 계산해 만든 가우시안. 

 

 

 

대신 비선형 함수의 해당 지점을 1차 테일러 전개, 선형 근사를 하여 p(x)를 대입시키면

(비선형 함수의 1차 근사식으로 가우시안 분포를 선형 변환)

파란 가우시안 분포에 가까운 (근사화 오차를 가진, 빨간) 가우시안 분포가 나온다.

 

위에선 비선형 함수를 선형 근사하여 어떻게 가우시안 분포를 적용시켰는지 봤는데

 

비선형 함수를 이제 1차 테일러 전개, 선형화하면 아래와 같이 표현 

여기서 G_t는 비선형 함수를 상태와 제어에 대해 편미분하여 구한 자코비안 행렬

비선형 함수에대한 선형 근사식으로

상태 전이 확률을 정리하면 다음과 같음

관측 비선형 함수도 선형 근사를 통해 표현할수 있으며, 관측 확률도 가우시안으로 다음과 같음

 

 

 

 

확장 칼만 필터 알고리즘

기존의 칼만 필터랑 거의 비슷한데 

상태 전이행렬 A, 제어 행렬 B, 관측 행렬 C대신

비선형 함수 f, u와 자코비안 G가 들어간 형태

 

구현 코드랑 같이 봐야 이게 뭔가 이해되겠지만

아직 그전에 짚고 넘어갈게 좀남아서 바로 구현하질 못하겟네 ㅜㅜ