卡尔曼滤波的推导

与维纳滤波不同，卡尔曼滤波不是简单地把系统看做一个黑匣子。卡尔曼滤波认为，可以对系统建立状态方程来增加对于系统的了解。

事实上，这种看法在我们生活中是具有普遍意义的。比如火箭的轨迹，严格遵守动力学原理；天气预报严格遵守大气学原理等等

卡尔曼滤波的的模型依赖于两个方程，即系统的状态方程和观测方程，如下
$$\begin{gathered} X_n=F_n(X_{n-1},V_n) \\ {Y}_n=H_n(X_n,W_n) \end{gathered}$$
其中，我们称${\hat{X}}_n$为n时刻的系统值，$Y_n$为 n 时刻的观测值。 $V_n,W_n$分别为系统噪声和观测噪声。如果我们将系统看做是线性系统，并且观测值线性依赖于真实值，可以将模型具体化为如下形式
$$\begin{gathered} X_n=F_n{X}_{n-1}+V_n,statefunction \\ {Y}_n=H_n{X}_n+W_n,observationFunction \end{gathered}$$
为了方便计算，对噪声做如下假设
$$\begin{gathered} E(V_n)=E(W_n)=0 \\ E(V_n{V}_m)=R_n{\delta }_{m-n} \\ E(W_n{W}_m)=P_n{\delta }_{m-n} \\ E(V_n{W}_n)=0 \end{gathered}$$
我们试图通过迭代的算法根据系统在(n-1)时刻的估计值得到系统在(n)时刻的真实值即：${\hat{X}}_{n-1|n-1}\to {\hat{X}}_{n|n}$

我们的做法是先利用(n-1)时刻的估计值，去预测系统在n时刻的值，即${\hat{X}}_{n-1|n-1}\to {\hat{X}}_{n|n-1}$；然后根据（n）时刻的观测值，校正得到n时刻的估计值，即${\hat{X}}_{n|n-1}\to {\hat{X}}_{n|n}$.

预测，即 ${\hat{X}}_{n-1|n-1}\to {\hat{X}}_{n|n-1}$

下面先实现利用（n-1）时刻的观测数据预测 n 时刻的系统值

根据正交性原理，n时刻的最有预测结果应该是n时刻的系统值在观测值构成的空间 { Y 1 , ⋯   , Y n − 1 } \{ Y_1,\cdots,Y_{n-1}\} {Y1​,⋯,Yn−1​}上的投影，即
X ^ n ∣ n − 1 = P r o j { Y 1 , ⋯   , Y n − 1 } X ^ n = P r o j { Y 1 , ⋯   , Y n − 1 } ( F n X ^ n − 1 + V n ) = P r o j { Y 1 , ⋯   , Y n − 1 } ( F n X ^ n − 1 ) = F n P r o j { Y 1 , ⋯   , Y n − 1 } X ^ n − 1 = F n X ^ n − 1 ∣ n − 1 \hat{X}_{n|n-1} = Proj_{ \{Y_1,\cdots,Y_{n-1}\} }\hat{X}_n \\ =Proj_{ \{Y_1,\cdots,Y_{n-1}\} }(F_n\hat{X}_{n-1}+V_n)\\ =Proj_{ \{Y_1,\cdots,Y_{n-1}\} }(F_n\hat{X}_{n-1})\\ =F_nProj_{ \{Y_1,\cdots,Y_{n-1}\} }\hat{X}_{n-1}\\ =F_n\hat{X}_{n-1|n-1} X^n∣n−1​=Proj{Y1​,⋯,Yn−1​}​X^n​=Proj{Y1​,⋯,Yn−1​}​(Fn​X^n−1​+Vn​)=Proj{Y1​,⋯,Yn−1​}​(Fn​X^n−1​)=Fn​Proj{Y1​,⋯,Yn−1​}​X^n−1​=Fn​X^n−1∣n−1​

校正,即${\hat{X}}_{n|n-1}\to {\hat{X}}_{n|n}$

完成上一步的预测之后，得到n时刻的观测值$Y_n$,便可以做校正
X ^ n ∣ n = P r o j { Y 1 , ⋯   , Y n } X n \hat{X}_{n|n} =Proj_{\{Y_1,\cdots,Y_n\}}X_n X^n∣n​=Proj{Y1​,⋯,Yn​}​Xn​
将$Y_n$投影到$Y_1,\cdots ,Y_{n-1}$上可得 O n = P r o j { Y 1 , ⋯   , Y n − 1 } Y n O_n=Proj_{\{Y_1,\cdots,Y_{n-1}\}}Y_n On​=Proj{Y1​,⋯,Yn−1​}​Yn​,则数据$Y_n$带来的有效信息为 Z n = Y n − O n = Y n − P r o j { Y 1 , ⋯   , Y n − 1 } Y n Z_n=Y_n-O_n=Y_n-Proj_{\{Y_1,\cdots,Y_{n-1}\}}Y_n Zn​=Yn​−On​=Yn​−Proj{Y1​,⋯,Yn−1​}​Yn​,下面计算冗余信息$O_n$
O n = P r o j { Y 1 , ⋯   , Y n − 1 } Y n = P r o j { Y 1 , ⋯   , Y n − 1 } ( H n X n + W n ) = H n P r o j { Y 1 , ⋯   , Y n − 1 } X n = H n X ^ n ∣ n − 1 O_n = Proj_{\{Y_1,\cdots,Y_{n-1}\}}Y_n \\ =Proj_{\{Y_1,\cdots,Y_{n-1}\}}(H_nX_n+W_n)\\ =H_nProj_{\{Y_1,\cdots,Y_{n-1}\}}X_n\\ =H_n\hat{X}_{n|n-1} On​=Proj{Y1​,⋯,Yn−1​}​Yn​=Proj{Y1​,⋯,Yn−1​}​(Hn​Xn​+Wn​)=Hn​Proj{Y1​,⋯,Yn−1​}​Xn​=Hn​X^n∣n−1​
则$Z_n=Y_n-H_n{\hat{X}}_{n|n-1}$

则$X_n$在$Z_n$上的投影可以有$Z_n$的线性组合$K_n$得到,即
$$Pro{j}_{Z_n}{X}_n=K_n(Y_n-H_n{\hat{X}}_{n|n-1})$$
因此根据正交性原理可得
X ^ n ∣ n = P r o j { Y 1 , ⋯   , Y n } X n = P r o j { Y 1 , ⋯   , Y n − 1 } X n + P r o j Z n X n = X ^ n ∣ n − 1 + K n ( Y n − H n X ^ n ∣ n − 1 ) \hat{X}_{n|n}=Proj_{\{Y_1,\cdots,Y_n\}}X_n=Proj_{\{Y_1,\cdots,Y_{n-1}\}}X_n+Proj_{Z_n}X_n \\ =\hat{X}_{n|n-1}+K_n(Y_n-H_n\hat{X}_{n|n-1}) X^n∣n​=Proj{Y1​,⋯,Yn​}​Xn​=Proj{Y1​,⋯,Yn−1​}​Xn​+ProjZn​​Xn​=X^n∣n−1​+Kn​(Yn​−Hn​X^n∣n−1​)
从(2)式中可以看出，n时刻预测的系统值，等于在（n-1）时刻的预测值加上误差$Y_n-H_n\widehat{(}{X}_{n|n-1})$的$K_n$倍。

计算校正增益 $K_n$

在计算$X_n$在有效信息$Z_n$上的投影时，引入了线性组合$K_n$,也就是误差的增益，这里给出计算方法。

由正交性原理可知
$$K_n=E[({\hat{X}}_n{Z}_n^T)]E[(Z_n{Z}_n^T){]}^{-1}$$
计算第一项
$$\begin{gathered} E[(X_n{Z}_n^T)]=E[{\hat{X}}_n(Y_n-H_n{\hat{X}}_{n|n-1}{)}^T] \\ =E[{\hat{X}}_n(H_n{\hat{X}}_n+W_n-H_n{\hat{X}}_{n|n-1}{)}^T] \\ =E[{\hat{X}}_n(H_n({\hat{X}}_n-{\hat{X}}_{n|n-1})-W_n{)}^T] \\ =E[{\hat{X}}_n({\hat{X}}_n-{\hat{X}}_{n|n-1}{)}^T{H}_n^T] \\ =E[{\hat{X}}_n({\hat{X}}_n-{\hat{X}}_{n|n-1}{)}^T]H_n^{T}sinceE[{\hat{X}}_{n|n-1}({\hat{X}}_n-{\hat{X}}_{n|n-1}{)}^T]=0 \\ =E[(\widehat{X_n}-{\hat{X}}_{n|n-1})(\widehat{X_n}-{\hat{X}}_{n|n-1}{)}^T]H_n^T \end{gathered}$$
令$R_{n|n-1}=E[(\widehat{X_n}-{\hat{X}}_{n|n-1})(\widehat{X_n}-{\hat{X}}_{n|n-1}{)}^T]H_n^T$，则$E[({\hat{X}}_n{Z}_n^T)]=R_{n|n-1}{H}_n^T$

计算第二项
$$\begin{gathered} E[Z_n{Z}_n^T]=E[(Y_n-H_n{\hat{X}}_{n|n-1})(Y_n-H_n{\hat{X}}_{n|n-1}{)}^T] \\ =E[(H_n{\hat{X}}_n-H_n{\hat{X}}_{n|n-1}-W_n)(H_n{\hat{X}}_n-H_n{\hat{X}}_{n|n-1}-W_n{)}^T] \\ =E[(H_n({\hat{X}}_n-{\hat{X}}_{n|n-1})-W_n)(H_n({\hat{X}}_n-{\hat{X}}_{n|n-1})-W_n{)}^T] \\ =H_{n}E[({\hat{X}}_n-{\hat{X}}_{n|n-1})({\hat{X}}_n-{\hat{X}}_{n|n-1}{)}^T]H_n^T \\ =H_n{R}_{n|n-1}{H}_n^T \end{gathered}$$
因此
$$K_n=R_{n|n-1}{H}_n^T(H_n{R}_{n|n-1}{H}_n^T{)}^{-1}$$

计算协方差矩阵$R_{n|n-1}$

在上一步计算误差校正增益的过程中，引入了协方差矩阵$R_{n|n-1}=E[({\hat{X}}_n-{\hat{X}}_{n|n-1})({\hat{X}}_n-{\hat{X}}_{n|n-1}{)}^T]$,下面给出$R_{n|n-1}$的迭代步骤，即$R_{n|n-1}\to R_{n|n}\to R_{n+1|n}$

先算第二步：$R_{n|n}\to R_{n+1|n}$
$$\begin{gathered} R_{n+1|n}=E[({\hat{X}}_{n+1}-{\hat{X}}_{n+1|n})({\hat{X}}_{n+1}-{\hat{X}}_{n+1|n}{)}^T] \\ =E[(F_{n+1}{\hat{X}}_n-{\hat{X}}_{n+1|n}+V_n)(F_{n+1}{\hat{X}}_n-{\hat{X}}_{n+1|n}+V_n{)}^T] \\ =E[(F_{n+1}{\hat{X}}_n-F_{n+1}{\hat{X}}_{n|n}+V_n)(F_{n+1}{\hat{X}}_n-F_{n+1}{\hat{X}}_{n|n}+V_n{)}^T] \\ =F_{n+1}E[({\hat{X}}_n-{\hat{X}}_{n|n})({\hat{X}}_n-{\hat{X}}_{n|n}{)}^T]F_{n+1}^T+E[V_n{V}_n^T] \\ =F_{n+1}{R}_{n|n}{F}_{n+1}^T+P_n \end{gathered}$$
再算第一步：$R_{n|n-1}\to R_{n|n}$
$$\begin{gathered} R_{n|n}=E[({\hat{X}}_n-{\hat{X}}_{n|n})({\hat{X}}_n-{\hat{X}}_{n|n}{)}^T] \\ =E[({\hat{X}}_n-{\hat{X}}_{n|n-1}-K_n(Y_n-H_n{\hat{X}}_{n|n-1}))({\hat{X}}_n-{\hat{X}}_{n|n-1}-K_n(Y_n-H_n{\hat{X}}_{n|n-1}){)}^T] \\ =E[({\hat{X}}_n-{\hat{X}}_{n|n-1}-K_n(H_n{X}_n-H_n{\hat{X}}_{n|n-1}-W_n))({\hat{X}}_n-{\hat{X}}_{n|n-1}-K_n(H_n{X}_n-H_n{\hat{X}}_{n|n-1}-W_n){)}^T] \\ =E[(({\hat{X}}_n-{\hat{X}}_{n|n-1})(I-K_n{H}_n)-K_n{W}_n))(({\hat{X}}_n-{\hat{X}}_{n|n-1})(I-K_n{H}_n)-K_n{W}_n{)}^T] \\ =(I-K_n{H}_n)E(({\hat{X}}_n-{\hat{X}}_{n|n-1})({\hat{X}}_n-{\hat{X}}_{n|n-1}{)}^T)(I-K_n{H}_n{)}^T+K_{n}E[W_n{W}_n^T]K_n \\ =(I-K_n{H}_n)R_{n|n-1}(I-K_n{H}_n{)}^T+K_n{Q}_n{K}_n^T \end{gathered}$$

总结

以上推到结果可总结为如下五个式子
$$\{\begin{array}{l}{\hat{X}}_{n|n-1}=F_n{\hat{X}}_{n-1|n-1}\\ {\hat{X}}_{n|n}={\hat{X}}_{n|n-1}+K_n(Y_n-H_n{\hat{X}}_{n|n-1})\\ K_n=R_{n|n-1}{H}_n^T(H_n{R}_{n|n-1}{H}_n^T{)}^{-1}\\ R_{n+1|n}=F_{n+1}{R}_{n|n}{F}_{n+1}^T{+}_n\\ R_{n|n}=(I-K_n{H}_n)R_{n|n-1}(I-K_n{H}_n{)}^T+K_n{Q}_n{K}_n^T\end{array}$$
上式中，$P_n$为系统状态噪声的协方差矩阵，$Q_n$为观测噪声的协方差矩阵