【卡尔曼滤波理论推导与实践】【理论】【3.2/5 卡尔曼增益02】

用测量值修正系统模型值，得到当前轮次的最优估计值。再将该最优估计值当作下一轮的系统模型输入，继续进行下一轮的最优估计，如此迭代进行。

继续推导上节留下来的${\mathrm{P}}_{\mathrm{k}\mathrm{模}}$。

根据协方差定义的公式，
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{模}}& =E[\overrightarrow{e_{k模}}\ast {\overrightarrow{e_{k模}}}^T]\\ & =E[(\overrightarrow{z_{k真}}-\overrightarrow{z_{k模}})\ast (\overrightarrow{z_{k真}}-\overrightarrow{z_{k模}}{)}^T]\end{array}$$

展开一下$(\overrightarrow{z_{k真}}-\overrightarrow{z_{k模}})$：
$$\begin{array}{rl}\overrightarrow{z_{k真}}-\overrightarrow{z_{k模}}& =\mathrm{A}\ast \overrightarrow{z_{k-1真}}+\mathrm{B}\ast F_{k-1}+\overrightarrow{w_{k-1}}-(\mathrm{A}\ast \overrightarrow{z_{k-1估}}+\mathrm{B}\ast F_{k-1})\\ & =\mathrm{A}\ast (z_{k-1真}-z_{k-1估})+\overrightarrow{w_{k-1}}\\ & =\mathrm{A}\overrightarrow{e_{k-1估}}+\overrightarrow{w_{k-1}}\end{array}$$

${\mathrm{P}}_{\mathrm{k}\mathrm{模}}$可以继续展开了：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{模}}& =E[(\mathrm{A}\overrightarrow{e_{k-1估}}+\overrightarrow{w_{k-1}})\ast (\mathrm{A}\overrightarrow{e_{k-1估}}+\overrightarrow{w_{k-1}}{)}^T]\end{array}$$
将转置操作分配到括号里：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{模}}& =E[(\mathrm{A}\overrightarrow{e_{k-1估}}+\overrightarrow{w_{k-1}})\ast ({\overrightarrow{e_{k-1估}}}^T{\mathrm{A}}^T+{\overrightarrow{w_{k-1}}}^T)]\end{array}$$
接下来将括号乘开。注意噪声$\overrightarrow{w_{k-1}}$的期望是0，所以和$\overrightarrow{w_{k-1}}$搭边的项都是0。乘开后可以得到以下结果：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{模}}& =E[(\mathrm{A}\overrightarrow{e_{k-1估}}{\overrightarrow{e_{k-1估}}}^T{\mathrm{A}}^T+\overrightarrow{w_{k-1}}{\overrightarrow{w_{k-1}}}^T]\end{array}$$
再把期望分配到括号里，$E[\overrightarrow{w_{k-1}}{\overrightarrow{w_{k-1}}}^T]$是一个模型噪声的协方差矩阵，是人为假定的常数，是已知的，记做$\mathrm{Q}$：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{模}}& =\mathrm{A}E[\overrightarrow{e_{k-1估}}{\overrightarrow{e_{k-1估}}}^T]{\mathrm{A}}^T+E[\overrightarrow{w_{k-1}}{\overrightarrow{w_{k-1}}}^T]\\ & =\mathrm{A}{\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}{\mathrm{A}}^T+\mathrm{Q}\end{array}$$
在上节已经推导过${\mathrm{P}}_{\mathrm{k}\mathrm{估}}$的表达式，将最优估计时的${\mathrm{G}}_{\mathrm{k}}$代入表达式，即可得到最优估计时的${\mathrm{P}}_{\mathrm{k}\mathrm{估}}$。
上节推导得到的两个表达式如下，注意只是将$k$换成$k-1$：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}& =(\mathrm{I}-{\mathrm{G}}_{\mathrm{k}-1}{)}^2{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}+{{\mathrm{G}}_{\mathrm{k}-1}}^2({\mathrm{H}}^{-1}{)}^2\mathrm{R}，{\mathrm{G}}_{\mathrm{k}-1}=\frac{{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}}{{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}+({\mathrm{H}}^{-1}{)}^2\mathrm{R}}\end{array}$$
你完全可以将${\mathrm{G}}_{\mathrm{k}-1}$直接无脑代入${\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}$，毕竟${\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}$等式右边全是上轮迭代得到的已知数，但是${\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}$可以进行化简减少计算量：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}& ={\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}+{{\mathrm{G}}_{\mathrm{k}-1}}^2{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}-2{\mathrm{G}}_{\mathrm{k}-1}{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}+{{\mathrm{G}}_{\mathrm{k}-1}}^2({\mathrm{H}}^{-1}{)}^2\mathrm{R}\\ & ={\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}+{\mathrm{G}}_{\mathrm{k}-1}[{\mathrm{G}}_{\mathrm{k}-1}({\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}+({\mathrm{H}}^{-1}{)}^2\mathrm{R})-2{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}]\\ & ={\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}+{\mathrm{G}}_{\mathrm{k}-1}({\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}-2{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}})\\ & =(\mathrm{I}-{\mathrm{G}}_{\mathrm{k}-1}){\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}\end{array}$$
等号右边都是上轮迭代得出的值，${\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}$可知了，从而${\mathrm{P}}_{\mathrm{k}\mathrm{模}}$也可知了，从而卡尔曼增益${\mathrm{G}}_{\mathrm{k}}$也可知了。

到此为止，所有东西都被推导出来了，能看到这是一轮迭代求解的过程，下一节对公式进行整理，也就是卡尔曼滤波的5个公式。