카테고리 없음

Kalman gain 구하기

ksyoon 2019. 5. 8. 23:36

아래의 내용은 KALMAN FILTERING Theory and Practice Using MATLAB 이란 책의 내용을 인용하여 내용을 한글로 정리한 글이다.

1. Estimator in Linear Form

선형 continuous time에서의 plant 방정식과 출력은 다음과 같다.

˙x=F(t)x(t)+w(t)

z(t)=H(t)x(t)+v(t)

Discrete time 에서는

xk=Φk1xx1+wk1

zk=Hkxk+vk

Optimal linear estimate는 x, z가 jointly Gaussian이면 general(nonlinear) optimal estimator와 동일하다. 그러므로 zk를 관찰(observation)하여 ˆxk(+)를 추정(estimate)한다. 즉 priori 추정과 z의 선형 함수가 ˆxk(+) 이다.

ˆxk(+)=K1kˆxk()+ˉKkzk

여기 ˆxk()xk의 priori estimate 이고 ˆxk(+) 는 posteriori estimate이다.

 

2. Kalman Gain 풀기

K1kˉKk는 아직 알려져 알려져 있지 않지만, 적당한 K1kˉKk 를 찾아서 새로운 estimate ˆxk(+) 가 orthogonality principle을 만족 하도록 한다.

E[xkˆxk(+)]zTi=0,i=1,2,...,k1,E[xkˆxk(+)]zTk=0.

여기서 z1,...,zk는 노이즈 텀인 wk 을 포함하고 있지 않다. 그러므로 wkvk는 상관관계가 아니고 EwkzTi=0for1ik

E[Φk1xk1+wk1K1kˆxk()ˉKkzk]zTi=0fori=1,...,k1

그런데, zk=Hkxk+vk 이므로 위식은 아래와 같이 된다.

E[Φk1xk1K1kˆxk()ˉKkHkxkˉKkvk]zTi=0fori=1,...,k1

또한 아래 식도 성립한다.

E[xk1ˆx(k1)(+)]zTk=0i=1,...,k1

그리고

EvkzTk=0i=1,...,k1

E[Φk1xk1K1kˆxk()ˉKkHkxkˉKkvk]zTi=0fori=1,...,k1은 아래와 같이 정리 된다.

Φk1Exk1zTiK1kEˆxk()zTiˉKkHkΦk1Exk1zTiˉKkEvkzTi=0,Φk1Exk1zTiK1kEˆxk()zTiˉKkHkΦk1Exk1zTi=0,E[xkˉKkHkxkK1kxk]zTiK1k(ˆxk()xk)zTi=0,[IK1kˉKkHk]ExkzTi=0.

만약

K1k=IˉKkHk

이면 xk에 상관 없이 위 식이 성립한다.

여기서 선택된 K1kˆxk(+)=K1kˆxk()+ˉKkzk 을 만족하고, 위에 언급된 orthogonality principle도 E[xkˆxk(+)]zTi=0 만족한다. 또한 ˉKkE[xkˆxk(+)]zTk=0을 만족한다.

여기서 에러를 다음과 같이 정의한다.

˜xk(+)=^xk(+)xk,˜xk()=ˆxk()xk,˜zk(+)=ˆzk()zk=Hkˆxk()zk

업데이트 전후의 ˜xk(+),˜xk() 는 각각 estimation error 이고 이 때 파라미터 ˆxk(+)xk에 선형적인 합이며 xk 또한 zk의 선형적인 합이다. 그래서 아래 식이 성립한다.

E[xkˆxk(+)]ˆzTk()=0

또한

E[xkˆxk(+)]˜zTk()=0

가 성립한다.

xk,ˆxk(+),˜zk를 위식에서 해당하는 식으로 바꾸면

E[Φk1xk1+wk1K1kˆxk()K1kzk][Hkˆxk()zk]T=0

하지만 시스템에서 EwkzTk=EwkˆxTk(+)=0 이므로

E[Φk1xk1K1kˆxk()K1kzk][Hkˆxk()zk]T=0

이다.

K1k,zk,˜xk()E˜xkvTk()=0 을 사용하여 바꾸면

0=E[Φk1xk1ˆxk()+ˉKkHkˆxk()ˉKkHkxkˉKkvk][Hkˆxk()Hkxkvk]T=E[(xkˆxk()]ˉKkHk(xkˆxk())ˉKkvk][Hk˜xk()vk]T=E[˜xk()ˉKkHk˜xk()ˉKkvk][Hk˜xk()vk]T

 

priori covariance의 정의에 의하여

Pk()=E˜xk()˜xTk()

이것은 다음 식을 만족한다.

[IˉKkHk]Pk()HTkˉKkRk=0

그러므로 kalman gain은 다음과 같이 표현 된다.

ˉKk=Pk()HTk[HkPk()HTk+Rk]1