卡尔曼滤波（KF）、拓展卡尔曼滤波（EKF）推导

背景知识

卡尔曼滤波是建立在贝叶斯滤波和高斯分布这两个前提上的，所以下面先大概讲一下这两者。

贝叶斯滤波

首先给出几个基本概念和公式

• 联合分布
  $$p(x,y)=p(X=x,Y=y)$$
  表示x,y同时发生的概率
• 条件概率
  $$p(x|y)=p(X=x|Y=y)$$
  表示x在y已经发生的基础上发生的概率，设x、y相互独立（以后都是如此），则有
  $$p(x|y)=\frac{p(x,y)}{p(y)}$$
• 先验概率
  可以被称作经验，当前数据读取前，系统估计出来的状态的概率分布
• 后验概率
  在数据读取后（观测后），综合先验概率和观测概率得到的状态的概率分布
• 全概率定律
  $$p(x)=\sum _{y}p(x|y)p(y)（离散情况）$$
  $$p(x)=\int p(x|y)p(y)dy（连续情况）$$
• 贝叶斯准则
  $$p(x|y)=\frac{p(y|x)p(x)}{p(y)}=\frac{p(y|x)p(x)}{{\sum }_{x^{{}^{\prime }}}p(y|x^{{}^{\prime }})p(x^{{}^{\prime }})}（离散）$$
  $$p(x|y)=\frac{p(y|x)p(x)}{p(y)}=\frac{p(y|x)p(x)}{\int p(y|x^{{}^{\prime }})p(x^{{}^{\prime }})d{x}^{{}^{\prime }}}（连续）$$
  其中x被称作状态，y被称作数据，$p(x)$被称作先验概率，$p(y|x)$被称为“逆”条件概率，而$p(y{)}^{-1}$和x无关，常作为一个系数$\eta$。
  需要注意的是，一般情况下，x是在运算过程中作为自变量，我们的p是x的概率分布函数（在卡尔曼滤波里为高斯分布，在一些情况下可能是分段函数），而不是针对具体的x的概率值。在状态的转移过程中，预测的概率分布由前一个时刻的全部可能的x取值加权积分／求和得到
• 完整性/马尔科夫性
  假设一个状态$x_t$可以最好地预测未来，也就是说过去的一切控制信息都包含在其中并无法对之后产生影响（或者说过去信息要作用于未来，必须依赖于$x_t$），则称$x_t$是完整的，贝叶斯滤波是基于这个假设的。

贝叶斯滤波就是建立在贝叶斯准则上的，基本方法是通过先验概率和观测值去得到后验概率，得到较为准确的概率分布。
贝叶斯滤波的算法简单概括为两（三）行：

$$\begin{array}{l}1:\overline{bel}(x_t)=\int p(x_t|u_t,x_{t-1})bel(x_{t-1})d{x}_{t-1}（连续）\\ 2:\overline{bel}(x_t)=\sum p(x_t|u_t,x_{t-1})bel(x_{t-1})（离散）\\ 3:bel(x_t)=\eta p(z_t|x_t)\overline{bel}(x_t)\end{array}$$

其中：

• $\eta$是归一化系数，在计算的过程中因各种常数的提出而不断发生变化。
• $x$表示状态量
• $z$表示观测数据
• $u$表示里程计记录的运动数据
• $bel、\overline{bel}$代表置信值，$bel(x_t)=p(x_t|z_t)，$即观测后得到的$x_t$概率分布，$\overline{bel}$代表内部预测的置信值。
• $p(x_t|u_t,x_{t-1})$表示状态转移概率，表示$x_{t-1}、u_t$确定的情况下，$x_t$的概率分布函数
• $p(z_t|x_t)$表观上表示$x_t$确定的情况下，$z_t$的概率分布（通过z关于x的函数），实际上由于x是未知的，z是已知的，在运算过程中x作为自变量，z作为常数。
• 行1、2代表的是预测或者叫控制更新
• 行3代表的是观测更新

算法有一个前提是完整性假设，如用$p(x_t|u_t,x_{t-1})$来表示$p(x_t|u_{1:t},x_{0:t-1},z_{1:t-1})$

简单推导

预测

$$\begin{array}{rl}\overline{bel}(x_t)& =p(x_t|z_{1:t-1},u_{1:t})\\ & =\int p(x_t|x_{t-1},z_{1:t-1},u_{1:t})p(x_{t-1}|z_{1:t-1},u_{1:t})d{x}_{t-1}\\ & =\int p(x_t|x_{t-1},u_t)bel(x_{t-1})d{x}_{t-1}\end{array}$$

观测更新

$$\begin{array}{rl}bel(x_t)=p(x_t|z_{1:t},u_{1:t})& =\frac{p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t})}{p(z_t|z_{1:t-1,u_{1:t}})}\\ & =\eta p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t})\\ & =\eta p(z_t|x_t)\overline{bel}(x_t)\end{array}$$

高斯分布（正态分布）

贝叶斯滤波的x取值范围是离散的，所以贝叶斯滤波并不是真正可用的算法，贝叶斯滤波在高斯分布的基础上构成的高斯滤波可以让x取值连续，卡尔曼滤波是其中的一种。

高斯分布由以下函数表示
$$p(x)=(2\pi {\sigma }^2{)}^{-\frac{1}{2}}{e}^{-\frac{1}{2}\frac{(x-\mu {)}^2}{{\sigma }^2}}（x为标量）$$
其中${\sigma }^2$为方差，$\mu$为均值
$$p(\underline{x})=(2\pi \Sigma {)}^{-\frac{1}{2}}{e}^{-\frac{1}{2}(\underline{x}-\underline{\mu }{)}^T{\Sigma }^{-1}(\underline{x}-\underline{\mu })}（\underline{x}为向量）$$
其中$\Sigma$为方差（协方差矩阵），$\underline{\mu }$为均值
为什么用高斯分布是因为高斯分布是自然界广泛存在的，且具有很好的性质，虽然也有缺点（单峰）。在卡尔曼滤波中，没有确定的状态，测量值、预测量和最后的校正值都是用高斯分布表示的。

高斯分布中， $p$的积分（累加）为1，当然这也是所有概率分布函数的规律。

高斯分布的融合

本人在第一遍推导的学习中没有用到以下公式，但用了后似乎后面的推导都形同虚设…先挖个坑

• 高斯分布乘高斯分布还是高斯分布（从结构上看显然）

• 对于高斯分布的乘积$X=X_1{X}_2$，其中
  $$\{\begin{array}{l}{X}_1\sim \mathbb{N}({\mu }_1,{\Sigma }_1)\\ X_2\sim \mathbb{N}({\mu }_2,{\Sigma }_2)\end{array}$$
  可以得到如下结果：

$$\{\begin{array}{l}K={\Sigma }_1({\Sigma }_1+{\Sigma }_2{)}^{-1})\\ \mu ={\mu }_1+K({\mu }_2-{\mu }_1)\\ \Sigma ={\Sigma }_1-K{\Sigma }_1\end{array}$$
其中K为卡尔曼增益，证明略

线代相关

自己去复习／预习

卡尔曼滤波（KF）

卡尔曼滤波建立在线性的假设上的，我们假设：
$$x_t=A_t{x}_{t-1}+B_t{u}_t+{\epsilon }_t（状态转移函数）$$
其中${\epsilon }_t$是均值为0，方差为$R_t$的高斯随机向量

$$z_t=C_t{x}_t+{\delta }_t（测量函数）$$
其中${\delta }_t$是均值为0，方差为$Q_t$的高斯随机向量

$$bel(x_0)=p(x_0)（初始置信度）$$
其中$p$是均值为${\mu }_0$，方差为${\Sigma }_0$的高斯随机向量

卡尔曼滤波可以表示为以下几行算法

$$\begin{array}{l}1:{\overline{\mu }}_t=A_t{\mu }_{t-1}+B_t{u}_t\\ 2:{\overline{\Sigma }}_t=A_t{\Sigma }_{t-1}{A}_t^T+R_t\\ 3:K_t={\overline{\Sigma }}_t{C}_t^T(C_t{\overline{\Sigma }}_t{C}_t^T+Q_t{)}^{-1}\\ 4:{\mu }_t={\overline{\mu }}_t+K_t(z_t-C_t{\overline{\mu }}_t)\\ 5:{\Sigma }_t=(I-K_t{C}_t){\overline{\Sigma }}_t\end{array}$$

下面给出证明

KF的数学推导

预测

根据贝叶斯滤波，我们得到
$$\overline{bel}(x_t)=\int p(x_t|u_t,x_{t-1})bel(x_{t-1})d{x}_{t-1}$$
所以有
$$\overline{bel}(x_t)=\eta \int e^{-L_t}d{x}_{t-1}$$
其中
$$L_t=\frac{1}{2}(x_t-A_t{x}_{t-1}-B_t{u}_t{)}^T{R}_t^{-1}(x_t-A_t{x}_{t-1}-B_t{u}_t)+\frac{1}{2}(x_{t-1}-{\mu }_{t-1}{)}^T{\Sigma }_{t-1}^{-1}(x_{t-1}-{\mu }_{t-1})$$
$L_t$是$x_t$也是$x_{t-1}$的二次函数
为了避免积分运算，我们令
$$L_t=L_t(x_{t-1},x_t)+L_t(x_t)$$
提出一项不包含$x_{t-1}$的$L_t(x_t)$
$$\overline{bel}(x_t)=\eta e^{-L_t(x_t)}\int e^{-L_t(x_{t-1},x_t)}d{x}_{t-1}$$
下面把$L_t(x_{t-1},x_t)$构造成二次型（配方后）（个人理解是这样做不会再次提出含$x_t$的项）
接下来计算$L_t$关于$x_{t-1}$的一二阶导数，得到
$$L_t(x_{t-1},x_t)=\frac{1}{2}(x_{t-1}-\Psi [A_t^T{R}_t^{-1}(x_t-B_t{u}_t)+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}]{)}^T{\Psi }^{-1}(x_{t-1}-\Psi [A_t^T{R}_t^{-1}(x_t-B_t{u}_t)+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}])$$

又因为
$$\int det(2\pi \Psi {)}^{-\frac{1}{2}}{e}^{-L_t(x_{t-1},x_t)}d{x}_{t-1}=1$$
所以
$$\int e^{-L_t(x_{t-1},x_t)}d{x}_{t-1}=det(2\pi \Psi {)}^{\frac{1}{2}}$$

所以
$$\overline{bel}(x_t)=\eta e^{-L_t(x_t)}$$
现在计算$L_t(x_t)$:
$$\begin{array}{rl}{L}_t(x_t)& =L_t-L_t(x_{t-1},x_t)\\ & =...(含x_{t-1}项全部消去)\\ & =\frac{1}{2}(x_t-B_t{u}_t{)}^T{R}_t^{-1}(x_t-B_t{u}_t)+\frac{1}{2}{\mu }_{t-1}^T{\Sigma }_{t-1}^{-1}{\mu }_{t-1}-\frac{1}{2}[A_t^T{R}_t^{-1}(x_t-B_t{u}_t)+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}{]}^T(A_t^T{R}_t^{-1}{A}_t+{\Sigma }_{t-1}^{-1})[A_t^T{R}_t^{-1}(x_t-B_t{u}_t)+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}]\end{array}$$
尽管这不是关于$x_t$的二次型（配方后），但确实是一个二次函数，无非影响了前面的系数。
求一二阶导数来得到$x_t$的均值和方差：
$$\begin{array}{rl}\frac{\partial L_t(x_t)}{\partial x_t}& =R_t^{-1}(x_t-B_t{u}_t)-R_t^{-1}{A}_t(A_t^T{R}_t^{-1}{A}_t+{\Sigma }_{t-1}^{-1}{)}^{-1}[A_t^T{R}_t^{-1}(x_t-B_t{u}_t)+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}]\\ & =[\underline{R_t^{-1}-R_t^{-1}{A}_t(A_t^T{R}_t^{-1}{A}_t+{\Sigma }_{t-1}^{-1}{)}^{-1}{A}_t^T{R}_t^{-1}}](x_t-B_t{u}_t)-R_t^{-1}{A}_t(A_t^T{R}_t^{-1}{A}_t+{\Sigma }_{t-1}^{-1}{)}^{-1}{\Sigma }_{t-1}^{-1}{\mu }_{t-1}\end{array}$$

由谢尔曼莫里森公式（证明打起来太慢了，提示：可以通过$\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]\left[\begin{array}{c}{x}_A\\ x_B\end{array}\right]=\left[\begin{array}{c}{y}_A\\ y_B\end{array}\right]$求分块矩阵$\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]$的两个逆矩阵的表达形式，利用两个逆矩阵相等得到。）
$$R_t^{-1}-R_t^{-1}{A}_t(A_t^T{R}_t^{-1}{A}_t+{\Sigma }_{t-1}^{-1}{)}^{-1}{A}_t^T{R}_t^{-1}=(R_t+A_t{\Sigma }_{t-1}{A}_t^T{)}^{-1}$$

因此
$$\begin{array}{rl}\frac{\partial L_t(x_t)}{\partial x_t}& =(R_t+A_t{\Sigma }_{t-1}{A}_t^T{)}^{-1}(x_t-B_t{u}_t)-R_t^{-1}{A}_t(A_t^T{R}_t^{-1}{A}_t+{\Sigma }_{t-1}^{-1}{)}^{-1}{\Sigma }_{t-1}^{-1}{\mu }_{t-1}\\ & =0\end{array}$$

得到
$$\begin{array}{rl}{x}_t& =B_t{u}_t+(R_t+A_t{\Sigma }_{t-1}{A}_t^T)R_t^{-1}{A}_t(A_t^T{R}_t^{-1}{A}_t+{\Sigma }_{t-1}^{-1}{)}^{-1}{\Sigma }_{t-1}^{-1}{\mu }_{t-1}\\ & =B_t{u}_t+A_t(I+{\Sigma }_{t-1}{A}_t^T{R}_t^{-1}{A}_t)(I+{\Sigma }_{t-1}{A}_t^T{R}_t^{-1}{A}_t{)}^{-1}{\mu }_{t-1}\\ & =B_t{u}_t+A_t{\mu }_{t-1}\end{array}$$
那么
$${\overline{\mu }}_t=A_t{\mu }_{t-1}+B_t{u}_t$$
$${\overline{\Sigma }}_t=[\frac{{\partial }^2{L}_t(x_t)}{\partial x_t^2}{]}^{-1}=(A_t{\Sigma }_{t-1}{A}_t^T+R_t)$$

测量更新

$$bel(x_t)=\eta p(z_t|x_t)\overline{bel}(x_t)=\eta e^{-J_t}$$
其中
$$J_t=\frac{1}{2}(z_t-C_t{x}_t{)}^T{Q}_t^{-1}(z_t-C_t{x}_t)+\frac{1}{2}(x_t-{\overline{\mu }}_t{)}^T{\Sigma }_t^{-1}(x_t-{\overline{\mu }}_t)$$
求导数得
$$\begin{array}{rl}{\Sigma }_t& =(C^T{Q}_t^{-1}{C}_t+{\overline{\Sigma }}_t^{-1}{)}^{-1}\end{array}$$
因为求二次函数的极小值，用${\mu }_t$替换$x_t$:
$$C_t^T{Q}_t^{-1}(z_t-C_t{\mu }_t)={\overline{\Sigma }}_t^{-1}({\mu }_t-{\overline{\mu }}_t)$$
$$\begin{array}{rl}左边& =C_t^T{Q}_t^{-1}(z_t-C_t{\mu }_t+C_t{\overline{\mu }}_t-C_t{\overline{\mu }}_t)\\ & =C_t^T{Q}_t^{-1}(z_t-C_t{\overline{\mu }}_t)-C_t^T{Q}_t^{-1}{C}_t({\mu }_t-{\overline{\mu }}_t)\end{array}$$
代回得
$$\begin{gathered} C_t^T{Q}_t^{-1}(z_t-C_t{\overline{\mu }}_t)={\Sigma }_t^{-1}({\mu }_t-{\overline{\mu }}_t) \\ {\Sigma }_t{C}_t^T{Q}_t^{-1}(z_t-C_t{\overline{\mu }}_t)=({\mu }_t-{\overline{\mu }}_t) \end{gathered}$$
令$K={\Sigma }_t{C}_t^T{Q}_t^{-1}$，称K为卡尔曼增益
$${\mu }_t-{\overline{\mu }}_t=K(z_t-C_t{\overline{\mu }}_t)$$

$$\begin{array}{rl}{K}_t& ={\Sigma }_t{C}_t^T{Q}_t^{-1}\\ & ={\Sigma }_t{C}_t^T{Q}_t^{-1}(C_t{\overline{\Sigma }}_t{C}_t^T+Q_t)(C_t{\overline{\Sigma }}_t{C}_t^T+Q_t{)}^{-1}\\ & ={\Sigma }_t(C_t^T{Q}_t^{-1}{C}_t{\overline{\Sigma }}_t{C}_t^T+C_t^T{Q}_t^{-1}{Q}_t)(C_t{\overline{\Sigma }}_t{C}_t^T+Q_t{)}^{-1}\\ & ={\Sigma }_t(C_t^T{Q}_t^{-1}{C}_t{\overline{\Sigma }}_t{C}_t^T+C_t^T)(C_t{\overline{\Sigma }}_t{C}_t^T+Q_t{)}^{-1}\\ & ={\Sigma }_t(C_t^T{Q}_t^{-1}{C}_t{\overline{\Sigma }}_t{C}_t^T+{\overline{\Sigma }}_t^{-1}{\overline{\Sigma }}_t{C}_t^T)(C_t{\overline{\Sigma }}_t{C}_t^T+Q_t{)}^{-1}\\ & ={\Sigma }_t(C_t^T{Q}_t^{-1}{C}_t+{\overline{\Sigma }}_t^{-1}){\overline{\Sigma }}_t{C}_t^T(C_t{\overline{\Sigma }}_t{C}_t^T+Q_t{)}^{-1}\\ & ={\overline{\Sigma }}_t{C}_t^T(C_t{\overline{\Sigma }}_t{C}_t^T+Q_t{)}^{-1}\end{array}$$

此时，可以继续化简${\Sigma }_t$
$$\begin{array}{rl}{\Sigma }_t& =(C^T{Q}_t^{-1}{C}_t+{\overline{\Sigma }}_t^{-1}{)}^{-1}\\ & ={\overline{\Sigma }}_t-{\overline{\Sigma }}_t{C}_t^T(Q_t+C_t{\overline{\Sigma }}_t{C}_t^T{)}^{-1}{C}_t{\overline{\Sigma }}_t\\ & =[(I-K_t{C}_t)]{\overline{\Sigma }}_t\end{array}$$

拓展卡尔曼滤波（EKF）

考虑到非线性情况
$$\begin{gathered} x_t=g(u_t,x_{t-1})+{\epsilon }_t \\ {z}_t=h(x_t)+{\delta }_t \end{gathered}$$
对此，EKF提出泰勒展开保留一阶导数：
$$\begin{gathered} G_t=\frac{\partial g(u_t,x_{t-1})}{\partial x_{t-1}},x_{t-1}={\mu }_{t-1} \\ {H}_t=\frac{\partial h(x_t)}{\partial x_t},x_t={\overline{\mu }}_t \end{gathered}$$
那么
$$\begin{gathered} g(u_t,x_{t-1})=g(u_t,{\mu }_{t-1})+G_t(x_{t-1}-{\mu }_{t-1}) \\ h(x_t)=h({\overline{\mu }}_t)+H_t(x_t-{\overline{\mu }}_t) \end{gathered}$$
类似KF，可证明EKF如下：

$$\begin{array}{l}1:{\overline{\mu }}_t=g(u_t,{\mu }_{t-1})\\ 2:{\overline{\Sigma }}_t=G_t{\Sigma }_{t-1}{G}_t^T+R_t\\ 3:K_t={\overline{\Sigma }}_t{H}_t^T(H_t{\overline{\Sigma }}_t{H}_t^T+Q_t{)}^{-1}\\ 4:{\mu }_t={\overline{\mu }}_t+K_t(z_t-h({\overline{\mu }}_t))\\ 5:{\Sigma }_t=(I-K_t{H}_t){\overline{\Sigma }}_t\end{array}$$