系统中的观测向量的维数总是小于系统状态向量的维数，考虑如何通过系统的观测向量取估计系统的状态向量. 考虑无控制项的离散动态系统 X k = ϕ k , k − 1 X k − 1 + Γ k − 1 W k − 1 Z k = H k X k + V k \begin{aligned} &X_k=\phi_{k, k-1}X_{k-1}+\Gamma_{k-1}W_{k-1}\\ &Z_k=H_kX_k+V_k \end{aligned} Xk=ϕk,k−1Xk−1+Γk−1Wk−1Zk=HkXk+Vk 设 Z k Z^k Zk表示第 k k k步及之前的全部观测值 Z k = [ Z 1 Z 2 ⋮ Z k ] Z^k= \left[ \begin{matrix} Z_1\\ Z_2\\ \vdots\\ Z_k \end{matrix} \right] Zk=⎣⎢⎢⎢⎡Z1Z2⋮Zk⎦⎥⎥⎥⎤ 令 X ^ j ∣ k \hat{X}_{j\mid k} X^j∣k表示第 k k k时刻及其之前的观察向量（ Z k Z^k Zk）对第 j j j时刻状态的估计值. 当 j = k j=k j=k时 X ^ j ∣ k \hat{X}_{j\mid k} X^j∣k称为滤波值 当 j > k j>k j>k时 X ^ j ∣ k \hat{X}_{j\mid k} X^j∣k称为外推值 当 j < k j<k j<k时 X ^ j ∣ k \hat{X}_{j\mid k} X^j∣k称为内插值 对模型噪声 W k W_k Wk和观测噪声 V k V_k Vk进行如下假设： (1).状态噪声和观测噪声均为白噪声，且互不相关 { 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 ) = 0 \left\{ \begin{aligned} &\mathbb{E}W_k=0\\ &cov(W_k, W_j)=\mathbb{E}W_kW_j^T=Q_k\delta_{kj}\\ &\mathbb{E}V_k=0\\ &cov(V_k, V_j)=\mathbb{E}V_kV_j^T=R_k\delta_{kj}\\ &cov(W_k, V_j)=0 \end{aligned} \right. ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧EWk=0cov(Wk,Wj)=EWkWjT=QkδkjEVk=0cov(Vk,Vj)=EVkVjT=Rkδkjcov(Wk,Vj)=0 (2).系统初始状态 X 0 X_0 X0与噪声序列 { W k } ， { V k } \{W_k\}，\{V_k\} {Wk}，{Vk}均不相关，即 { c o v ( X 0 , W k ) = 0 c o v ( X 0 , V k ) = 0 E X 0 = μ 0 V X 0 = E ( X 0 − μ 0 ) ( X 0 − μ 0 ) T = P 0 \left\{ \begin{aligned} &cov(X_0, W_k)=0\\ &cov(X_0, V_k)=0\\ &\mathbb{E}X_0=\mu_0\\ &\mathbb{V}X_0=\mathbb{E}(X_0-\mu_0)(X_0-\mu_0)^T=P_0 \end{aligned} \right. ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧cov(X0,Wk)=0cov(X0,Vk)=0EX0=μ0VX0=E(X0−μ0)(X0−μ0)T=P0 根据投影定理推导状态估计递推公式，令 X ^ k − 1 ∣ k − 1 \hat{X}_{k-1\mid k-1} X^k−1∣k−1表示前 k − 1 k-1 k−1次观测值 Z k − 1 Z^{k-1} Zk−1对第 k − 1 k-1 k−1时刻的状态向量估计，简写为 X ^ t − 1 \hat{X}_{t-1} X^t−1. X ^ k − 1 ∣ k − 1 = X ^ k − 1 = E ^ ( X k − 1 ∣ Z k − 1 ) \hat{X}_{k-1\mid k-1}=\hat{X}_{k-1}=\hat{E}(X_{k-1}\mid Z^{k-1}) X^k−1∣k−1=X^k−1=E^(Xk−1∣Zk−1) 利用 Z k − 1 Z^{k-1} Zk−1观测值对 X k X_k Xk进行估计 X ^ k ∣ k − 1 = E ^ ( X k ∣ Z k − 1 ) \hat{X}_{k\mid k-1}=\mathbb{\hat{E}}(X_k\mid Z^{k-1}) X^k∣k−1=E^(Xk∣Zk−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 X ^ k − 1 + Γ k − 1 E ^ ( W k − 1 ∣ Z k − 1 ) \begin{aligned} \hat{X}_{k\mid k-1}&=\mathbb{\hat{E}}(X_k\mid Z^{k-1})=\mathbb{\hat{E}}(\phi_{k, k-1}X_{k-1}+\Gamma_{k-1}W_{k-1}\mid Z^{k-1})\\ &=\phi_{k, k-1}\hat{X}_{k-1}+\Gamma_{k-1}\mathbb{\hat{E}}(W_{k-1}\mid Z^{k-1}) \end{aligned} X^k∣k−1=E^(Xk∣Zk−1)=E^(ϕk,k−1Xk−1+Γk−1Wk−1∣Zk−1)=ϕk,k−1X^k−1+Γk−1E^(Wk−1∣Zk−1) 因为 W k − 1 W_{k-1} Wk−1与 Z 1 , Z 2 , … , Z k − 1 Z_1, Z_2, \dots, Z_{k-1} Z1,Z2,…,Zk−1不相关（正交），且均值为零，所以 E ^ ( W k − 1 ∣ Z k − 1 ) = E W k − 1 + c o v ( W k − 1 , Z k − 1 ) ( V Z k − 1 ) − 1 ( Z k − 1 − E Z k − 1 ) = 0 \mathbb{\hat{E}}(W_{k-1}\mid Z^{k-1})=\mathbb{E}W_{k-1}+cov(W_{k-1}, Z^{k-1})(\mathbb{V}Z^{k-1})^{-1}(Z^{k-1}-\mathbb{E}Z^{k-1})=0 E^(Wk−1∣Zk−1)=EWk−1+cov(Wk−1,Zk−1)(VZk−1)−1(Zk−1−EZk−1)=0 估计方程化简为 X ^ k ∣ k − 1 = ϕ k , k − 1 X ^ k − 1 \hat{X}_{k\mid k-1}=\phi_{k, k-1}\hat{X}_{k-1} X^k∣k−1=ϕk,k−1X^k−1 同理可得 E ^ ( Z k ∣ Z k − 1 ) = E ^ ( H k X k + V k ∣ Z k − 1 ) = E ^ ( H k 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 \begin{aligned} \mathbb{\hat{E}}(Z_k\mid Z^{k-1})&=\mathbb{\hat{E}}(H_kX_k+V_k\mid Z^{k-1})=\mathbb{\hat{E}}(H_kX_k\mid Z^{k-1})+\mathbb{\hat{E}}(V_k\mid Z^{k-1})\\ &=H_k\hat{X}_{k\mid k-1}\\ &=H_k\phi_{k, k-1}\hat{X}_{k-1}\\ &=\hat{Z}_{k, k-1} \end{aligned} E^(Zk∣Zk−1)=E^(HkXk+Vk∣Zk−1)=E^(HkXk∣Zk−1)+E^(Vk∣Zk−1)=HkX^k∣k−1=Hkϕk,k−1X^k−1=Z^k,k−1 令 { Z ~ k ∣ k − 1 = Z k − Z ^ k ∣ k − 1 X ~ k ∣ k − 1 = X k − X ^ k ∣ k − 1 \left\{ \begin{aligned} &\widetilde{Z}_{k\mid k-1}=Z_k-\hat{Z}_{k\mid k-1}\\ &\widetilde{X}_{k\mid k-1}=X_k-\hat{X}_{k\mid k-1} \end{aligned} \right. {Z k∣k−1=Zk−Z^k∣k−1X k∣k−1=Xk−X^k∣k−1 第 k k k时刻的最优估计为 X ^ k = E ^ ( X k ∣ Z k − 1 ) + K k Z ~ k = ϕ k , k − 1 X ^ k − 1 + K k [ Z k − H k ϕ k , k − 1 X ^ k − 1 ] \hat{X}_k=\mathbb{\hat{E}}(X_k\mid Z^{k-1})+K_k\widetilde{Z}_k=\phi_{k, k-1}\hat{X}_{k-1}+K_k[Z_k-H_k\phi_{k, k-1}\hat{X}_{k-1}] X^k=E^(Xk∣Zk−1)+KkZ k=ϕk,k−1X^k−1+Kk[Zk−Hkϕk,k−1X^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 K_k=\mathbb{E}[\widetilde{X}_{k\mid k-1}\widetilde{Z}^T_{k\mid k-1}][\mathbb{E}(\widetilde{Z}_{k\mid k-1}\widetilde{Z}^T_{k\mid k-1})]^{-1} Kk=E[X k∣k−1Z k∣k−1T][E(Z k∣k−1Z k∣k−1T)]−1 因为 { E [ X ~ k ∣ k − 1 Z ~ k ∣ k − 1 T ] = P k ∣ k − 1 H k T E ( Z ~ k ∣ k − 1 Z ~ k ∣ k − 1 T ) = H k P k ∣ k − 1 H k T + R k \left\{ \begin{aligned} &\mathbb{E}[\widetilde{X}_{k\mid k-1}\widetilde{Z}^T_{k\mid k-1}]=P_{k\mid k-1}H_k^T\\ &\mathbb{E}(\widetilde{Z}_{k\mid k-1}\widetilde{Z}^T_{k\mid k-1})=H_kP_{k\mid k-1}H_k^T+R_k \end{aligned} \right. ⎩⎨⎧E[X k∣k−1Z k∣k−1T]=Pk∣k−1HkTE(Z k∣k−1Z k∣k−1T)=HkPk∣k−1HkT+Rk 其中， P k ∣ k − 1 = E [ X ~ k ∣ k − 1 X ~ k ∣ k − 1 T ] P_{k\mid k-1}=\mathbb{E}[\widetilde{X}_{k\mid k-1}\widetilde{X}^T_{k\mid k-1}] Pk∣k−1=E[X k∣k−1X k∣k−1T]，可以得到 K k = P k ∣ k − 1 H k T [ H k P k ∣ k − 1 H k T + R k ] − 1 K_k=P_{k\mid k-1}H_k^T[H_kP_{k\mid k-1}H_k^T+R_k]^{-1} Kk=Pk∣k−1HkT[HkPk∣k−1HkT+Rk]−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 = ϕ k , k − 1 P k − 1 ϕ k , k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T \begin{aligned} P_{k\mid k-1}&=\mathbb{E}[X_k-\hat{X}_{k\mid k-1}][X_k-\hat{X}_{k\mid k-1}]^T\\ &=\mathbb{E}(\phi_{k, k-1}X_{k-1}+\Gamma_{k-1}W_{k-1}-\phi_{k, k-1}\hat{X}_{k-1})(\phi_{k, k-1}X_{k-1}+\Gamma_{k-1}W_{k-1}-\phi_{k, k-1}\hat{X}_{k-1})^T\\ &=\phi_{k, k-1}P_{k-1}\phi_{k, k-1}^T+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T \end{aligned} Pk∣k−1=E[Xk−X^k∣k−1][Xk−X^k∣k−1]T=E(ϕk,k−1Xk−1+Γk−1Wk−1−ϕk,k−1X^k−1)(ϕk,k−1Xk−1+Γk−1Wk−1−ϕk,k−1X^k−1)T=ϕk,k−1Pk−1ϕk,k−1T+Γk−1Qk−1Γk−1T 其中 P k − 1 = E [ X k − 1 − X ^ k − 1 ] [ X k − 1 − X ^ k − 1 ] T P_{k-1}=\mathbb{E}[X_{k-1}-\hat{X}_{k-1}][X_{k-1}-\hat{X}_{k-1}]^T Pk−1=E[Xk−1−X^k−1][Xk−1−X^k−1]T 所以 P k = E [ X k − X ^ k − 1 ] [ X k − X ^ k ] T = E [ X k − X ^ k ∣ k − 1 − K k ( X k − H k X ^ k ∣ k − 1 ) ] [ X k − X ^ k ∣ k − 1 − K k ( X k − H k X ^ k ∣ k − 1 ) ] 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} P_k&=\mathbb{E}[X_k-\hat{X}_{k-1}][X_k-\hat{X}_k]^T\\ &=\mathbb{E}[X_k-\hat{X}_{k\mid k-1}-K_k(X_k-H_k\hat{X}_{k\mid k-1})][X_k-\hat{X}_{k\mid k-1}-K_k(X_k-H_k\hat{X}_{k\mid k-1})]^T\\ &=(I-K_kH_k)P_{k\mid k-1}(I-K_kH_k)^T+K_kR_kK_k^T \end{aligned} Pk=E[Xk−X^k−1][Xk−X^k]T=E[Xk−X^k∣k−1−Kk(Xk−HkX^k∣k−1)][Xk−X^k∣k−1−Kk(Xk−HkX^k∣k−1)]T=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkT 化简为 P k = ( I − K k H k ) P k ∣ k − 1 P_k=(I-K_kH_k)P_{k\mid k-1} Pk=(I−KkHk)Pk∣k−1 整理得到Kalman滤波方程如下 { X ^ k = ϕ k , k − 1 X ^ k − 1 + K k [ Z k − H k ϕ k , k − 1 X ^ k − 1 ] K k = P k ∣ k − 1 H k T [ H k P k ∣ k − 1 H k T + R 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 \left\{ \begin{aligned} &\hat{X}_k=\phi_{k, k-1}\hat{X}_{k-1}+K_k[Z_k-H_k\phi_{k, k-1}\hat{X}_{k-1}]\\ &K_k=P_{k\mid k-1}H_k^T[H_kP_{k\mid k-1}H_k^T+R_k]^{-1}\\ &P_{k\mid k-1}=\phi_{k, k-1}P_{k-1}\phi^T_{k, k-1}+\Gamma_{k-1}Q_{k-1}\Gamma^T_{k-1}\\ &P_k=(I-K_kH_k)P_{k\mid k-1} \end{aligned} \right. ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧X^k=ϕk,k−1X^k−1+Kk[Zk−Hkϕk,k−1X^k−1]Kk=Pk∣k−1HkT[HkPk∣k−1HkT+Rk]−1Pk∣k−1=ϕk,k−1Pk−1ϕk,k−1T+Γk−1Qk−1Γk−1TPk=(I−KkHk)Pk∣k−1
现代控制理论（第二版） 清华大学出版社 张嗣瀛 高立群
