背景
在分析随机控制系统时，系统中含有随机变量，而我们往往不能直接获知系统的状态。为此，我们需要通过一些系统的观测量来估计系统的状态。
对于随机控制系统，我们通常用下面的状态方程及观测方程描述：
X k = ϕ k , k − 1 X k − 1 + B k − 1 U k − 1 + Γ k − 1 W k − 1 Z k = H k X k + + Y k + V k \begin{aligned}
& \boldsymbol{X}_k = \boldsymbol{\phi}_{k,k-1} \boldsymbol{X}_{k-1} + \boldsymbol{B}_{k-1} \boldsymbol{U}_{k-1} + \boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1} \\
& \boldsymbol{Z}_k = \boldsymbol{H}_k \boldsymbol{X}_k + +\boldsymbol{Y}_k + \boldsymbol{V}_k
\end{aligned} X k = ϕ k , k − 1 X k − 1 + B k − 1 U k − 1 + Γ k − 1 W k − 1 Z k = H k X k + + Y k + V k 其中 X k ∈ R n \boldsymbol{X}_k \in R^n X k ∈ R n ϕ k , k − 1 ∈ R n × n \boldsymbol{\phi}_{k,k-1} \in R^{n \times n} ϕ k , k − 1 ∈ R n × n Γ \Gamma Γ
我们的任务是利用观测值来取得系统状态估计值，即求解 X ^ = E ( X ∣ Z ) \hat{\boldsymbol{X}} = E(\boldsymbol{X}|\boldsymbol{Z}) X ^ = E ( X ∣ Z )
随机变量的希尔伯特空间
为了将投影定理引入随机变量的讨论中，我们需要构建一个希尔伯特空间。我们不妨定义内积计算：
⟨ X , Y ⟩ = E X Y T \langle \boldsymbol{X}, \boldsymbol{Y} \rangle = E\boldsymbol{XY}^T ⟨ X , Y ⟩ = E XY T 由于在接下来的讨论中，随机变量的取值都是实数集，因此不难知道此内积空间是完备的。实际上，对于此内积的定义，若两个随机变量的期望都为0，则内积等价于协方差。
投影
首先，我们先给出投影的定义。
若与 X \boldsymbol{X} X X ^ \hat{\boldsymbol{X}} X ^
X ^ = a + B Z E ( X − X ^ ) = 0 E ( X − X ^ ) Z T = 0 \begin{aligned}
& \hat{\boldsymbol{X}} = \boldsymbol{a} + \boldsymbol{BZ} \\
& E(\boldsymbol{X} - \hat{\boldsymbol{X}}) = \boldsymbol{0} \\
& E(\boldsymbol{X} - \hat{\boldsymbol{X}})\boldsymbol{Z}^T = 0
\end{aligned} X ^ = a + BZ E ( X − X ^ ) = 0 E ( X − X ^ ) Z T = 0 则称 X ^ \hat{\boldsymbol{X}} X ^ X \boldsymbol{X} X Z \boldsymbol{Z} Z
其中无偏性很好理解，即状态估计值的期望应该与状态的期望相等。
关于线性和正交性可以这样理解：在这里，X 和 Z 并不一定是同维度的。一般地，Z 的维度都会比 X 要低。而 X 的估计值需要取决于 Z，故其应该是 Z 的线性组合，X 的估计值也应该属于 Z 张成的空间（当然这里加了个偏置项 a，X 估计值不严格为线性组合及属于 Z 张成的空间，但不影响正交性）。此时，我们就可以将 X 正交分解为 X ^ \hat{\boldsymbol{X}} X ^ ( X − X ^ ) (\boldsymbol{X} - \hat{\boldsymbol{X}}) ( X − X ^ ) ( X − X ^ ) (\boldsymbol{X} - \hat{\boldsymbol{X}}) ( X − X ^ ) Z \boldsymbol{Z} Z X ^ \hat{\boldsymbol{X}} X ^ Z \boldsymbol{Z} Z
在几何意义上，可以想象 Z \boldsymbol{Z} Z X \boldsymbol{X} X X ^ \hat{\boldsymbol{X}} X ^ X \boldsymbol{X} X Z \boldsymbol{Z} Z
在接下来的讨论中，如果一个向量满足以上性质，被证明是状态的投影，那么它就是状态的最优估计。接下来，我们不妨将最优估计记作 E ^ ( X ∣ Z ) \hat{E}(\boldsymbol{X}|\boldsymbol{Z}) E ^ ( X ∣ Z )
投影定理
根据投影的定义，我们可以推出两个重要定理。这两个定理都是通过已知的一个投影，通过变换推出另一个投影。
定理一
设 X \boldsymbol{X} X Z \boldsymbol{Z} Z n n n m m m
E ^ ( A X ∣ Z ) = A E ^ ( X ∣ Z ) \hat{E}(\boldsymbol{AX} | \boldsymbol{Z}) = \boldsymbol{A} \hat{E}(\boldsymbol{X} | \boldsymbol{Z}) E ^ ( AX ∣ Z ) = A E ^ ( X ∣ Z ) 其中 A \boldsymbol{A} A l × n l \times n l × n
证 分别证明 A E ^ ( X ∣ Z ) \boldsymbol{A} \hat{E}(\boldsymbol{X} | \boldsymbol{Z}) A E ^ ( X ∣ Z )
要证明无偏性，只需证明 E ( A E ^ ( X ∣ Z ) ) = E A X E(\boldsymbol{A} \hat{E}(\boldsymbol{X} | \boldsymbol{Z})) = E\boldsymbol{A}\boldsymbol{X} E ( A E ^ ( X ∣ Z )) = E A X E ^ ( X ∣ Z ) = E X \hat{E}(\boldsymbol{X} | \boldsymbol{Z}) = E\boldsymbol{X} E ^ ( X ∣ Z ) = E X
E [ A E ^ ( X ∣ Z ) ] = A E [ E ^ ( X ∣ Z ) ] = A E X = E A X E[\boldsymbol{A} \hat{E}(\boldsymbol{X} | \boldsymbol{Z})] = \boldsymbol{A} E[\hat{E}(\boldsymbol{X} | \boldsymbol{Z})] = \boldsymbol{A} E \boldsymbol{X} = E\boldsymbol{AX} E [ A E ^ ( X ∣ Z )] = A E [ E ^ ( X ∣ Z )] = A E X = E AX 无偏性得证。
要证明正交性，需要证明 E [ A X − A E ^ ( X ∣ Z ) ] Z T = 0 E[\boldsymbol{AX} - \boldsymbol{A} \hat{E}(\boldsymbol{X} | \boldsymbol{Z})] \boldsymbol{Z}^T = 0 E [ AX − A E ^ ( X ∣ Z )] Z T = 0 E ( X − E ^ ( X ∣ Z ) ) Z T = 0 E(\boldsymbol{X} - \hat{E}(\boldsymbol{X} | \boldsymbol{Z}))\boldsymbol{Z}^T = 0 E ( X − E ^ ( X ∣ Z )) Z T = 0
E [ A X − A E ^ ( X ∣ Z ) ] Z T = A E [ X − E ^ ( X ∣ Z ) ] Z T = 0 E[\boldsymbol{AX} - \boldsymbol{A} \hat{E}(\boldsymbol{X} | \boldsymbol{Z})] \boldsymbol{Z}^T = \boldsymbol{A} E[\boldsymbol{X} - \hat{E}(\boldsymbol{X} | \boldsymbol{Z})] \boldsymbol{Z}^T = 0 E [ AX − A E ^ ( X ∣ Z )] Z T = A E [ X − E ^ ( X ∣ Z )] Z T = 0 正交性得证。
这个定理揭示了被估计量进行线性变换后，对应的估计只需进行一次相同的线性变换即可。
定理二
设 X \boldsymbol{X} X Z 1 \boldsymbol{Z}_1 Z 1 Z 2 \boldsymbol{Z}_2 Z 2 n n n m 1 m_1 m 1 m 2 m_2 m 2 Z = [ Z 1 Z 2 ] \boldsymbol{Z} = \begin{bmatrix} \boldsymbol{Z}_1 \\ \boldsymbol{Z}_2 \end{bmatrix} Z = [ Z 1 Z 2 ]
E ^ ( X ∣ Z ) = E ^ ( X ∣ Z 1 ) + ( E X ~ Z ~ 2 T ) ( E Z ~ 2 Z ~ 2 T ) − 1 Z ~ 2 \hat{E}(\boldsymbol{X} | \boldsymbol{Z}) = \hat{E}(\boldsymbol{X} | \boldsymbol{Z}_1) + (E\tilde{\boldsymbol{X}}\tilde{\boldsymbol{Z}}_2^T) (E\tilde{\boldsymbol{Z}}_2\tilde{\boldsymbol{Z}}_2^T)^{-1} \tilde{\boldsymbol{Z}}_2 E ^ ( X ∣ Z ) = E ^ ( X ∣ Z 1 ) + ( E X ~ Z ~ 2 T ) ( E Z ~ 2 Z ~ 2 T ) − 1 Z ~ 2 其中，
X ~ = X − E ^ ( X ∣ Z 1 ) Z ~ 2 = Z 2 − E ^ ( Z 2 ∣ Z 1 ) \begin{aligned}
& \tilde{\boldsymbol{X}} = \boldsymbol{X} - \hat{E}(\boldsymbol{X}|\boldsymbol{Z}_1) \\
& \tilde{\boldsymbol{Z}}_2 = \boldsymbol{Z}_2 - \hat{E}(\boldsymbol{Z}_2|\boldsymbol{Z}_1)
\end{aligned} X ~ = X − E ^ ( X ∣ Z 1 ) Z ~ 2 = Z 2 − E ^ ( Z 2 ∣ Z 1 ) 观察一下这个式子，可以发现 ( E X ~ Z ~ 2 T ) ( E Z ~ 2 Z ~ 2 T ) − 1 Z ~ 2 (E\tilde{\boldsymbol{X}}\tilde{\boldsymbol{Z}}_2^T) (E\tilde{\boldsymbol{Z}}_2\tilde{\boldsymbol{Z}}_2^T)^{-1} \tilde{\boldsymbol{Z}}_2 ( E X ~ Z ~ 2 T ) ( E Z ~ 2 Z ~ 2 T ) − 1 Z ~ 2 ⟨ X ~ , Z ~ 2 ⟩ ⟨ Z ~ 2 , Z ~ 2 ⟩ Z ~ 2 \frac{\langle \tilde{\boldsymbol{X}}, \tilde{\boldsymbol{Z}}_2 \rangle}{\langle \tilde{\boldsymbol{Z}}_2, \tilde{\boldsymbol{Z}}_2 \rangle} \tilde{\boldsymbol{Z}}_2 ⟨ Z ~ 2 , Z ~ 2 ⟩ ⟨ X ~ , Z ~ 2 ⟩ Z ~ 2 X ~ \tilde{\boldsymbol{X}} X ~ Z ~ 2 \tilde{\boldsymbol{Z}}_2 Z ~ 2
在几何意义上，我们可以把 Z 1 \boldsymbol{Z}_1 Z 1 Z 2 \boldsymbol{Z}_2 Z 2 X \boldsymbol{X} X E ^ ( X ∣ Z 1 ) \hat{E}(\boldsymbol{X}|\boldsymbol{Z}_1) E ^ ( X ∣ Z 1 ) X \boldsymbol{X} X Z 1 \boldsymbol{Z}_1 Z 1 E ^ ( X ∣ Z ) \hat{E}(\boldsymbol{X} | \boldsymbol{Z}) E ^ ( X ∣ Z ) X \boldsymbol{X} X Z 1 \boldsymbol{Z}_1 Z 1 Z 2 \boldsymbol{Z}_2 Z 2 Z 1 \boldsymbol{Z}_1 Z 1 X \boldsymbol{X} X Z 2 \boldsymbol{Z}_2 Z 2 Z 1 \boldsymbol{Z}_1 Z 1 X ~ \tilde{\boldsymbol{X}} X ~ Z ~ 2 \tilde{\boldsymbol{Z}}_2 Z ~ 2 Z 1 \boldsymbol{Z}_1 Z 1 X ~ \tilde{\boldsymbol{X}} X ~ Z ~ 2 \tilde{\boldsymbol{Z}}_2 Z ~ 2
证明过程教材上给的不详细，这里就先略过了。
这个定理揭示了在已有估计值的基础上，如何利用新的观测结果产生更加准确的估计值。
无控制项的线性动态系统的卡尔曼滤波
卡尔曼滤波是一个可以通过少量观测数据，估计出任意时刻的系统状态的方法，有非常广泛的应用。它的理论核心正是上面介绍的投影定理。
背景
我们先定义一个无控制项的离散动态系统：
X k = ϕ k , k − 1 X k − 1 + Γ k − 1 W k − 1 Z k = H k X k + V k \begin{aligned}
& \boldsymbol{X}_k = \boldsymbol{\phi}_{k,k-1} \boldsymbol{X}_{k-1} + \boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1} \\
& \boldsymbol{Z}_k = \boldsymbol{H}_k \boldsymbol{X}_k + \boldsymbol{V}_k
\end{aligned} X k = ϕ k , k − 1 X k − 1 + Γ k − 1 W k − 1 Z k = H k X k + V k 为了叙述方便，我们引入以下记号：
Z k \boldsymbol{Z}^k Z k
Z k = [ Z 1 Z 2 ⋮ Z k ] \boldsymbol{Z}^k =
\begin{bmatrix}
Z_1 \\
Z_2 \\
\vdots \\
Z_k
\end{bmatrix} Z k = Z 1 Z 2 ⋮ Z k X ^ j ∣ k \hat{\boldsymbol{X}}_{j|k} X ^ j ∣ k Z k \boldsymbol{Z}^k Z k E ^ ( X j ∣ Z k ) \hat{E} (\boldsymbol{X}_j | \boldsymbol{Z}^k) E ^ ( X j ∣ Z k ) X ^ k = X ^ k ∣ k \hat{\boldsymbol{X}}_k = \hat{\boldsymbol{X}}_{k|k} X ^ k = X ^ k ∣ k
此外，我们还对噪声做如下假设：
状态噪声和观测噪声均为白噪声，且互不相关。即：
E W k = 0 , c o v ( W k , W j ) = E W k W j T = Q k δ k j E V k = 0 , c o v ( V k , V j ) = E V k V j T = R k δ k j c o v ( W k , V j ) = E W k V j T = 0 \begin{aligned}
& E\boldsymbol{W}_k = \boldsymbol{0}, \qquad cov(\boldsymbol{W}_k, \boldsymbol{W}_j) = E \boldsymbol{W}_k \boldsymbol{W}_j^T = \boldsymbol{Q}_k \delta_{kj} \\
& E\boldsymbol{V}_k = \boldsymbol{0}, \qquad cov(\boldsymbol{V}_k, \boldsymbol{V}_j) = E \boldsymbol{V}_k \boldsymbol{V}_j^T = \boldsymbol{R}_k \delta_{kj} \\
& cov(\boldsymbol{W}_k, \boldsymbol{V}_j) = E \boldsymbol{W}_k \boldsymbol{V}_j^T = 0
\end{aligned} E W k = 0 , co v ( W k , W j ) = E W k W j T = Q k δ k j E V k = 0 , co v ( V k , V j ) = E V k V j T = R k δ k j co v ( W k , V j ) = E W k V j T = 0 其中 δ k j \delta_{kj} δ k j j = k j=k j = k j ≠ k j \neq k j = k
系统的初始状态与噪声序列均不相关。即：
c o v ( X 0 , W k ) = 0 , c o v ( X 0 , V k ) = 0 E X 0 = μ 0 , D X 0 = E ( X 0 − μ 0 ) ( X 0 − μ 0 ) T \begin{aligned}
& cov(\boldsymbol{X}_0,\boldsymbol{W}_k) = 0, \qquad cov(\boldsymbol{X}_0,\boldsymbol{V}_k) = 0 \\
& E\boldsymbol{X}_0 = \boldsymbol{\mu}_0, \qquad D\boldsymbol{X}_0 = E(\boldsymbol{X}_0 - \boldsymbol{\mu}_0)(\boldsymbol{X}_0 - \boldsymbol{\mu}_0)^T
\end{aligned} co v ( X 0 , W k ) = 0 , co v ( X 0 , V k ) = 0 E X 0 = μ 0 , D X 0 = E ( X 0 − μ 0 ) ( X 0 − μ 0 ) T 推导
首先，我们先根据先前的状态来估计下一步状态，即求解 X ^ k − 1 ∣ k − 1 \hat{\boldsymbol{X}}_{k-1|k-1} X ^ k − 1∣ k − 1 X ^ k ∣ k − 1 \hat{\boldsymbol{X}}_{k|k-1} X ^ k ∣ k − 1
X ^ k ∣ k − 1 = E ^ ( X k ∣ Z k − 1 ) = E ^ ( ϕ k , k − 1 X k − 1 + Γ k − 1 W k − 1 ∣ Z k − 1 ) = ϕ k , k − 1 E ^ ( X k − 1 ∣ Z k − 1 ) + Γ k − 1 E ^ ( W k − 1 ∣ Z k − 1 ) = ϕ k , k − 1 E ^ ( X k − 1 ∣ Z k − 1 ) = ϕ k , k − 1 X ^ k − 1 \begin{aligned}
\hat{\boldsymbol{X}}_{k|k-1}
&= \hat{E}(\boldsymbol{X}_k | \boldsymbol{Z}^{k-1}) \\
&= \hat{E}(\boldsymbol{\phi}_{k,k-1} \boldsymbol{X}_{k-1} + \boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1} | \boldsymbol{Z}^{k-1}) \\
&= \boldsymbol{\phi}_{k,k-1} \hat{E}(\boldsymbol{X}_{k-1} | \boldsymbol{Z}^{k-1}) + \boldsymbol{\Gamma}_{k-1} \hat{E}(\boldsymbol{W}_{k-1} | \boldsymbol{Z}^{k-1}) \\
&= \boldsymbol{\phi}_{k,k-1} \hat{E}(\boldsymbol{X}_{k-1} | \boldsymbol{Z}^{k-1}) \\
&= \boldsymbol{\phi}_{k,k-1} \hat{\boldsymbol{X}}_{k-1}
\end{aligned} X ^ k ∣ k − 1 = E ^ ( X k ∣ Z k − 1 ) = E ^ ( ϕ k , k − 1 X k − 1 + Γ k − 1 W k − 1 ∣ Z k − 1 ) = ϕ k , k − 1 E ^ ( X k − 1 ∣ Z k − 1 ) + Γ k − 1 E ^ ( W k − 1 ∣ Z k − 1 ) = ϕ k , k − 1 E ^ ( X k − 1 ∣ Z k − 1 ) = ϕ k , k − 1 X ^ k − 1 在更新下一步状态时，我们往往还需要结合当前的观测值，即利用上面的投影定理二来修正估计。为此，我们需要先求解观测值的估计值：
Z ^ k , k − 1 = E ^ ( Z k ∣ Z k − 1 ) = H k E ^ ( X k ∣ Z k − 1 ) + E ^ ( V k ∣ Z k − 1 ) = H k X ^ k ∣ k − 1 = H k ϕ k , k − 1 X ^ k − 1 \begin{aligned}
\hat{\boldsymbol{Z}}_{k,k-1}
&= \hat{E}(\boldsymbol{Z}_k | \boldsymbol{Z}^{k-1}) \\
&= \boldsymbol{H}_k \hat{E}(\boldsymbol{X}_k | \boldsymbol{Z}^{k-1}) + \hat{E}(\boldsymbol{V}_k | \boldsymbol{Z}^{k-1}) \\
&= \boldsymbol{H}_k \hat{\boldsymbol{X}}_{k|k-1} \\
&= \boldsymbol{H}_k \boldsymbol{\phi}_{k,k-1} \hat{\boldsymbol{X}}_{k-1}
\end{aligned} Z ^ k , k − 1 = E ^ ( Z k ∣ Z k − 1 ) = H k E ^ ( X k ∣ Z k − 1 ) + E ^ ( V k ∣ Z k − 1 ) = H k X ^ k ∣ k − 1 = H k ϕ k , k − 1 X ^ k − 1 随后定义估计误差项：
Z ~ k ∣ k − 1 = Z k − Z ^ k ∣ k − 1 X ~ k ∣ k − 1 = X k − X ^ k ∣ k − 1 \begin{aligned}
\tilde{\boldsymbol{Z}}_{k|k-1} &= \boldsymbol{Z}_k - \hat{\boldsymbol{Z}}_{k|k-1} \\
\tilde{\boldsymbol{X}}_{k|k-1} &= \boldsymbol{X}_k - \hat{\boldsymbol{X}}_{k|k-1}
\end{aligned} Z ~ k ∣ k − 1 X ~ k ∣ k − 1 = Z k − Z ^ k ∣ k − 1 = X k − X ^ k ∣ k − 1 由投影定理二：
X ^ k = E ^ ( X k ∣ Z k − 1 ) + ( E X ~ k ∣ k − 1 Z ~ k ∣ k − 1 T ) ( E Z ~ k ∣ k − 1 Z ~ k ∣ k − 1 T ) − 1 Z ~ k ∣ k − 1 = E ^ ( X k ∣ Z k − 1 ) + K k Z ~ k ∣ k − 1 = ϕ k , k − 1 X ^ k − 1 + K k ( Z k − H k ϕ k , k − 1 X ^ k − 1 ) \begin{aligned}
\hat{\boldsymbol{X}}_k
&= \hat{E}(\boldsymbol{X}_k | \boldsymbol{Z}^{k-1}) + (E \tilde{\boldsymbol{X}}_{k|k-1} \tilde{\boldsymbol{Z}}_{k|k-1}^T)(E \tilde{\boldsymbol{Z}}_{k|k-1} \tilde{\boldsymbol{Z}}_{k|k-1}^T)^{-1} \tilde{\boldsymbol{Z}}_{k|k-1} \\
&= \hat{E}(\boldsymbol{X}_k | \boldsymbol{Z}^{k-1}) + \boldsymbol{K}_k \tilde{\boldsymbol{Z}}_{k|k-1} \\
&= \boldsymbol{\phi}_{k,k-1} \hat{\boldsymbol{X}}_{k-1} + \boldsymbol{K}_k (\boldsymbol{Z}_k - \boldsymbol{H}_k \boldsymbol{\phi}_{k,k-1} \hat{\boldsymbol{X}}_{k-1})
\end{aligned} X ^ k = E ^ ( X k ∣ Z k − 1 ) + ( E X ~ k ∣ k − 1 Z ~ k ∣ k − 1 T ) ( E Z ~ k ∣ k − 1 Z ~ k ∣ k − 1 T ) − 1 Z ~ k ∣ k − 1 = E ^ ( X k ∣ Z k − 1 ) + K k Z ~ k ∣ k − 1 = ϕ k , k − 1 X ^ k − 1 + K k ( Z k − H k ϕ k , k − 1 X ^ k − 1 ) 其中投影标量 K k = ( E X ~ k ∣ k − 1 Z ~ k ∣ k − 1 T ) ( E Z ~ k ∣ k − 1 Z ~ k ∣ k − 1 T ) − 1 \boldsymbol{K}_k = (E \tilde{\boldsymbol{X}}_{k|k-1} \tilde{\boldsymbol{Z}}_{k|k-1}^T)(E \tilde{\boldsymbol{Z}}_{k|k-1} \tilde{\boldsymbol{Z}}_{k|k-1}^T)^{-1} K k = ( E X ~ k ∣ k − 1 Z ~ k ∣ k − 1 T ) ( E Z ~ k ∣ k − 1 Z ~ k ∣ k − 1 T ) − 1 K k \boldsymbol{K}_k K k
容易得出：
Z ~ k ∣ k − 1 = Z k − Z ^ k ∣ k − 1 = H k X k + V k − H k X ^ k ∣ k − 1 = H k X ~ k ∣ k − 1 + V k \begin{aligned}
\tilde{\boldsymbol{Z}}_{k|k-1}
&= \boldsymbol{Z}_k - \hat{\boldsymbol{Z}}_{k|k-1} \\
&= \boldsymbol{H}_k \boldsymbol{X}_k + \boldsymbol{V}_k - \boldsymbol{H}_k \hat{\boldsymbol{X}}_{k|k-1} \\
&= \boldsymbol{H}_k \tilde{\boldsymbol{X}}_{k|k-1} + \boldsymbol{V}_k
\end{aligned} Z ~ k ∣ k − 1 = Z k − Z ^ k ∣ k − 1 = H k X k + V k − H k X ^ k ∣ k − 1 = H k X ~ k ∣ k − 1 + V k 那么有：
E X ~ k ∣ k − 1 Z ~ k ∣ k − 1 T = E X ~ k ∣ k − 1 ( H k X ~ k ∣ k − 1 + V k ) T = E ( X ~ k ∣ k − 1 X ~ k ∣ k − 1 T H k T ) + E ( X ~ k ∣ k − 1 V k T ) = E ( X ~ k ∣ k − 1 X ~ k ∣ k − 1 T ) H k T = P k ∣ k − 1 H k T \begin{aligned}
E \tilde{\boldsymbol{X}}_{k|k-1} \tilde{\boldsymbol{Z}}_{k|k-1}^T
&= E\tilde{\boldsymbol{X}}_{k|k-1}(\boldsymbol{H}_k \tilde{\boldsymbol{X}}_{k|k-1} + \boldsymbol{V}_k)^T \\
&= E(\tilde{\boldsymbol{X}}_{k|k-1} \tilde{\boldsymbol{X}}_{k|k-1}^T \boldsymbol{H}_k^T) + E(\tilde{\boldsymbol{X}}_{k|k-1} \boldsymbol{V}_k^T) \\
&= E(\tilde{\boldsymbol{X}}_{k|k-1} \tilde{\boldsymbol{X}}_{k|k-1}^T) \boldsymbol{H}_k^T \\
&= \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T
\end{aligned} E X ~ k ∣ k − 1 Z ~ k ∣ k − 1 T = E X ~ k ∣ k − 1 ( H k X ~ k ∣ k − 1 + V k ) T = E ( X ~ k ∣ k − 1 X ~ k ∣ k − 1 T H k T ) + E ( X ~ k ∣ k − 1 V k T ) = E ( X ~ k ∣ k − 1 X ~ k ∣ k − 1 T ) H k T = P k ∣ k − 1 H k T 其中用 P k ∣ k − 1 = E ( X ~ k ∣ k − 1 X ~ k ∣ k − 1 T ) \boldsymbol{P}_{k|k-1} = E(\tilde{\boldsymbol{X}}_{k|k-1} \tilde{\boldsymbol{X}}_{k|k-1}^T) P k ∣ k − 1 = E ( X ~ k ∣ k − 1 X ~ k ∣ k − 1 T ) X ~ k ∣ k − 1 \tilde{\boldsymbol{X}}_{k|k-1} X ~ k ∣ k − 1
同样地：
E Z ~ k ∣ k − 1 Z ~ k ∣ k − 1 T = E ( H k X ~ k ∣ k − 1 + V k ) ( H k X ~ k ∣ k − 1 + V k ) T = E ( H k X ~ k ∣ k − 1 X ~ k ∣ k − 1 T H k T + H k X ~ k ∣ k − 1 V k T + V k X ~ k ∣ k − 1 T H k T + V k V k T ) = H k E ( X ~ k ∣ k − 1 X ~ k ∣ k − 1 T ) H k T + E V k V k T = H k P k ∣ k − 1 H k T + R k \begin{aligned}
E \tilde{\boldsymbol{Z}}_{k|k-1} \tilde{\boldsymbol{Z}}_{k|k-1}^T
&= E (\boldsymbol{H}_k \tilde{\boldsymbol{X}}_{k|k-1} + \boldsymbol{V}_k) (\boldsymbol{H}_k \tilde{\boldsymbol{X}}_{k|k-1} + \boldsymbol{V}_k)^T \\
&= E (\boldsymbol{H}_k \tilde{\boldsymbol{X}}_{k|k-1} \tilde{\boldsymbol{X}}_{k|k-1}^T \boldsymbol{H}_k^T + \boldsymbol{H}_k \tilde{\boldsymbol{X}}_{k|k-1} \boldsymbol{V}_k^T + \boldsymbol{V}_k \tilde{\boldsymbol{X}}_{k|k-1}^T \boldsymbol{H}_k^T + \boldsymbol{V}_k \boldsymbol{V}_k^T) \\
&= \boldsymbol{H}_k E(\tilde{\boldsymbol{X}}_{k|k-1} \tilde{\boldsymbol{X}}_{k|k-1}^T) \boldsymbol{H}_k^T + E \boldsymbol{V}_k \boldsymbol{V}_k^T \\
&= \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + \boldsymbol{R}_k
\end{aligned} E Z ~ k ∣ k − 1 Z ~ k ∣ k − 1 T = E ( H k X ~ k ∣ k − 1 + V k ) ( H k X ~ k ∣ k − 1 + V k ) T = E ( H k X ~ k ∣ k − 1 X ~ k ∣ k − 1 T H k T + H k X ~ k ∣ k − 1 V k T + V k X ~ k ∣ k − 1 T H k T + V k V k T ) = H k E ( X ~ k ∣ k − 1 X ~ k ∣ k − 1 T ) H k T + E V k V k T = H k P k ∣ k − 1 H k T + R k 接着我们再求解出 P k ∣ k − 1 \boldsymbol{P}_{k|k-1} P k ∣ k − 1
P k ∣ k − 1 = E ( X k − X ^ k ∣ k − 1 ) ( X k − X ^ k ∣ k − 1 ) T = E ( ϕ k , k − 1 X k − 1 + Γ k − 1 W k − 1 − ϕ k , k − 1 X ^ k − 1 ) ( ϕ k , k − 1 X k − 1 + Γ k − 1 W k − 1 − ϕ k , k − 1 X ^ k − 1 ) T = E ( ϕ k , k − 1 X ~ k − 1 + Γ k − 1 W k − 1 ) ( ϕ k , k − 1 X ~ k − 1 + Γ k − 1 W k − 1 ) T = E ( ϕ k , k − 1 X ~ k − 1 X ~ k − 1 T ϕ k , k − 1 T ) + 2 E ( Γ k − 1 W k − 1 W k − 1 T Γ k − 1 T ) + E ( Γ k − 1 W k − 1 W k − 1 T Γ k − 1 T ) = ϕ k , k − 1 P k − 1 ϕ k , k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T \begin{aligned}
\boldsymbol{P}_{k|k-1}
&= E(\boldsymbol{X}_k - \hat{\boldsymbol{X}}_{k|k-1})(\boldsymbol{X}_k - \hat{\boldsymbol{X}}_{k|k-1})^T \\
&= E(\boldsymbol{\phi}_{k,k-1} \boldsymbol{X}_{k-1} + \boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1} - \boldsymbol{\phi}_{k,k-1} \hat{\boldsymbol{X}}_{k-1})
(\boldsymbol{\phi}_{k,k-1} \boldsymbol{X}_{k-1} + \boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1} - \boldsymbol{\phi}_{k,k-1} \hat{\boldsymbol{X}}_{k-1})^T \\
&= E(\boldsymbol{\phi}_{k,k-1} \tilde{\boldsymbol{X}}_{k-1} + \boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1})
(\boldsymbol{\phi}_{k,k-1} \tilde{\boldsymbol{X}}_{k-1} + \boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1})^T \\
&= E(\boldsymbol{\phi}_{k,k-1} \tilde{\boldsymbol{X}}_{k-1} \tilde{\boldsymbol{X}}_{k-1}^T \boldsymbol{\phi}_{k,k-1}^T) + 2E(\boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1} \boldsymbol{W}_{k-1}^T \boldsymbol{\Gamma}_{k-1}^T) + E(\boldsymbol{\Gamma}_{k-1} \boldsymbol{W}_{k-1} \boldsymbol{W}_{k-1}^T \boldsymbol{\Gamma}_{k-1}^T) \\
&= \boldsymbol{\phi}_{k,k-1} \boldsymbol{P}_{k-1} \boldsymbol{\phi}_{k,k-1}^T + \boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{\Gamma}_{k-1}^T
\end{aligned} P k ∣ k − 1 = E ( X k − X ^ k ∣ k − 1 ) ( X k − X ^ k ∣ k − 1 ) T = E ( ϕ k , k − 1 X k − 1 + Γ k − 1 W k − 1 − ϕ k , k − 1 X ^ k − 1 ) ( ϕ k , k − 1 X k − 1 + Γ k − 1 W k − 1 − ϕ k , k − 1 X ^ k − 1 ) T = E ( ϕ k , k − 1 X ~ k − 1 + Γ k − 1 W k − 1 ) ( ϕ k , k − 1 X ~ k − 1 + Γ k − 1 W k − 1 ) T = E ( ϕ k , k − 1 X ~ k − 1 X ~ k − 1 T ϕ k , k − 1 T ) + 2 E ( Γ k − 1 W k − 1 W k − 1 T Γ k − 1 T ) + E ( Γ k − 1 W k − 1 W k − 1 T Γ k − 1 T ) = ϕ k , k − 1 P k − 1 ϕ k , k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T 继续递推：
P k = E ( X k − X ^ k ) ( X k − X ^ k ) T = E ( X k − X k ∣ k − 1 − K k Z ~ k ∣ k − 1 ) ( X k − X k ∣ k − 1 − K k Z ~ k ∣ k − 1 ) T = E ( X ~ k ∣ k − 1 − K k Z k + K k Z ^ k ∣ k − 1 ) ( X ~ k ∣ k − 1 − K k Z k + K k Z ^ k ∣ k − 1 ) T = E ( X ~ k ∣ k − 1 − K k H k X k − K k V k + K k H k X ^ k ∣ k − 1 ) ( X ~ k ∣ k − 1 − K k H k X k − K k V k + K k H k X ^ k ∣ k − 1 ) T = E ( X ~ k ∣ k − 1 − K k H k X ~ k ∣ k − 1 − K k V k ) ( X ~ k ∣ k − 1 − K k H k X ~ k ∣ k − 1 − K k V k ) T = E [ ( I − K k H k ) X ~ k ∣ k − 1 − K k V k ] [ ( I − K k H k ) X ~ k ∣ k − 1 − K k V k ] T = ( I − K k H k ) E ( X ~ k ∣ k − 1 X ~ k ∣ k − 1 T ) ( I − K k H k ) T + K k E ( V k V k T ) K k T = ( I − K k H k ) P k ∣ k − 1 ( I − K k H k ) T + K k R k K k T \begin{aligned}
\boldsymbol{P}_{k}
&= E(\boldsymbol{X}_k - \hat{\boldsymbol{X}}_{k})(\boldsymbol{X}_k - \hat{\boldsymbol{X}}_{k})^T \\
&= E(\boldsymbol{X}_k - \boldsymbol{X}_{k|k-1} - \boldsymbol{K}_k \tilde{\boldsymbol{Z}}_{k|k-1})(\boldsymbol{X}_k - \boldsymbol{X}_{k|k-1} - \boldsymbol{K}_k \tilde{\boldsymbol{Z}}_{k|k-1})^T \\
&= E(\tilde{\boldsymbol{X}}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{Z}_k + \boldsymbol{K}_k \hat{\boldsymbol{Z}}_{k|k-1})(\tilde{\boldsymbol{X}}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{Z}_k + \boldsymbol{K}_k \hat{\boldsymbol{Z}}_{k|k-1})^T \\
&= E(\tilde{\boldsymbol{X}}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{H}_k \boldsymbol{X}_k - \boldsymbol{K}_k \boldsymbol{V}_k + \boldsymbol{K}_k \boldsymbol{H}_k \hat{\boldsymbol{X}}_{k|k-1})(\tilde{\boldsymbol{X}}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{H}_k \boldsymbol{X}_k - \boldsymbol{K}_k \boldsymbol{V}_k + \boldsymbol{K}_k \boldsymbol{H}_k \hat{\boldsymbol{X}}_{k|k-1})^T \\
&= E(\tilde{\boldsymbol{X}}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{H}_k \tilde{\boldsymbol{X}}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{V}_k)(\tilde{\boldsymbol{X}}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{H}_k \tilde{\boldsymbol{X}}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{V}_k)^T \\
&= E[(\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \tilde{\boldsymbol{X}}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{V}_k][(\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \tilde{\boldsymbol{X}}_{k|k-1} - \boldsymbol{K}_k \boldsymbol{V}_k]^T \\
&= (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) E(\tilde{\boldsymbol{X}}_{k|k-1} \tilde{\boldsymbol{X}}_{k|k-1}^T) (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)^T + \boldsymbol{K}_k E(\boldsymbol{V}_k \boldsymbol{V}_k^T) \boldsymbol{K}_k^T \\
&= (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1} (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)^T + \boldsymbol{K}_k \boldsymbol{R}_k \boldsymbol{K}_k^T
\end{aligned} P k = E ( X k − X ^ k ) ( X k − X ^ k ) T = E ( X k − X k ∣ k − 1 − K k Z ~ k ∣ k − 1 ) ( X k − X k ∣ k − 1 − K k Z ~ k ∣ k − 1 ) T = E ( X ~ k ∣ k − 1 − K k Z k + K k Z ^ k ∣ k − 1 ) ( X ~ k ∣ k − 1 − K k Z k + K k Z ^ k ∣ k − 1 ) T = E ( X ~ k ∣ k − 1 − K k H k X k − K k V k + K k H k X ^ k ∣ k − 1 ) ( X ~ k ∣ k − 1 − K k H k X k − K k V k + K k H k X ^ k ∣ k − 1 ) T = E ( X ~ k ∣ k − 1 − K k H k X ~ k ∣ k − 1 − K k V k ) ( X ~ k ∣ k − 1 − K k H k X ~ k ∣ k − 1 − K k V k ) T = E [( I − K k H k ) X ~ k ∣ k − 1 − K k V k ] [( I − K k H k ) X ~ k ∣ k − 1 − K k V k ] T = ( I − K k H k ) E ( X ~ k ∣ k − 1 X ~ k ∣ k − 1 T ) ( I − K k H k ) T + K k E ( V k V k T ) K k T = ( I − K k H k ) P k ∣ k − 1 ( I − K k H k ) T + K k R k K k T 此式还可以进一步简化为：
P k = ( I − K k H k ) P k ∣ k − 1 \boldsymbol{P}_{k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1} P k = ( I − K k H k ) P k ∣ k − 1 至此，我们已经推出了一套完整的卡尔曼滤波公式：
P k ∣ k − 1 = ϕ k , k − 1 P k − 1 ϕ k , k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T P k = ( I − K k H k ) P k ∣ k − 1 K k = ( P k ∣ k − 1 H k T ) ( H k P k ∣ k − 1 H k T + R k ) − 1 X ^ k = ϕ k , k − 1 X ^ k − 1 + K k ( Z k − H k ϕ k , k − 1 X ^ k − 1 ) \begin{aligned}
\boldsymbol{P}_{k|k-1}
&= \boldsymbol{\phi}_{k,k-1} \boldsymbol{P}_{k-1} \boldsymbol{\phi}_{k,k-1}^T + \boldsymbol{\Gamma}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{\Gamma}_{k-1}^T \\
\boldsymbol{P}_{k} &= (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1} \\
\boldsymbol{K}_k &= (\boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T)(\boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + \boldsymbol{R}_k)^{-1} \\
\hat{\boldsymbol{X}}_k &= \boldsymbol{\phi}_{k,k-1} \hat{\boldsymbol{X}}_{k-1} + \boldsymbol{K}_k (\boldsymbol{Z}_k - \boldsymbol{H}_k \boldsymbol{\phi}_{k,k-1} \hat{\boldsymbol{X}}_{k-1})
\end{aligned} P k ∣ k − 1 P k K k X ^ k = ϕ k , k − 1 P k − 1 ϕ k , k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T = ( I − K k H k ) P k ∣ k − 1 = ( P k ∣ k − 1 H k T ) ( H k P k ∣ k − 1 H k T + R k ) − 1 = ϕ k , k − 1 X ^ k − 1 + K k ( Z k − H k ϕ k , k − 1 X ^ k − 1 ) 其中，P k ∣ k − 1 \boldsymbol{P}_{k|k-1} P k ∣ k − 1 P k \boldsymbol{P}_{k} P k X ^ k \hat{\boldsymbol{X}}_k X ^ k K k \boldsymbol{K}_k K k P k ∣ k − 1 \boldsymbol{P}_{k|k-1} P k ∣ k − 1 Z k \boldsymbol{Z}_k Z k X ^ k − 1 \hat{\boldsymbol{X}}_{k-1} X ^ k − 1 K = H − 1 K = H^{-1} K = H − 1
