\(状态空间方程\)
\[\begin{cases}
X_k=AX_{k-1}+Bu_{k-1}+w_{k-1} \rightarrow Process\ Noise(过程噪音) \\
Z_k=HX_k+v_k \rightarrow Measurement\ Noise(测量噪音) \\
\end{cases}
\]
\[\begin{align*}
&假设过程噪音是符合正态分布的,即:&\\
&p(w)\sim (0,Q),0\rightarrow期望,Q\rightarrow协方差矩阵\\
&{\color{red}{Q=E[ww^T]}}
\end{align*}
\]
\(eg:\)
\[\begin{align*}
&设存在
\begin{bmatrix}
x_1 \\
x_2 \\
\end{bmatrix}
和对应的
\begin{bmatrix}
w_1 \\
w_2 \\
\end{bmatrix},则:& \\
&Q={\color{red}{E
\left[
{\color{black}{
\begin{bmatrix}
w_1 \\
w_2 \\
\end{bmatrix}
\begin{bmatrix}
w_1 & w_2 \\
\end{bmatrix}}}
\right]}}
=
\begin{bmatrix}
{\color{red}{E}}w_1^2 & {\color{red}{E}}w_1w_2 \\
{\color{red}{E}}w_2w_1 & {\color{red}{E}}w_2^2 \\
\end{bmatrix} \\
&{\color{blue}{Var(x)=E(x^2)-E^2(x)}},E(x)=0,则:\\
&Var(x)=E(x^2)\\
所&以:\\
&Q=
\begin{bmatrix}
\sigma_{w_1}^2 & \sigma_{w_1}\sigma_{w_2} \\
\sigma_{w_2}\sigma_{w_1} & \sigma_{w_2}^2 \\
\end{bmatrix}
,即Covariance\ Matrix(协方差矩阵)\end{align*}
\]
\[\begin{align*}
&假设测量噪音是符合正态分布的,即:&\\
&p(v)\sim (0,R),0\rightarrow期望,R\rightarrow协方差矩阵\\
&{\color{red}{R=E[vv^T]}}
\end{align*}
\]
\[\begin{align*}
&\hat{X}_k^{\color{red}{-\ \rightarrow 先验估计值}}=A\hat{X}_{k-1}+Bu_{k-1}[算出来的] \tag{1}&\\
&Z_k=HX_k\rightarrow\hat{X}_{k_{MEA}}=H^{-1} Z_k[测出来的] \tag{2} \\
&\hat{X}_k^{\color{red}{\ \rightarrow 后验估计值}}=\hat{X}_k^-+G(H^{-1} Z_k-\hat{X}_k^-),G\in[0,1] \\
&G=0:\hat{X}_k=\hat{X}_k^-;G=1:\hat{X}_k=H^{-1} Z_k\\
&令G = K_kH,{\color{red}{\hat{X}_k=\hat{X}_k^-+K_k(Z_k-H\hat{X}_k^-)}},K_k\in [0,H^{-1}]\\
&K_k = 0,\hat{X}_k=\hat{X}_k^-;K_k = H^{-1},\hat{X}_k=H^{-1}Z_k
\end{align*}
\]
\(目标:\)
\(寻找K_k,使得\hat{X}_k \rightarrow X_k:\)
\[\begin{align*}
&引入e_k=X_k-\hat{X}_k,e_k也符合正态分布:&\\
&p(e_k)\sim(0,P)\\
&P=E[ee^T]=
\begin{bmatrix}
\sigma_{e_1}^2 & \sigma_{e_1}\sigma_{e_2} \\
\sigma_{e_2}\sigma_{e_1} & \sigma_{e_2}^2 \\
\end{bmatrix}\\
&{\color{red}{tr(P)_{\rightarrow trace(迹)}=\sigma_{e_1}^2+\sigma_{e_2}^2最小时,方差最小}} \\
\end{align*}
\]
\(寻找合适的k,使得tr(P)最小:\)
\[\begin{align*}
e_k&=X_k-\hat{X}_k& \\
P&=E[ee^T] \\
&=E[(X_k-\hat{X}_k)(X_k-\hat{X}_k)^T]\\
X_k-\hat{X}_k&=X_k-[\hat{X}_k^-+K_k(Z_k-H\hat{X}_k^-)]\\
&=X_k-\hat{X}_k^--K_kZ_k+K_kH\hat{X}_k^-\\
\because Z_k&=HX_k+v_k\\
\therefore 原式&=X_k-\hat{X}_k^--K_k(HX_k+v_k)+K_kH\hat{X}_k^-\\
&=X_k-\hat{X}_k^--K_kHX_k-K_kv_k+K_kH\hat{X}_k^-\\
&=(I-K_kH){\color{red}{\underline{\color{black}{(X_k-\hat{X}_k^-)}}}_{\rightarrow e_k^-}} -K_kv_k \\
So:&\\
P&=E\left[
[(I-K_kH)e_k^--K_kv_k][(I-K_kH)e_k^--K_kv_k]^T
\right] \\
&=E\left[
[(I-K_kH)e_k^- - K_k v_k][{e_k^-}^T (I-K_kH)^T - v_k^T K_k^T]
\right] \\
&=E\left[
(I-K_kH) e_k^- {e_k^-}^T (I-K_kH)^T - (I-K_kH)e_k^- v_k^T K_k^T - K_k v_k {e_k^-}^T (I-K_kH)^T + K_k v_k v_k^T K_k^T \\
\right] \\
&=E\left[(I-K_kH) e_k^- {e_k^-}^T (I-K_kH)^T\right]- E\left[(I-K_kH)e_k^- v_k^T K_k^T\right] - E\left[K_k v_k {e_k^-}^T (I-K_kH)^T\right] + E\left[K_k v_k v_k^T K_k^T\right] \\
\because\ & I-K_kH,K_k是常数;且e_k^-,v_k^T相互独立 \\
\therefore\ & E\left[(I-K_kH)e_k^- v_k^T K_k^T\right]=(I-K_kH) E\left[e_k^- v_k^T\right] K_k^T=(I-K_kH) E\left[e_k^-\right] E\left[v_k^T\right] K_k^T \\
又\because\ & E\left[e_k^-\right],E\left[v_k^T\right] = 0 \\
\therefore\ & E\left[(I-K_kH)e_k^- K_k^T v_k^T\right] = 0 \\
Similarly:\ & E\left[K_k v_k {e_k^-}^T (1-K_kH)^T\right] = 0 \\
\therefore\ P_k&=E\left[(I-K_kH) e_k^- {e_k^-}^T (I-K_kH)^T\right] + E\left[K_k v_k v_k^T K_k^T\right] \\
&=(I-K_kH) {\color{red}{\underline{\color{black}{E\left[e_k^- {e_k^-}^T \right]}}}_{\rightarrow P_k^-}} (I-K_kH)^T + K_k E\left[v_k v_k^T\right] K_k^T \\
&=(P_k^- - K_kHP_k^-)(I-H^TK_k^T) + K_k R K_k^T \\
&=P_k^- - {\color{red}{\underline{\color{black}{P_k^- H^T K_k^T}}}} - {\color{red}{\underline{\color{black}{K_k H P_k^-}}}} + K_k H P_k^- H^T K_k^T + K_k R K_k^T \\
\because\ & [(P_k^- H^T)K_k^T]^T = K_k (P_k^- H^T)^T = K_k H {P_k^-}^T \\
\therefore\ & {\color{red}{tr(P_k)=tr(P_k^-)}} - 2tr(K_k H P_k^-) + tr(K_k H P_k^- H^T K_k^T) + tr(K_k R K_k^T) \tag{3} \\
\end{align*}
\]
\(对(3)式求导,找到极值:\)
\[\begin{align*}
&{\color{blue}{Th.1\ \frac{d tr(AB)}{d A} = B^T}}& \\
&Pf.1\ 设A=
\begin{bmatrix}
a_{11} & a_{12} \\
a_{21} & a_{22} \\
\end{bmatrix}
,B=
\begin{bmatrix}
b_{11} & b_{12} \\
b_{21} & b_{22} \\
\end{bmatrix} \\
&则AB=
\begin{bmatrix}
a_{11} b_{11} + a_{12} b_{21} & a_{11} b_{12} + a_{12} b_{22} \\
a_{21} b_{11} + a_{22} b_{21} & a_{21} b_{12} + a_{22} b_{22} \\
\end{bmatrix} \\
&tr(AB) = a_{11} b_{11} + a_{12} b_{21} + a_{21} b_{12} + a_{22} b_{22} \\
&\frac{d tr(AB)}{d A} =
\begin{bmatrix}
\frac{\partial tr(AB)}{\partial a_{11}} & \frac{\partial tr(AB)}{\partial a_{12}} \\
\frac{\partial tr(AB)}{\partial a_{21}} & \frac{\partial tr(AB)}{\partial a_{22}} \\
\end{bmatrix}
=
\begin{bmatrix}
b_{11} & b_{21} \\
b_{12} & b_{22} \\
\end{bmatrix}
=B^T \\
&{\color{blue}{Th.2\ \frac{d tr(ABA^T)}{d A} = A(B + B^T)}} \\
&Pf.2\ A B A^T=
\begin{bmatrix}
a_{11}^2 b_{11} + a_{11} a_{12} (b_{12} + b_{21}) + a_{12}^2 b_{22} & a_{11} a_{21} b_{11} + a_{12} a_{21} b_{21} + a_{11} a_{22} b_{12} + a_{12} a_{22} b_{22} \\
a_{11} a_{21} b_{11} + a_{11} a_{22} b_{21} + a_{12} a_{21} b_{12} + a_{12} a_{22} b_{22} & a_{21}^2 b_{11} + a_{21} a_{22} (b_{12} + b_{21}) + a_{22}^2 b_{22} \\
\end{bmatrix}\\
&tr(ABA^T) = (a_{11}^2 + a_{21}^2)b_{11} + (a_{11} a_{12} + a_{21} a_{22})(b_{12} + b_{21}) + (a_{12}^2 + a_{22}^2)b_{22} \\
&\frac{d tr(ABA^T)}{d A} =
\begin{bmatrix}
\frac{\partial tr(ABA^T)}{\partial a_{11}} & \frac{\partial tr(ABA^T)}{\partial a_{12}} \\
\frac{\partial tr(ABA^T)}{\partial a_{21}} & \frac{\partial tr(ABA^T)}{\partial a_{22}} \\
\end{bmatrix}
=
\begin{bmatrix}
2 a_{11} b_{11} + a_{12} (b_{12} + b_{21}) & a_{11} (b_{12} + b_{21}) + 2 a_{12} b_{22} \\
2 a_{21} b_{11} + a_{22} (b_{12} + b_{21}) & a_{21} (b_{12} + b_{21}) + 2 a_{22} b_{22} \\
\end{bmatrix}
=
\begin{bmatrix}
a_{11} & a_{12} \\
a_{21} & a_{22} \\
\end{bmatrix}
\begin{bmatrix}
2 b_{11} & b_{12} + b_{21} \\
b_{12} + b_{21} & 2 b_{22} \\
\end{bmatrix}
=
\begin{bmatrix}
a_{11} & a_{12} \\
a_{21} & a_{22} \\
\end{bmatrix}
\left(
\begin{bmatrix}
b_{11} & b_{12} \\
b_{21} & b_{22} \\
\end{bmatrix}
+
\begin{bmatrix}
b_{11} & b_{21} \\
b_{12} & b_{22} \\
\end{bmatrix}
\right)
= A(B + B^T) \\
&{\color{blue}{Th.3\ 当B = B^T时，\frac{d tr(ABA^T)}{d A} = 2AB}} \\
\end{align*}
\]
\[\begin{align*}
找到合适&的k,使得 \frac{d tr(P_k)}{d K_k} = 0,对(3)式求导:& \\
\because\ tr(P_k) &= tr(P_k^-) - 2tr(K_k H P_k^-) + tr(K_k H P_k^- H^T K_k^T) + tr(K_k R K_k^T) \\
\therefore\ \frac{d tr(P_k)}{d K_k} &= 0 - 2(H P_k^-)^T + 2 K_k H P_k^- H^T + 2 K_k R = 0 \\
\therefore\ & K_k (H P_k^- H^T + R) - {P_k^-}_{\color{red}{\rightarrow P_k^- = {P_k^-}^T}} H^T = 0 \\
\therefore\ & {\color{red}{{K_k}_{\rightarrow Kalman\ Gain} = \frac{P_k^- H^T}{H P_k^- H^T + R}}} \\
当 R \uparrow 时&,K_k\rightarrow 0,\hat{X}_k=\hat{X}_k^-;当 R \downarrow 时,K_k \rightarrow H^{-1},\hat{X}_k=H^{-1}Z_k
\end{align*}
\]