Kalman滤波二

卡尔曼滤波5大公式的推导

首先，我们回顾一下，在上一节中，我们得到了一个“计算结果”${X\text{X}X}_k$和“测量结果”${Z\text{Z}Z}_k$
$$\begin{array}{rl}{X\text{X}X}_k& =A{X\text{X}X}_{k-1}+B{u\text{u}u}_{k-1}+{\omega \text{ω}\omega }_{k-1}\\ {Z\text{Z}Z}_k& =H{X\text{X}X}_k+{\mu \text{μ}\mu }_k\end{array}$$

然而，在我们实际过程中，由于过程噪声和测量噪声的不可辨识性(我们将世界中无法表示且会对状态变量带来不确定性的影响视作噪声(KF中通常为白噪声)，因此我们实际可以得到的部分为：

$$\begin{array}{rl}{X\text{X}X}_k& =A{X\text{X}X}_{k-1}+B{u\text{u}u}_{k-1}\\ {Z\text{Z}Z}_k& =H{X\text{X}X}_k\end{array}$$

我们此时可以将，${\widehat{X\text{X}X}}_k^{-}$称为先验估计值，也就是我们可直接计算得出的值;${\widehat{X\text{X}X}}_{k-1}$为上一次我们得到的估计值，也就是我们上一次的后验估计值；而${X\text{X}X}_{k_{mea}}$称为测量值。
即上面的式子可以化为:
$$\begin{array}{rl}{\widehat{X\text{X}X}}_k^{-}& =A{\widehat{X\text{X}X}}_{k-1}+B{u\text{u}u}_{k-1}\\ {X\text{X}X}_{k_{mea}}& =H^{-1}{Z\text{Z}Z}_k\end{array}$$

而我们可以再回顾一下数据融合的观点，如何得到我们更加准确的结果${\widehat{X\text{X}X}}_k$(也称为后验结果)，即：
$$\begin{array}{rl}{\widehat{X\text{X}X}}_k& ={\widehat{X\text{X}X}}_k^{-}+G\text{G}G({X\text{X}X}_{k_{mea}}-{\widehat{X\text{X}X}}_k^{-})\\ & ={\widehat{X\text{X}X}}_k^{-}+{K\text{K}K}_k({Z\text{Z}Z}_k-H{\widehat{X\text{X}X}}_k^{-})\end{array}$$

这里$G\text{G}G={K\text{K}K}_{k}H$，而${K\text{K}K}_k$称为卡尔曼增益！

与之前一样，如果$G\text{G}G\to 1$估计值更加相信测量结果，而若$G\text{G}G\to 0$，估计值更加相信计算结果！

我们接着明确我们的目标：如何使得我们所估计的结果更加趋近实际的真实值？
其实也就是使得${e\text{e}e}_k={X\text{X}X}_k-{\widehat{X\text{X}X}}_k$的协方差矩阵${P\text{P}P}_k$的迹($tr({P\text{P}P}_k)$)最小(联系一下之前我们使得估计值的总体的方差是最小的！)

其实这个与我们的最小二乘法紧密相关！！

在这里，我们假设过程噪声$P(\omega \text{ω}\omega )\sim N(0,Q\text{Q}Q)$，测量噪声$P(\mu \text{μ}\mu )\sim N(0,R\text{R}R)$，并且两者相互独立的！$P\text{P}P$和$R\text{R}R$分别为对应的协方差矩阵。
我们继续化简误差${e\text{e}e}_k$：
$$\begin{array}{rl}{e\text{e}e}_k& ={X\text{X}X}_k-{\widehat{X\text{X}X}}_k\\ & ={X\text{X}X}_k-({\widehat{X\text{X}X}}_k^{-}+{K\text{K}K}_k({Z\text{Z}Z}_k-H{\widehat{X\text{X}X}}_k^{-}))\\ & ={X\text{X}X}_k-{\widehat{X\text{X}X}}_k^{-}-{K\text{K}K}_k(H{X\text{X}X}_k+{\mu \text{μ}\mu }_k)+{K\text{K}K}_{k}H{\widehat{X\text{X}X}}_k^{-}\\ & =(I\text{I}I-{K\text{K}K}_{k}H)({X\text{X}X}_k-{\widehat{X\text{X}X}}_k^{-})-{K\text{K}K}_k{\mu \text{μ}\mu }_k\\ & =(I\text{I}I-{K\text{K}K}_{k}H){e\text{e}e}_k^{-}-{K\text{K}K}_k{\mu \text{μ}\mu }_k\end{array}$$

这里我们称${e\text{e}e}_k^{-}$为先验误差

为了后面我们能够更加流畅的推导，先补充一些小知识点：
假设多变量$\omega \text{ω}\omega =[{\omega }_1{\omega }_2{]}^T$，并且${\omega }_1$和${\omega }_2$之间是相互独立的，那么其协方差矩阵$P(\omega \text{ω}\omega )=E(\omega \text{ω}\omega {\omega \text{ω}\omega }^T)$
证：
$$\begin{array}{rl}E(\omega \text{ω}\omega {\omega \text{ω}\omega }^T)& =E\left(\begin{array}{cc}{\omega }_1^2& {\omega }_1{\omega }_2\\ {\omega }_2{\omega }_1& {\omega }_2^2\end{array}\right)\\ & =\left(\begin{array}{cc}E({\omega }_1^2)& E({\omega }_1{\omega }_2)\\ E({\omega }_2{\omega }_1)& E({\omega }_2^2)\end{array}\right)\\ & =\left(\begin{array}{cc}{\sigma }_1^2& {\sigma }_1{\sigma }_2\\ {\sigma }_2{\sigma }_1& {\sigma }_2^2\end{array}\right)\end{array}$$

正因为我们之前的假设：“变量都满足无偏的正态分布”，所以我们的$E({\omega }_1)=0,E({\omega }_2)=0$，而由于我们的方差：
$$\begin{array}{rl}Var({\omega }_1)& =E\{({\omega }_1-E({\omega }_1){)}^2\}\\ & =E({\omega }_1^2)-E^2({\omega }_1)\\ & =E({\omega }_1^2)\end{array}$$

两个变量之间的协方差也是这样的：
$$\begin{array}{rl}Var({\omega }_1,{\omega }_2)& =E\{({\omega }_1-E({\omega }_1)({\omega }_2-E({\omega }_2)\}\\ & =E({\omega }_1{\omega }_2)-E({\omega }_1)E({\omega }_2)\\ & =E({\omega }_1{\omega }_2)\end{array}$$

协方差矩阵非对角元素，反应的是不同元素之间的关系。

下面我们就正式的进行推导了：
$$\begin{array}{rl}{P\text{P}P}_k& =P({e\text{e}e}_k)\\ & =E({e\text{e}e}_k{e\text{e}e}_k^T)\\ & =E(((I\text{I}I-{K\text{K}K}_{k}H){e\text{e}e}_k^{-}-{K\text{K}K}_k{\mu \text{μ}\mu }_k)((I\text{I}I-{K\text{K}K}_{k}H){e\text{e}e}_k^{-}-{K\text{K}K}_k{\mu \text{μ}\mu }_k{)}^T)\\ & =E((I\text{I}I-{K\text{K}K}_{k}H){e\text{e}e}_k^{-}{e\text{e}e}_k^{-T}(I\text{I}I-{K\text{K}K}_{k}H{)}^T-(I\text{I}I-{K\text{K}K}_{k}H)){e\text{e}e}_k^{-}{\mu \text{μ}\mu }_k^T{K\text{K}K}_k^T-{K\text{K}K}_k{\mu \text{μ}\mu }_k{e\text{e}e}_k^{-T}(I\text{I}I-{K\text{K}K}_{k}H{)}^T+{K\text{K}K}_k{\mu \text{μ}\mu }_k{\mu \text{μ}\mu }_k^T{K\text{K}K}_k^T\\ & =(I\text{I}I-{K\text{K}K}_{k}H)E({e\text{e}e}_k^{-}{e\text{e}e}_k^{-T})(I\text{I}I-{K\text{K}K}_{k}H{)}^T+{K\text{K}K}_{k}E({\mu \text{μ}\mu }_k{\mu \text{μ}\mu }_k^T){K\text{K}K}_k^T\\ & =(I\text{I}I-{K\text{K}K}_{k}H){P\text{P}P}_k^{-}(I\text{I}I-{K\text{K}K}_{k}H{)}^T+{K\text{K}K}_{k}R\text{R}R{K\text{K}K}_k^T\\ & =(I\text{I}I-{K\text{K}K}_{k}H){P\text{P}P}_k^{-}(I\text{I}I-H^T{K\text{K}K}_k^T)+{K\text{K}K}_{k}R\text{R}R{K\text{K}K}_k^T\\ & ={P\text{P}P}_k^{-}-{K\text{K}K}_{k}H{P\text{P}P}_k^{-}-{P\text{P}P}_k^{-}{H}^T{K\text{K}K}_k^T+{K\text{K}K}_{k}H{P\text{P}P}_k^{-}{H}^T{K\text{K}K}_k^T+{K\text{K}K}_{k}R\text{R}R{K\text{K}K}_k^T\end{array}$$

注意，这里我们有两个关键点需要注意一下哦，变量之间假设是相互独立的，并且我们的噪声是服从无偏的正态分布的。
即：$E({e\text{e}e}_k^{-}{\mu \text{μ}\mu }_k^T)=E({e\text{e}e}_k^{-})E({\mu \text{μ}\mu }_k^T)=0=E({\mu \text{μ}\mu }_k{e\text{e}e}_k^{-T})$

下面，再一次明确我们的目的：如何找到最优的卡尔曼增益使得我们的$tr({P\text{P}P}_k)$最小？(也就是使得我们的估计值更加接近真实值)
我们可以直接利用求极值的方法，来获得最小的误差(因为误差可以无限大，也就是所误差是没有最大值的，因此我们仅可能得到极小值)
$$\begin{array}{rl}tr(P\text{P}P)& =tr({P\text{P}P}_k^{-}-{K\text{K}K}_{k}H{P\text{P}P}_k^{-}-{P\text{P}P}_k^{-}{H}^T{K\text{K}K}_k^T+{K\text{K}K}_{k}H{P\text{P}P}_k^{-}{H}^T{K\text{K}K}_k^T+{K\text{K}K}_{k}R\text{R}R{K\text{K}K}_k^T)\\ & =tr({P\text{P}P}_k^{-})-2tr({K\text{K}K}_{k}H{P\text{P}P}_k^{-})+tr({K\text{K}K}_{k}H{P\text{P}P}_k^{-}{H}^T{K\text{K}K}_k^T)+tr({K\text{K}K}_{k}R\text{R}R{K\text{K}K}_k^T))\end{array}$$

• 要明确我们协方差矩阵是一个对称阵，而且要知道转置并不会改变我们所求的迹。
• 引入几个矩阵分析的公式：
  $$\begin{array}{rl}\frac{dtr(AB)}{dA}& =B^T\\ \frac{dtr(AB{A}^T)}{dA}& =2AB\end{array}$$

所以我们可以令$\frac{dtr({P\text{P}P}_k)}{d{K\text{K}K}_k}=0$，求得我们得到最小值的时候，${K\text{K}K}_k$的取值啦！
$$\begin{array}{rl}\frac{dtr({P\text{P}P}_k)}{d{K\text{K}K}_k}& =\frac{tr({P\text{P}P}_k^{-})-2tr({K\text{K}K}_{k}H{P\text{P}P}_k^{-})+tr({K\text{K}K}_{k}H{P\text{P}P}_k^{-}{H}^T{K\text{K}K}_k^T)+tr({K\text{K}K}_{k}R\text{R}R{K\text{K}K}_k^T))}{d{K\text{K}K}_k}\\ & =0-2(H{P\text{P}P}_k^{-}{)}^T+2{K\text{K}K}_{k}H{P\text{P}P}_k^{-}{H}^T+2{K\text{K}K}_{k}R\text{R}R=0\end{array}$$
因此，我们可以得到卡尔曼增益${K\text{K}K}_k$
$${K\text{K}K}_k={P\text{P}P}_k^{-}{H}^T(H{P\text{P}P}_k^{-}{H}^T+R\text{R}R{)}^{-1}$$

至此，我们现在已经得到了卡尔曼滤波的：

• 先验估计：${\widehat{X\text{X}X}}_k^{-}=A{\widehat{X\text{X}X}}_{k-1}+B{u\text{u}u}_{k-1}$
• 后验估计：${\widehat{X\text{X}X}}_k={\widehat{X\text{X}X}}_k^{-}+{K\text{K}K}_k({Z\text{Z}Z}_k-H{\widehat{X\text{X}X}}_k^{-})$
• 卡尔曼增益：${K\text{K}K}_k={P\text{P}P}_k^{-}{H}^T(H{P\text{P}P}_k^{-}{H}^T+R\text{R}R{)}^{-1}$

然而我们仔细想想，上述的公式中还有一个参数${P\text{P}P}_k^{-}$未知的，因而下面我们接着求这个参数：
(有一个更加简单的理解方式：不过感觉有点牵强，${\widehat{x\text{x}x}}_{k-1}\to {P\text{P}P}_{k-1}$，而${\widehat{x\text{x}x}}_k^{-}\to {P\text{P}P}_k^{-}$，并且因为${\widehat{X\text{X}X}}_k^{-}=A{\widehat{X\text{X}X}}_{k-1}$，就可以根据协方差矩阵的性质，我们就可以得到${P\text{P}P}_k^{-}=A{P\text{P}P}_{k-1}{A}^T$，而由于有外界的干扰，所以我们需要在加上$Q\text{Q}Q$，即： ${P\text{P}P}_k^{-}=A{P\text{P}P}_{k-1}{A}^T+Q\text{Q}Q$
$$\begin{array}{rl}{P\text{P}P}_k^{-}& =E({e\text{e}e}_k^{-}{e\text{e}e}_k^{-T})\\ & =E((A{e\text{e}e}_{k-1}+{\omega \text{ω}\omega }_{k-1})(A{e\text{e}e}_{k-1}+{\omega \text{ω}\omega }_{k-1}{)}^T)\\ & =E((A{e\text{e}e}_{k-1}+{\omega \text{ω}\omega }_{k-1})({e\text{e}e}_{k-1}^T{A}^T+{\omega \text{ω}\omega }_{k-1}^T))\\ & =A{P\text{P}P}_{k-1}{A}^T+Q\text{Q}Q\end{array}$$

在这里尽管我们给出了参数${P\text{P}P}_k^{-}$，但是我们还引入了参数了${P\text{P}P}_{k-1}$。
为此我们还需要来得到我们的参数${P\text{P}P}_{k-1}$的更新值：
$$\begin{array}{rl}{P\text{P}P}_k& ={P\text{P}P}_k^{-}-{K\text{K}K}_{k}H{P\text{P}P}_k^{-}-{P\text{P}P}_k^{-}{H}^T{K\text{K}K}_k^T+{K\text{K}K}_{k}H{P\text{P}P}_k^{-}{H}^T{K\text{K}K}_k^T+{K\text{K}K}_{k}R\text{R}R{K\text{K}K}_k^T\\ & ={P\text{P}P}_k^{-}-{K\text{K}K}_{k}H{P\text{P}P}_k^{-}-{P\text{P}P}_k^{-}{H}^T{K\text{K}K}_k^T+{K\text{K}K}_k(H{P\text{P}P}_k^{-}{H}^T+R\text{R}R){K\text{K}K}_k^T\\ & ={P\text{P}P}_k^{-}-{K\text{K}K}_{k}H{P\text{P}P}_k^{-}-{P\text{P}P}_k^{-}{H}^T{K\text{K}K}_k^T+{P\text{P}P}_k^{-}{H}^T{K\text{K}K}_k^T\\ & ={P\text{P}P}_k^{-}-{K\text{K}K}_{k}H{P\text{P}P}_k^{-}\\ & =(I\text{I}I-{K\text{K}K}_{k}H){P\text{P}P}_k^{-}\end{array}$$
至此，我们完成了卡尔曼滤波器的所有的推导，下面我们来进行总结：

先验估计	校正	备注
${\widehat{X\text{X}X}}_k^{-}=A{\widehat{X\text{X}X}}_{k-1}+B{u\text{u}u}_{k-1}$	${K\text{K}K}_k={P\text{P}P}_k^{-}{H}^T(H{P\text{P}P}_k^{-}{H}^T+R\text{R}R{)}^{-1}$	需要初始值${\widehat{X\text{X}X}}_0$
${P\text{P}P}_k^{-}=A{P\text{P}P}_{k-1}{A}^T+Q$	${\widehat{X\text{X}X}}_k={\widehat{X\text{X}X}}_k^{-}+{K\text{K}K}_k({Z\text{Z}Z}_k-H{\widehat{X\text{X}X}}_k^{-})$	需要初始值${P\text{P}P}_0$
	${P\text{P}P}_k=(I\text{I}I-{K\text{K}K}_{k}H){P\text{P}P}_k^{-}$

工作流程如下：

参考资料

卡尔曼滤波算法
细致入微的Kalman滤波算法
细致入微的Kalman滤波算法，翻译

补充资料

卡尔曼滤波算法与最小二乘