\(Key:\)
\[\begin{align*}
&z_1 = 6.5 mm,\sigma_1 = 0.2 mm;z_2 = 7.3 mm,\sigma_2 = 0.4 mm & \\
&求最优估计: \hat{z} = ? \\
\hat{z} &= z_1 + \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2} (z_2 - z_1) \\
&= 6.5 + \frac{0.2^2}{0.2^2 + 0.4^2} \cdot (7.3 - 6.5) \\
&= 6.66 \\
So:& \\
&The\ key\ is\ 6.66.
\end{align*}
\]
\(Example:\)
\[\begin{align*}
States&: \\
&x_1:位置;x_2:速度& \\
匀速&: \\
&位置:x_{1,k} = x_{1,k-1} + \Delta T x_{2,k-1} = x_{1,k-1} + x_{2,k-1},{\color{red}{\Delta T = 1}} \\
&速度:x_{2,k} = x_{2,k-1} \\
&采样时间:\Delta T\ k时刻与k-1时刻的间隔 \\
因为&具有不确定性：\\
&位置:x_{1,k} = x_{1,k-1} + x_{2,k-1} + w_{1,k-1} \\
&速度:x_{2,k} = x_{2,k-1} + w_{2,k-1} \\
&w为Process\ Noise(过程噪声),p(w) \sim N(0,Q) \\
测量&: \\
&z_{1,k} = x_{1,k} \\
&z_{2,k} = x_{2,k} \\
同样&因为具有不确定性：\\
&z_{1,k} = x_{1,k} + v_{1,k} \\
&z_{2,k} = x_{2,k} + v_{2,k} \\
&v为Measure\ Noise(过程噪声),p(v) \sim N(0,R) \\
\therefore\ &
\begin{aligned}
&\begin{bmatrix}
x_{1,k} \\
x_{2,k} \\
\end{bmatrix}
=
\begin{bmatrix}
1 & 1 \\
0 & 1 \\
\end{bmatrix}
\begin{bmatrix}
x_{1,k-1} \\
x_{2,k-1} \\
\end{bmatrix}
+
\begin{bmatrix}
w_{1,k-1} \\
w_{2,k-1} \\
\end{bmatrix}
\Rightarrow {\color{red}{X_k = A X_{k-1} + w_{k-1}}} \\
&\begin{bmatrix}
z_{1,k} \\
z_{2,k} \\
\end{bmatrix}
=
\begin{bmatrix}
1 & 0 \\
0 & 1 \\
\end{bmatrix}
\begin{bmatrix}
x_{1,k} \\
x_{2,k} \\
\end{bmatrix}
+
\begin{bmatrix}
v_{1,k} \\
v_{2,k} \\
\end{bmatrix}
\Rightarrow {\color{red}{Z_k = H X_k + v_k}} \\
\end{aligned}
{\color{red}{\Rightarrow \hat{X}_k最优}}
\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 &= (I - K_k H) P_k^- \\
\end{align*}
\]