\[\begin{align*}
&状态空间方程
\begin{cases}
X_k=AX_{k-1}+Bu_{k-1}+w_{k-1},w \sim p(0,Q) \\
Z_k=HX_k+v_k,v \sim p(0,R) \\
\end{cases}\\
&先验估计值:\hat{X}_k^- = A \hat{X}_{k-1} + B u_{k-1}& \\
&后验估计值:\hat{X}_k = \hat{X}_k^- + K_k(Z_k-H\hat{X}_k^-) \\
&Kalman\ Gain:K_k = \frac{P_k^- H^T}{H P_k^- H^T + R}
\end{align*}
\]
\(求 P_k^- :\)
\[\begin{align*}
e_k^- &= X_k - \hat{X}_k^-& \\
&= (AX_{k-1}+Bu_{k-1}+w_{k-1}) - (A \hat{X}_{k-1} + B u_{k-1}) \\
&= A (X_{k-1} - \hat{X}_{k-1}) + w_{k-1} \\
&= A e_{k-1}^- + w_{k-1} \\
P_k^- &= E[e_k^- {e_k^-}^T] \\
&= E\left[
(A e_{k-1}^- + w_{k-1})(A e_{k-1}^- +w_{k-1})^T
\right]\\
&= E\left[
(A e_{k-1}^- + w_{k-1})({e_{k-1}^-}^T A^T + {w_{k-1}}^T)
\right]\\
&= E\left[
A e_{k-1}^- {e_{k-1}^-}^T A^T + A e_{k-1}^- {w_{k-1}}^T + w_{k-1} {e_{k-1}^-}^T A^T + w_{k-1} {w_{k-1}}^T
\right]\\
&= E[A e_{k-1}^- {e_{k-1}^-}^T A^T] + E[A e_{k-1}^- {w_{k-1}}^T] + E[w_{k-1} {e_{k-1}^-}^T A^T] + E[w_{k-1} {w_{k-1}}^T] \\
其中&,e_{k-1}^-与w_{k-1}相互独立，且E[e_{k-1}^-],E[w_{k-1}] = 0 \\
\therefore\ &E[A e_{k-1}^- {w_{k-1}}^T] = A E[e_{k-1}^-] E[{w_{k-1}}^T] = 0 \\
Similarly &: E[w_{k-1} {e_{k-1}^-}^T A^T] = E[w_{k-1}] E[{e_{k-1}^-}^T] A^T = 0 \\
\therefore\ 原式 &= A E[e_{k-1}^- {e_{k-1}^-}^T] A^T + E[w_{k-1} {w_{k-1}}^T] \\
&= A P_{k-1} A^T + Q \\
即:&\\
&{\color{blue}{P_k^- = A P_{k-1} A^T + Q}}
\end{align*}
\]
\(预测\)
\[\begin{align*}
&先验: \hat{X}_k^- = A \hat{X}_{k-1}^- + B u_{k-1}& \\
&先验误差协方差: P_k^- = A P_{k-1} A^T + Q,{\color{green}{P_{k-1} \rightarrow 上一次误差的协方差}} \\
\end{align*}
\]
\(校正\)
\[\begin{align*}
Kalman\ Gain:K_k &=\frac{P_k^- H^T}{H P_k^- H^T + R} \\
后验估计: \hat{X}_k &= \hat{X}_k^- + K_k (Z_k - H \hat{X}_k^-) \\
更新误差协方差: P_k &= P_k^- - P_k^- H^T K_k^T - K_k H P_k^- + K_k H P_k^- H^T K_k^T + K_k R K_k^T \\
&= P_k^- - P_k^- H^T K_k^T - K_k H P_k^- + K_k (H P_k^- H^T + R) K_k^T \\
&= P_k^- - P_k^- H^T K_k^T - K_k H P_k^- + \frac{P_k^- H^T}{H P_k^- H^T + R} (H P_k^- H^T + R) K_k^T \\
&= P_k^- - P_k^- H^T K_k^T - K_k H P_k^- + P_k^- H^T K_k^T \\
&= P_k^- - K_k H P_k^- \\
&= (I - K_k H) P_k^-
\end{align*}
\]