\(Revisit\ Linear\ System:\)
\[\begin{align*}
&X_k = A X_{k-1} + B u_{k-1} = w_{k-1},p(w) \sim N(0,Q)& \\
&Z_k = H_{k-1} + v_k,p(v) \sim N(0,R) \\
\end{align*}
\]
\(Data\ Fusion:\)
\[\begin{align*}
预测:& \\
&先验: \hat{X}_k^- = A \hat{X}_{k-1}^- + B u_{k-1}& \\
&先验误差协方差: P_k^- = A P_{k-1} A^T + Q \\
校正:& \\
&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 = (I - K_k H) P_k^- \\
\end{align*}
\]
\(Nonlinear\ System(非线性系统)\)
\[\begin{align*}
&X_k = f(X_{k-1},u_{k-1},w_{k-1}),p(w) \sim N(0,Q)& \\
&Z_k = h(X_k,v_k),p(v) \sim N(0,R),f、h为非线性形式& \\
&{\color{red}{正态分布的随机变量通过非线性系统后就不再是正态的了!}} \\\end{align*}
\]
\(Linearization(线性化)\)
\(Taylor\ Series(泰勒级数)\)
\(f(X) = f(X_0) + \frac{\partial f}{\partial X} (X - X_0)\)
\[\begin{align*}
&{\color{brown}{系统有误差，无法在真实点线性化。}}& \\
&(1)f(X_k) 在 {\hat{X}_{k-1}}_{\color{red}{\rightarrow k-1时的后验估计(上一次)}} 处线性化 \\
&X_k = f(\hat{X}_{k-1},u_{k-1},w_{k-1}) + A (X_k - \hat{X}_{k-1}) + W_k w_{k-1} \\
&令w_{k-1} 误差假设为0,f(\hat{X}_{k-1},u_{k-1},0) = \widetilde{X}_k,A = \left. {\frac{\partial f}{\partial X}} \right|_{\hat{X}_{k-1},u_{k-1}},W_k = \left. \frac{\partial f}{\partial w} \right|_{\hat{X}_{k-1},u_{k-1}} \\
&EXample: \\
&X_1 = X_1 + sin X_2 =f_1 \\
&X_2 = X_1^2 = f_2 \\
&A = \frac{\partial f}{\partial X} =
\begin{bmatrix}
\frac{\partial f_1}{\partial X_1} & \frac{\partial f_1}{\partial X_2} \\
\frac{\partial f_2}{\partial X_1} & \frac{\partial f_2}{\partial X_2} \\
\end{bmatrix}
= \left.
\begin{bmatrix}
1 & cos X_2 \\
2 X_1 & 0 \\
\end{bmatrix} \right|_{\hat{X}_{k-1},u_{k-1}} \\
&\color{red}{{\therefore A_k =
\begin{bmatrix}
1 & cos {\hat{X}_{2_{k-1}}} \\
2 {\hat{X}_{1_{k-1}}} & 0 \\
\end{bmatrix},A_k 随 k 变化}} \\
&(2)Z_k 在 \widetilde{X}_k 处线性化 \\
&Z_k= h(\widetilde{X}_k,v_k) + H(X_k - \widetilde{X}_k) + V v_k \\
&令 v_k 误差假设为0,h(\widetilde{X}_k,0) = \widetilde{Z}_k,H = \left. {\frac{\partial h}{\partial X}} \right|_{\widetilde{X}_k},V = \left. {\frac{\partial h}{\partial v}} \right|_{\widetilde{X}_k} \\
&{\color{red}{X_k = \widetilde{X}_k + A (X_{k-1} + \hat{X}_{k-1}) + W w_{k-1}}} \\
&{\color{red}{Z_k = \widetilde{Z}_k + H (X_k - \widetilde{X}_k) + V v_k}} \\
&注意: \\
&{\color{blue}{p(w) \sim N(0,Q),p(v) \sim N(0,R)}} \\
&{\color{blue}{p(W w_{k-1}) \sim N(0,W Q W^T),p(V v_k) \sim N(0,V R V^T)}} \\
\end{align*}
\]
\(Extened\ Kalman\ Filter(EKF,扩展卡尔曼滤波器)的状态空间方程\)
\[\begin{align*}
预测:& \\
&先验: \hat{X}_k^- = f(\hat{X}_{k-1},u_{k-1},0)& \\
&先验误差协方差: P_k^- = A P_{k-1} A^T + W Q W^T \\
校正:& \\
&Kalman\ Gain:K_k =\frac{P_k^- H^T}{H P_k^- H^T + V R V^T} \\
&后验估计: \hat{X}_k = \hat{X}_k^- + K_k (Z_k - h(\hat{X}_k^-,0) \\
&更新误差协方差: P_k = (I - K_k H) P_k^- \\
\end{align*}
\]