확장 칼만 필터에 대해서부터 전개해보자.
System model and Measurement model
xk+1=Fkxk+wkwk∼N(0,σx)zk=Hkxk+vkvk∼N(0,σz)
상태 방정식으로 표현된 위 시스템에 대해서 다음과 같이 칼만 필터 과정이 전개된다.
0. Parameter Initialization
Initial Posteriori estimate vector and its covariance matrix
P0,ˆx0
Set System noise covariance matrix and Measurement noise covariance matrix
Q,R
1. Time Update (Prediction Step)
Priori estimate ˆx−k and Priori covariance matrix P−k
{ˆx−k=Fkˆxk−1P−k=E[e−k(e−k)T]=FkPk−1FTk+Qk
Innovation vector d−k and Innovation covariance matrix S−k
d−k=zk−Hkˆx−k=vk+Hk(xk−ˆx−k)S−k=Rk+HkP−kHTk
Get Kalman Gain
Kk=P−kHTk(S−k)−1
2. Measurement Update (Estimation Step)
여기서 계측 값 zk에 대해 연산을 수행하고, 추정 상태 ˆxk을 얻을 수 있다.
Posteriori estimate vector ˆxk and Posteriori estimate covariance matrix Pk
{ˆxk=ˆx−k+Kkd−kPk=P−k−KkS−1kKTk=(I−KkHk)P−K
Residual vector dk and Residual covariance matrix Sk
{dk=zk−HkˆxkSk=Rk+HkPkHTk
유도 1. [4]
Priori error covariance matrix 의 유도
P−k=E[e−k(e−k)T]=FkPk−1FTk+Qk
e−k=xk−ˆxk=Fk−1xk−1−(Fk−1ˆxk−1+wk)=Fk−1(xk−1−ˆxk−1)−wk=Fk−1ek−1−wk
Posteriori error covariance matrix 와 Kalman Gain의 유도
priori estimate ˆx−k 를 계측 zk으로 개선하기 위해서, 다음 식과 같이 잡음이 있는 계측과 priori estimate 의 선형 결합을 선택해보자.
ˆxk=ˆx−k+Kk(zk−Hkˆx−k)
zk=Hkxk+vk
Kk를 결합 계수라고 하자. 여기서의 문제는 갱신한 추정이 최적인 결합 계수 Kk를 찾아내는 것이다. 그래서 Wiener solution 과 같이, 성능 지표로써 평균 제곱 오차가 최소가 되도록 한다. 갱신된 추정인 Posteriori estimate와 연관된 오차 공분산 행렬 Pk를 구성해보면 다음과 같다.
Pk=E[ekeTk]=E[(xk−ˆxk)(xk−ˆxk)T]
(xk−ˆxk)=(xk−ˆx−k)−Kk(HkxK+vk−Hkˆx−k)=[I−KkHk][xkˆx−k]−Kkvk
위 식으로부터 Pk를 구하면 다음과 같은 quadratic form 의 Pk를 구하게 된다. 이는 P−k와 Rk가 양의 정정행렬이라면 Pk가 양의 정정행렬이 된다. 이를 Joseph Stabilized version이라고 한다.[3]
Pk=(I−KkHk)P−k(I−KkHk)T+KkRkKTk=P−k−KkHkP−k−P−kHTkKTk+Kk(HkPkHTk+Rk) KTk
위 식은 Kk 에 대해 linear 및 quadratic 한 항으로 구성되어있다. 상태 벡터 요소의 추정 값에서 평균 제곱 오차의 합을 최소화하기 위해서 Pk를 최소화하도록 하는 Kk를 구해보자.
여기서 행렬 미분 공식을 활용해보면..
d[tr(AB)]dA=BTAB∈Rn×nd[tr(ACAT)]dA=2ACC=CT
d[tr(Pk)]dKk=−2(HkP−k)T+2Kk(HkPkHTK+Rk)
\therefore) K_k = P_k^- H_k^T \left( H_k P_k^- H_k^T + R_k \right)^{-1}
위의 결합 계수를 칼만 이득이라고 한다.
Joseph Stabilized version 식을 전개하고, 위의 칼만 이득을 넣으면 다시한번 정리할 수 있다.
P_k = \left(I - K_k H_k \right) P_k^-
정리하면..
\matrix{P_k & =& P_k^- - K_k H_k P_k^- - P_k^- H_k^T K_k^T + K_k \left( H_k P_k H_k^T + R_k \right)\ K_k^T \\ & = & \left(I - K_k H_k \right) P_k^- \\ & = & P_k^- - P_k^- H_k^T \left( H_k P_k^- H_k^T + R_k \right)^{-1} H_k P_k^- \\ & = & P_k^- - K_k \left( H_k P_k^- H_k^T + R_k \right) K_k^T }
위와 같이 posteriori error covariance matrix P_k를 계산하였으니, 이를 기반으로 priori error covariance matrix P_k^-를 계산해보자.
\matrix{ e_{k+1}^- &=& x_{k+1} - \hat{x}_{k+1}^- \\ &=& F_k x_k - F_k \hat{x}_k - w_k \\ &=& F_k e_k - w_k }
\matrix{ P_k^- &=& E \left[ e_k^- \left( e_k^- \right)^T \right] \\ \therefore) P_k^-&=&F_k P_k F_k^T + Q_k }
[1] Akhlaghi, S., Zhou, N., & Huang, Z. (2017). Adaptive adjustment of noise covariance in Kalman filter for dynamic state estimation. 2017 IEEE Power & Energy Society General Meeting. doi:10.1109/pesgm.2017.8273755
[2] WANG, J. (1999). Stochastic Modeling for Real-Time Kinematic GPS/GLONASS Positioning. Navigation, 46(4), 297–305. doi:10.1002/j.2161-4296.1999.tb02416.x
[3] Lewis, F. L, Xie, L, and Popa D., Optimal and Robust Estimation - with an Introduction ot Stochastic Control Theory, 2nd, pp.73-75.
[4] Brown R. G. and Hwang P. Y. C. Introduction to Random Signals and Applied Kalman Filtering, 3rd Ed., pp.214-247.
** EOF **
'G.N.C.' 카테고리의 다른 글
| Van der Pol Oscillator (0) | 2024.03.15 |
|---|---|
| PID, Anti-Windup 구현 (0) | 2023.10.16 |