Skip to content

Preliminaries of KF – Optimal Gain to minimize estimation error covariance

15/05/2011

In the last post, we present an observer for a noisy system. We already know the estimator should be unbiased and minimum-variance. In this post, we shall show a how to choose the gain K to achieve minimum variance. I think my derivation is quite clear and this is a quite good memo. Note the optimal gain K^* and corresponding estimator presented here is still not Kalman Filter. We shall present the derivation of KF in the future.

Noisy system:

x_{i+1}=Ax_i+Bu_i+Bw_i
y_i=Cx_i+v_i

Estimator:

\hat{x}_{i+1}=A\hat{x}_i+Bu_i+K(y_i-\hat{y}_i)
\hat{y}_i=C\hat{x}_i

State estimation error:

\tilde{x}_{i+1}=(A-KC)\tilde{x}_i+Bw_i-Kv_i

Question:  how to find K_i to make P_{i+1}=E(\tilde{x}_{i+1}\tilde{x}_{i+1}^T) minimum? Note the gain K_i is varying with i.

AnswerI present the solution first and then give the derivation. The optimal gain is

K^*=AP_iC^TR_{e,i}^{-1}=AP_iC^T(R+CP_iC^T)^{-1}

Note: P_i is the covariance matrix of the state estimation error; R_{e,i} is the covariance matrix of the output estimation error!!! R_{e,i}=E(e_ie_i^T) where e_i=y_i-C\hat{x}_i=C\tilde{x}_i+v_i. It is easy to prove R_{e,i}=R+CP_iC^T.

\line(1,0){420}

Proof: The idea is easy, but notations may be complex.

1. \tilde{x}_{i+1}=(A-KC)\tilde{x}_{i}+Bw_i-Kv_i.

2. Becasue x_i, w_i, v_i are uncorrlated, i.e.,

E(\tilde{x}_{i}w_i)=0, E(\tilde{x}_{i}v_i)=0, E(v_iw_i)=0

and E(\tilde{x}_{i}\tilde{x}_{i})=P_i, E(w_iw_i^T)=Q, E(v_iv_i^T)=R. Therefore, we have

E(\tilde{x}_{i+1}\tilde{x}_{i+1}^T)=(A-KC)E(\tilde{x}_{i}\tilde{x}_{i}^T)(A-KC)^T+BE(w_iw_I^T)B^T+KE(v_iv_i^T)K^T

so that

P_{i+1}=(A-KC)P_i(A-KC)^T+BQB^T+KRK^T.

3. Completion of squares:

P_{i+1}=(A-KC)P_i(A-KC)^T+BQB^T+KRK^T=AP_iA^T+BQB^T-AP_iC^TK^T-KCP_iA^T+K(CP_iC^T)K^T

Rewrite

-AP_iC^TK^T-KCP_iA^T+K(CP_iC^T)K^T=MK^T+KM^T+KR_{e,i}K^T

Then

MK^T+KM^T+KR_{e,i}K^T=(K-T)R_{e,i}(K-T)^T-TR_{e,i}T^T

where T=AP_iC^T(R+CP_iC^T)^{-1}.  Hence we have

P_{i+1}=AP_iA^T+BQB^T-TR_{e,i}T^T+(K-T)R_{e,i}(K-T)^T

4. Therefore, we claim K^*=AP_iC^T(R+CP_iC^T)^{-1} and the optimal P_{i+1} is

P_{i+1}=AP_iA^T+BQB^T-TR_{e,i}T^T.

PS: Note the above equation is also applicable to time variant systems. That is if we change A,B,C,Q,R to A_i,B_i,C_i,Q_i,R_i, the results are the same.

Advertisements
No comments yet

Leave a Reply

Please log in using one of these methods to post your comment:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s