Kalman gain 구하기
아래의 내용은 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=Φk−1xx−1+wk−1
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,...,k−1,E⟨[xk−ˆxk(+)]zTk⟩=0.
여기서 z1,...,zk는 노이즈 텀인 wk 을 포함하고 있지 않다. 그러므로 wk 와 vk는 상관관계가 아니고 E⟨wkzTi⟩=0for1≤i≤k
E⟨[Φk−1xk−1+wk−1−K1kˆxk(−)−ˉKkzk]zTi⟩=0fori=1,...,k−1
그런데, zk=Hkxk+vk 이므로 위식은 아래와 같이 된다.
E⟨[Φk−1xk−1−K1kˆxk(−)−ˉKkHkxk−ˉKkvk]zTi⟩=0fori=1,...,k−1
또한 아래 식도 성립한다.
E⟨[xk−1−ˆx(k−1)(+)]zTk⟩=0i=1,...,k−1
그리고
E⟨vkzTk⟩=0i=1,...,k−1
E⟨[Φk−1xk−1−K1kˆxk(−)−ˉKkHkxk−ˉKkvk]zTi⟩=0fori=1,...,k−1은 아래와 같이 정리 된다.
Φk−1E⟨xk−1zTi⟩−K1kE⟨ˆxk(−)zTi⟩−ˉKkHkΦk−1E⟨xk−1zTi⟩−ˉKkE⟨vkzTi⟩=0,Φk−1E⟨xk−1zTi⟩−K1kE⟨ˆxk(−)zTi⟩−ˉKkHkΦk−1E⟨xk−1zTi⟩=0,E⟨[xk−ˉKkHkxk−K1kxk]zTi−K1k(ˆxk(−)−xk)zTi⟩=0,[I−K1k−ˉKkHk]E⟨xkzTi⟩=0.
만약
K1k=I−ˉKkHk
이면 xk에 상관 없이 위 식이 성립한다.
여기서 선택된 K1k은 ˆxk(+)=K1kˆxk(−)+ˉKkzk 을 만족하고, 위에 언급된 orthogonality principle도 E⟨[xk−ˆxk(+)]zTi⟩=0 만족한다. 또한 ˉKk 도 E⟨[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⟨[Φk−1xk−1+wk−1−K1kˆxk(−)−K1kzk][Hkˆxk(−)−zk]T⟩=0
하지만 시스템에서 E⟨wkzTk⟩=E⟨wkˆxTk(+)⟩=0 이므로
E⟨[Φk−1xk−1−K1kˆxk(−)−K1kzk][Hkˆxk(−)−zk]T⟩=0
이다.
K1k,zk,˜xk(−)를 E⟨˜xkvTk(−)⟩=0 을 사용하여 바꾸면
0=E⟨[Φk−1xk−1−ˆxk(−)+ˉKkHkˆxk(−)−ˉKkHkxk−ˉKkvk][Hkˆxk(−)−Hkxk−vk]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