还是将卡尔曼滤波核心思想钉在开头：

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

上节，推导得到了状态空间方程，可以得到系统模型值了。目前核心思想中的【修正】这个概念还处于迷雾中，本节对此进行推导。
往下看之前，如果让你来原创，你会怎么用表达式将【测量值修正系统模型值】这句话的逻辑清晰地写出来？

---

修正

有差值才有【修正】，而差值是测量值和系统模型之间的差，是在被修正值的基础上加这个差值，因此这是一个关于测量值和系统模型值的逻辑。【修正】的逻辑应当是这样的：将测量值与模型值的差作为修正值，乘个系数后，叠加到模型值，这就是卡尔曼滤波的【修正】。这个系数，被称为卡尔曼增益。由于$\overrightarrow{z_k}$是一个向量，因此这个系数是矩阵形式，将其记作$\mathrm{G}$，将每轮次的卡尔曼增益记作${\mathrm{G}}_{\mathrm{k}}$，很明显这是一个对角矩阵。将这个逻辑写做数学表达式：
$$\begin{array}{rl}\overrightarrow{z_{k估}}& =\overrightarrow{z_{k模}}+{\mathrm{G}}_{\mathrm{k}}\ast (\overrightarrow{z_{k测}}-\overrightarrow{z_{k模}})\end{array}$$
观察上式，等式右边的$\overrightarrow{z_{k模}}$在上节的状态空间方程里推导得到了，$\overrightarrow{z_{k测}}$也可以根据上节的测量方程$\overrightarrow{y_{k测}}=\mathrm{H}\ast \overrightarrow{z_{k测}}$得到，只有卡尔曼增益${\mathrm{G}}_{\mathrm{k}}$还不知道，我们要找到最佳的${\mathrm{G}}_{\mathrm{k}}$使得$\overrightarrow{z_{k估}}$最接近$\overrightarrow{z_{k真}}$。接下来对${\mathrm{G}}_{\mathrm{k}}$进行推导，推导完毕后，卡尔曼滤波就整个都可知了。

最优估计

卡尔曼滤波思想中要的是最优估计$\overrightarrow{z_{k估}}$，即估计值与真值的差值越接近零越好，当然真值的数值是无法知道的，但是我们上节建立了真值的数学模型。估计值与真值的差距记作$\overrightarrow{e_{k估}}$，可以用如下公式来表达：
$$\overrightarrow{e_{k估}}=\overrightarrow{z_{k真}}-\overrightarrow{z_{k估}}$$
$\overrightarrow{e_{k估}}$是一个向量，也就是说让这个向量的长度最小，即平方和最小。然而，真值里的噪声只是假定为正态分布，每轮次的具体数值是不可知的，因此直接用平方和最小的方法不可行。
还有另一种方法有着与平方和一样的表达式，那就是协方差矩阵的迹，协方差矩阵解决了噪声的数值问题，因为正态分布的协方差矩阵是可知的。于是，记$\overrightarrow{e_{k估}}$的协方差为${\mathrm{P}}_{\mathrm{k}\mathrm{估}}$，求${\mathrm{G}}_{\mathrm{k}}$使得$\overrightarrow{e_{k估}}$接近零的问题转为求${\mathrm{G}}_{\mathrm{k}}$使得$tr({\mathrm{P}}_{\mathrm{k}\mathrm{估}})$最小的问题。
$tr({\mathrm{P}}_{\mathrm{k}\mathrm{估}})$表达式与平方和一样，是一个开口向上的二次方程，因此极值点必定是最小值点，$\frac{dtr({\mathrm{P}}_{\mathrm{k}\mathrm{估}})}{d{\mathrm{G}}_{\mathrm{k}}}=0$时的$({\mathrm{P}}_{\mathrm{k}\mathrm{估}},{\mathrm{G}}_{\mathrm{k}})$就是最优估计$\overrightarrow{z_{k估}}$要用到的值。

卡尔曼增益

${\mathrm{P}}_{\mathrm{k}\mathrm{估}}$还不知道，先把它搞出来再算卡尔曼增益${\mathrm{G}}_{\mathrm{k}}$。
根据协方差定义的公式，
$$\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估}}& =\overrightarrow{z_{k真}}-(\overrightarrow{z_{k模}}+{\mathrm{G}}_{\mathrm{k}}\ast ({\mathrm{H}}^{-1}\ast y_{k测}-\overrightarrow{z_{k模}}))\\ & =\overrightarrow{z_{k真}}-(\overrightarrow{z_{k模}}+{\mathrm{G}}_{\mathrm{k}}\ast ({\mathrm{H}}^{-1}\ast (\mathrm{H}\ast \overrightarrow{z_{k真}}+\overrightarrow{v_k})-\overrightarrow{z_{k模}}))\\ & =\overrightarrow{z_{k真}}-\overrightarrow{z_{k模}}-{\mathrm{G}}_{\mathrm{k}}\ast \overrightarrow{z_{k真}}+{\mathrm{G}}_{\mathrm{k}}\ast \overrightarrow{z_{k模}}-{\mathrm{G}}_{\mathrm{k}}\ast {\mathrm{H}}^{-1}\ast \overrightarrow{v_k}\end{array}$$ 发现可以合并化简，因为$\overrightarrow{z_{k真}}-\overrightarrow{z_{k模}}=\overrightarrow{e_{k模}}$，所以继续化简为：
$$\overrightarrow{z_{k真}}-\overrightarrow{z_{k估}}=(\mathrm{I}-{\mathrm{G}}_{\mathrm{k}})\overrightarrow{e_{k模}}-{\mathrm{G}}_{\mathrm{k}}\ast {\mathrm{H}}^{-1}\ast \overrightarrow{v_k}$$

${\mathrm{P}}_{\mathrm{k}\mathrm{估}}$可以继续展开了：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{估}}& =E[((\mathrm{I}-{\mathrm{G}}_{\mathrm{k}})\overrightarrow{e_{k模}}-{\mathrm{G}}_{\mathrm{k}}\ast {\mathrm{H}}^{-1}\ast \overrightarrow{v_k})\ast ((\mathrm{I}-{\mathrm{G}}_{\mathrm{k}})\overrightarrow{e_{k模}}-{\mathrm{G}}_{\mathrm{k}}\ast {\mathrm{H}}^{-1}\ast \overrightarrow{v_k}{)}^T]\end{array}$$
将转置操作分配到括号里：

$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{估}}& =E[((\mathrm{I}-{\mathrm{G}}_{\mathrm{k}})\overrightarrow{e_{k模}}-{\mathrm{G}}_{\mathrm{k}}{\mathrm{H}}^{-1}\overrightarrow{v_k})({\overrightarrow{e_{k模}}}^T(\mathrm{I}-{\mathrm{G}}_{\mathrm{k}}{)}^T-{\overrightarrow{v_k}}^T({\mathrm{G}}_{\mathrm{k}}{\mathrm{H}}^{-1}{)}^T)]\end{array}$$
接下来将括号乘开。这里有两点有助于化简：噪声$\overrightarrow{v_k}$的期望是零，又与$\overrightarrow{e_{k模}}$相互独立，所以$\overrightarrow{v_k}$和$\overrightarrow{e_{k模}}$搭边的项都是零；$\mathrm{I},{\mathrm{G}}_{\mathrm{k}},\mathrm{H}$是对角矩阵，转置等于本身，而且左乘右乘没区别。乘开后可以得到以下结果：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{估}}& =E[(\mathrm{I}-{\mathrm{G}}_{\mathrm{k}}{)}^2\overrightarrow{e_{k模}}{\overrightarrow{e_{k模}}}^T+{{\mathrm{G}}_{\mathrm{k}}}^2({\mathrm{H}}^{-1}{)}^2\overrightarrow{v_k}{\overrightarrow{v_k}}^T]\end{array}$$
再把期望分配到括号里：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{估}}& =(\mathrm{I}-{\mathrm{G}}_{\mathrm{k}}{)}^{2}E[\overrightarrow{e_{k模}}{\overrightarrow{e_{k模}}}^T]+{{\mathrm{G}}_{\mathrm{k}}}^2({\mathrm{H}}^{-1}{)}^{2}E[\overrightarrow{v_k}{\overrightarrow{v_k}}^T]\end{array}$$
$E[\overrightarrow{e_{k模}}{\overrightarrow{e_{k模}}}^T]$是一个协方差矩阵，这里记作${\mathrm{P}}_{\mathrm{k}\mathrm{模}}$，这个协方差矩阵的值现在还未知。$E[\overrightarrow{v_k}{\overrightarrow{v_k}}^T]$是一个测量噪声的协方差矩阵，是人为假定的常数，是已知的，记做$\mathrm{R}$。再化简为：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{估}}& =(\mathrm{I}-{\mathrm{G}}_{\mathrm{k}}{)}^2{\mathrm{P}}_{\mathrm{k}\mathrm{模}}+{{\mathrm{G}}_{\mathrm{k}}}^2({\mathrm{H}}^{-1}{)}^2\mathrm{R}\end{array}$$
接下来就可以求$tr({\mathrm{P}}_{\mathrm{k}\mathrm{估}})$关于${\mathrm{G}}_{\mathrm{k}}$的导数，从而算出极值点了。这里用到了链式求导法则和矩阵求导：$\frac{dtr(\mathrm{AB})}{d\mathrm{A}}={\mathrm{B}}^T$。注意两点：协方差矩阵是斜对角对称矩阵，其转置等于本身；对角阵左乘和右乘是一样的。计算$tr({\mathrm{P}}_{\mathrm{k}\mathrm{估}})$的极值点：
$$\frac{dtr({\mathrm{P}}_{\mathrm{k}\mathrm{估}})}{d{\mathrm{G}}_{\mathrm{k}}}=-2(\mathrm{I}-{\mathrm{G}}_{\mathrm{k}}){\mathrm{P}}_{\mathrm{k}\mathrm{模}}+2{\mathrm{G}}_{\mathrm{k}}({\mathrm{H}}^{-1}{)}^2\mathrm{R}=0$$
得出卡尔曼增益${\mathrm{G}}_{\mathrm{k}}$等于：
$${\mathrm{G}}_{\mathrm{k}}=\frac{{\mathrm{P}}_{\mathrm{k}\mathrm{模}}}{{\mathrm{P}}_{\mathrm{k}\mathrm{模}}+({\mathrm{H}}^{-1}{)}^2\mathrm{R}}$$

---

注意${\mathrm{P}}_{\mathrm{k}\mathrm{模}}$现在还没推导出来，在下节进行推导。