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.