卡尔曼滤波器Kalman Filter公式推导

引言

卡尔曼滤波器（Kalman Filter, KF）是一种状态空间模型，状态为内因素未知，观测为外因素可视为已知量，其模型如图所示：

状态$s_n$仅与其前一时刻状态$s_{n-1}$有关，与其历史状态无关，这里以线性关系为例，其表达式为：
$$\{\begin{array}{c}{\mathrm{X}}_n={\mathrm{F}}_n{\mathrm{X}}_{n-1}+{\mathrm{V}}_n\\ {\mathrm{Y}}_n={\mathrm{H}}_n{\mathrm{X}}_{n-1}+{\mathrm{W}}_n\end{array}$$
其中$X_n$表示时刻 n 时的状态，$Y_n$表示时刻 n 时的观测值，$V_n$为状态噪声，$W_n$为观测噪声，假设$V_n$与$W_n$为零均值，且相互独立的高斯白噪声，即有：
$$\{\begin{array}{c}\mathrm{E}\left({\mathrm{V}}_n\right)=\mathrm{E}\left({\mathrm{W}}_n\right)=0\\ \mathrm{E}\left({\mathrm{V}}_n{\mathrm{W}}_n\right)=0\\ \mathrm{E}\left({\mathrm{W}}_n{\mathrm{W}}_{\mathrm{m}}\right)={\mathrm{Q}}_n{\delta }_{\mathrm{n}\mathrm{m}}\\ \mathrm{E}\left({\mathrm{V}}_n{\mathrm{V}}_{\mathrm{m}}\right)={\mathrm{T}}_n{\delta }_{\mathrm{n}\mathrm{m}}\end{array}$$
其中$Q_n$与$T_n$分别表示$W_n$与$V_n$的自相关值，${\delta }_{\mathrm{n}\mathrm{m}}$表示
$${\delta }_{\mathrm{n}\mathrm{m}}=\{\begin{array}{cc}1& n=m\\ 0& \text{ otherwise }\end{array}$$
$F_n$，$H_n$，$Q_n$，$T_n$为系数可认为是确定性的常量（即不含随机性），而$X_n$，$Y_n$，$V_n$，$W_n$为随机变量（注意在随机过程中务必清楚什么量是确定的，什么量是随机的！！！）。卡尔曼滤波器要做的即是从含有噪声的观测值中，尽可能准确地估计出当前时刻的状态。
这里简单介绍下投影的概念，使用前m个时刻的观测值${\mathrm{Y}}_1,{\mathrm{Y}}_2,\dots ,{\mathrm{Y}}_{\mathrm{m}}$和第n时刻的状态值$X_n$来估计n时刻的真实状态值${\hat{\mathrm{x}}}_{\mathrm{n}\mid \mathrm{m}}$，可表示为：
x ^ n ∣ m = Proj ⁡ { Y 1 , Y 2 , … , Y m } X n ( n > = m ) \hat{\mathrm{x}}_{\mathrm{n} \mid \mathrm{m}}=\operatorname{Proj}_{\left\{\mathrm{Y}_{1}, \mathrm{Y}_{2}, \ldots, \mathrm{Y}_{\mathrm{m}}\right\}} \mathrm{X}_{n} \quad(\mathrm{n}>=\mathrm{m}) x^n∣m​=Proj{Y1​,Y2​,…,Ym​}​Xn​(n>=m)
即$X_n$在m维平面（${\mathrm{Y}}_1,{\mathrm{Y}}_2,\dots ,{\mathrm{Y}}_{\mathrm{m}}$）作出的投影。
卡尔曼滤波的推导分为两个步骤，即：

• 预测：${\hat{\mathrm{x}}}_{\mathrm{n}-1\mid \mathrm{n}-1}\to {\hat{\mathrm{x}}}_{\mathrm{n}\mid \mathrm{n}-1}$，由n-1时刻的状态值预测第n时刻的状态值；
• 校正：${\hat{\mathrm{x}}}_{\mathrm{n}\mid \mathrm{n}-1}\to {\hat{\mathrm{x}}}_{\mathrm{n}\mid \mathrm{n}}$，对之前的结果作出校正

预测与校正

预测:${\hat{\mathrm{x}}}_{\mathrm{n}-1\mid \mathrm{n}-1}\to {\hat{\mathrm{x}}}_{\mathrm{n}\mid \mathrm{n}-1}$

x ^ n ∣ n − 1 = Proj ⁡ { Y 1 , Y 2 , … , Y n − 1 } X n = Proj ⁡ { Y 1 , Y 2 , … , Y n − 1 } ( F n X n − 1 + V n ) = Proj ⁡ { Y 1 , Y 2 , … , Y n − 1 } ( F n X n − 1 ) + Proj ⁡ { Y 1 , Y 2 , … , Y n − 1 } V n = Proj ⁡ { Y 1 , Y 2 , … , Y n − 1 } ( F n X n − 1 ) = F n Proj ⁡ { Y 1 , Y 2 , … , Y n − 1 } X n − 1 = F n X ^ n − 1 ∣ n − 1 \begin{aligned} \hat{x}_{n \mid n-1} &=\operatorname{Proj}_{\left\{Y_{1}, Y_{2}, \ldots, Y_{n-1}\right\}} X_{n}=\operatorname{Proj}_{\left\{Y_{1}, Y_{2}, \ldots, Y_{n-1}\right\}}\left(F_{n} X_{n-1}+V_{n}\right) \\ &=\operatorname{Proj}_{\left\{Y_{1}, Y_{2}, \ldots, Y_{n-1}\right\}}\left(F_{n} X_{n-1}\right)+\operatorname{Proj}_{\left\{Y_{1}, Y_{2}, \ldots, Y_{n-1}\right\}} V_{n} \\ &=\operatorname{Proj}_{\left\{Y_{1}, Y_{2}, \ldots, Y_{n-1}\right\}}\left(F_{n} X_{n-1}\right) \\ &=F_{n} \operatorname{Proj}_{\left\{Y_{1}, Y_{2}, \ldots, Y_{n-1}\right\}} X_{n-1} \\ &=F_{n} \hat{X}_{n-1 \mid n-1} \end{aligned} x^n∣n−1​​=Proj{Y1​,Y2​,…,Yn−1​}​Xn​=Proj{Y1​,Y2​,…,Yn−1​}​(Fn​Xn−1​+Vn​)=Proj{Y1​,Y2​,…,Yn−1​}​(Fn​Xn−1​)+Proj{Y1​,Y2​,…,Yn−1​}​Vn​=Proj{Y1​,Y2​,…,Yn−1​}​(Fn​Xn−1​)=Fn​Proj{Y1​,Y2​,…,Yn−1​}​Xn−1​=Fn​X^n−1∣n−1​​

注意V与Y是正交的，因此 Proj ⁡ { Y 1 , Y 2 , … , Y n − 1 } V n = 0 \operatorname{Proj}_{\left\{Y_{1}, Y_{2}, \ldots, Y_{n-1}\right\}} V_{n}=0 Proj{Y1​,Y2​,…,Yn−1​}​Vn​=0。

校正：${\hat{\mathrm{x}}}_{\mathrm{n}\mid \mathrm{n}-1}\to {\hat{\mathrm{x}}}_{\mathrm{n}\mid \mathrm{n}}$

我们将$Y_n$进行拆分，$Y_n=Z_n+O_n$，其中 O n = Proj ⁡ { Y 1 , Y 2 , … , Y n − 1 } Y n = Proj ⁡ { Y 1 , Y 2 , … , Y n − 1 } ( H n X n + W n ) = H n Proj ⁡ { Y 1 , Y 2 , … , Y n − 1 } X n = H n X ^ n ∣ n − 1 \begin{aligned} O_{n} &=\operatorname{Proj}_{\left\{Y_{1}, Y_{2}, \ldots, Y_{n-1}\right\}} Y_{n} \\ &=\operatorname{Proj}_{\left\{Y_{1}, Y_{2}, \ldots, Y_{n-1}\right\}}\left(H_{n} X_{n}+W_{n}\right) \\ &=H_{n} \operatorname{Proj}_{\left\{Y_{1}, Y_{2}, \ldots, Y_{n-1}\right\}} X_{n} \\ &=H_{n} \hat{X}_{n \mid n-1} \end{aligned} On​​=Proj{Y1​,Y2​,…,Yn−1​}​Yn​=Proj{Y1​,Y2​,…,Yn−1​}​(Hn​Xn​+Wn​)=Hn​Proj{Y1​,Y2​,…,Yn−1​}​Xn​=Hn​X^n∣n−1​​
$Z_n=Y_n-O_n=Y_n-H_n{\hat{x}}_{n\mid n-1}$表示投影误差（又称为新息，即新产生的信息）。
X ^ n ∣ n = Proj ⁡ { Y 1 , Y 2 , … , Y n } X n = Proj ⁡ { Y 1 , Y 2 , … , Y n − 1 } X n + Proj ⁡ Z n X n = X ^ n ∣ n − 1 + Proj ⁡ z n X n \begin{aligned} \hat{X}_{n \mid n} &=\operatorname{Proj}_{\left\{Y_{1}, Y_{2}, \ldots, Y_{n}\right\}} X_{n} \\ &=\operatorname{Proj}_{\left\{Y_{1}, Y_{2}, \ldots, Y_{n-1}\right\}} X_{n}+\operatorname{Proj}_{Z_{n}} X_{n} \\ &=\hat{X}_{n \mid n-1}+\operatorname{Proj}_{z_{n}} X_{n} \end{aligned} X^n∣n​​=Proj{Y1​,Y2​,…,Yn​}​Xn​=Proj{Y1​,Y2​,…,Yn−1​}​Xn​+ProjZn​​Xn​=X^n∣n−1​+Projzn​​Xn​​
${\mathrm{Proj}}_{z_n}{X}_n$即为校正量，即预测误差，有：
$${\mathrm{Proj}}_{z_n}{X}_n=K_n\left(Y_n-H_n{\hat{X}}_{n\mid n-1}\right)$$
其中$K_n$称为卡尔曼增益，将在下节介绍。最终得到校正结果：
$${\hat{X}}_{n\mid n}={\hat{X}}_{n\mid n-1}+K_n\left(Y_n-H_n{\hat{X}}_{n\mid n-1}\right)$$

卡尔曼增益$K_n$

注意到上式得到的校正结果分为两部分，第一部分为预测模型，第二部分为观察模型，由卡尔曼增益$K_n$调节两者的比例，即卡尔曼增益高越大表示更相信观察模型，反之则更相信预测模型，下面我们对该值进行求解。
$$K_n=\mathrm{E}\left(X_n{Z}_n^T\right){\left(\mathrm{E}\left(Z_n{Z}_n^T\right)\right)}^{-1}$$
其中E表示求期望（均值），上角标T表示对矩阵求转置。

第一项

$$\begin{array}{rl}\mathrm{E}\left(X_n{Z}_n^T\right)& =\mathrm{E}\left(X_n{\left(Y_n-H_n{\hat{X}}_{n\mid n-1}\right)}^T\right)\\ & =\mathrm{E}\left(X_n{\left(H_n{X}_n+W_n-H_n{\hat{X}}_{n\mid n-1}\right)}^T\right)\\ & =\mathrm{E}\left(X_n{\left(X_n-{\hat{X}}_{n\mid n-1}\right)}^T{H}_n^T\right)+\mathrm{E}\left(X_n{W}_n^T\right)\\ & =\mathrm{E}\left(X_n{\left(X_n-{\hat{X}}_{n\mid n-1}\right)}^T{H}_n^T\right)\\ & =\mathrm{E}\left(X_n\left(X_n-{\hat{X}}_{n\mid n-1}\right)\right)H_n^T\end{array}$$
注意到 X ^ n ∣ n − 1 = Proj ⁡ { Y 1 , Y 2 , … , Y n − 1 } X n \hat{X}_{n \mid n-1}=\operatorname{Proj}_{\left\{Y_{1}, Y_{2}, \ldots, Y_{n-1}\right\}} X_{n} X^n∣n−1​=Proj{Y1​,Y2​,…,Yn−1​}​Xn​，即${\hat{X}}_{n\mid n-1}$是$X_n$在超平面${\mathrm{Y}}_1,{\mathrm{Y}}_2,\dots ,{\mathrm{Y}}_{\mathrm{n}-}$上的投影，如图所示：

投影误差$X_n-{\hat{X}}_{n\mid n-1}$与投影量${\hat{X}}_{n\mid n-1}$正交(此即正交性原理，即最优估计的误差正交于估计原材料)，即${\hat{X}}_{n\mid n-1}{\left(X_n-{\hat{X}}_{n\mid n-1}\right)}^T=0$，因此上式可进一步改写为：
$$\mathrm{E}\left(X_n{Z}_n^T\right)=\mathrm{E}\left(\left(X_n-{\hat{X}}_{n\mid n-1}\right){\left(X_n-{\hat{X}}_{n\mid n-1}\right)}^T\right)H_n^T$$
其中${\hat{R}}_{n\mid n-1}=\mathrm{E}\left(\left(X_n-{\hat{X}}_{n\mid n-1}\right){\left(X_n-{\hat{X}}_{n\mid n-1}\right)}^T\right)$为预测误差的协方差矩阵，因此：
$$\mathrm{E}\left(X_n{Z}_n^T\right)={\hat{R}}_{n\mid n-1}{H}_n^T$$

第二项

$$\begin{array}{rl}\mathrm{E}\left(Z_n{Z}_n^T\right)& =E\left(\left(Y_n-H_n{\hat{X}}_{n\mid n-1}\right){\left(Y_n-H_n{\hat{X}}_{n\mid n-1}\right)}^T\right)\\ & =E\left(\left(H_n{X}_n+W_n-H_n{\hat{X}}_{n\mid n-1}\right){\left(H_n{X}_n+W_n-H_n{\hat{X}}_{n\mid n-1}\right)}^T\right)\\ & =H_{n}E\left(\left(X_n-{\hat{X}}_{n\mid n-1}\right)\left(X_n-{\hat{X}}_{n\mid n-1}\right)\right)H_n^T+E\left(W_n{W}_n^T\right)\\ & =H_n{\hat{R}}_{n\mid n-1}{H}_n^T+Q_n\end{array}$$
综上可以得到卡尔曼增益的表达式：
$$K_n={\hat{R}}_{n\mid n-1}{H}_n^T{\left(H_n{\hat{R}}_{n\mid n-1}{H}_n^T+Q_n\right)}^{-1}$$

预测误差的协方差阵${\hat{R}}_{n\mid n-1}$

同求解状态过程步骤类似，对协方差阵${\hat{R}}_{n\mid n-1}$的求解同样分为预测、校正两部分，即：

• ${\hat{\mathrm{R}}}_{\mathrm{n}\mid \mathrm{n}-1}\to {\hat{\mathrm{R}}}_{\mathrm{n}\mid \mathrm{n}}$；
• ${\hat{\mathrm{R}}}_{\mathrm{n}\mid \mathrm{n}}\to {\hat{\mathrm{R}}}_{\mathrm{n}+1\mid \mathrm{n}}$

$$\begin{array}{rl}{\hat{R}}_{n+1\mid n}& =\mathrm{E}\left(\left(X_{n+1}-{\hat{X}}_{n+1\mid n}\right){\left(X_{n+1}-{\hat{X}}_{n+1\mid n}\right)}^T\right)\\ & =\mathrm{E}\left(\left(F_{n+1}{X}_n+V_{n+1}-F_{n+1}{\hat{X}}_{n\mid n}\right){\left(F_{n+1}{X}_n+V_{n+1}-F_{n+1}{\hat{X}}_{n\mid n}\right)}^T\right)\\ & =\mathrm{E}\left(\left(F_{n+1}\left(X_n-{\hat{X}}_{n\mid n}\right)+V_{n+1}\right){\left(F_{n+1}\left(X_n-{\hat{X}}_{n\mid n}\right)+V_{n+1}\right)}^T\right)\\ & =F_{n+1}\mathrm{E}(\left(X_n-{\hat{X}}_{n\mid n}\right){\left(X_n-{\hat{X}}_{n\mid n}\right)}^T{F}_{n+1}^T+\mathrm{E}\left(V_{n+1}{V}_{n+1}^T\right)\\ & =F_{n+1}{\hat{R}}_{n\mid n}{F}_{n+1}^T+T_{n+1}\end{array}$$

$$\begin{array}{rl}{\hat{R}}_{n\mid n}& =\mathrm{E}\left(\left(X_n-{\hat{X}}_{n\mid n}\right){\left(X_n-{\hat{X}}_{n\mid n}\right)}^T\right)\\ & =\mathrm{E}\left(\left(X_n-{\hat{X}}_{n\mid n-1}-K_n\left(Y_n-H_n{\hat{X}}_{n\mid n-1}\right)\right){\left(X_n-{\hat{X}}_{n\mid n-1}-K_n\left(Y_n-H_n{\hat{X}}_{n\mid n-1}\right)\right)}^T\right)\\ & =E\left(\left(X_n-{\hat{X}}_{n\mid n-1}-K_n\left(H_n{X}_n+W_n\right)+K_n{H}_n{\hat{X}}_{n\mid n-1}\right){\left(X_n-{\hat{X}}_{n\mid n-1}-K_n\left(H_n{X}_n+W_n\right)+K_n{H}_n{\hat{X}}_{n\mid n-1}\right)}^T\right)\\ & =E\left(\left(I-K_n{H}_n\right)\left(X_n-{\hat{X}}_{n\mid n-1}\right)-K_n{W}_n\right){\left(\left(I-K_n{H}_n\right)\left(X_n-{\hat{X}}_{n\mid n-1}\right)-K_n{W}_n\right)}^T)\\ & =\left(I-K_n{H}_n\right)E\left(\left(X_n-{\hat{X}}_{n\mid n-1}\right){\left(X_n-{\hat{X}}_{n\mid n-1}\right)}^T\right){\left(I-K_n{H}_n\right)}^T+K_n{Q}_n{K}_n^T\\ & =\left(I-K_n{H}_n\right){\hat{R}}_{n\mid n-1}{\left(I-K_n{H}_n\right)}^T+K_n{Q}_n{K}_n^T\\ & =\left(I-K_n{H}_n\right){\hat{R}}_{n\mid n-1}\end{array}$$