第二十课.卡尔曼滤波器

卡尔曼滤波器的参数

首先回顾卡尔曼滤波的概率图：

基于之前内容，卡尔曼滤波器是一种动态图模型，除了遵循齐次马尔科夫假设和观测独立性假设外，卡尔曼滤波器还遵循$Z_t$和$Z_{t-1}$之间，$X_t$和$Z_t$之间的线性关系：$$Z_t=A{Z}_{t-1}+B+\epsilon$$$$X_t=C{Z}_t+D+\delta$$
这里，$(A,B,C,D)$是线性系数，$(\epsilon ,\delta )$是两个满足均值为0的高斯分布随机变量，记作：$$\epsilon \sim N(0,Q),\delta \sim N(0,R)$$
另外，卡尔曼滤波器中，无论是隐变量$Z$，还是观测变量$X$，都服从高斯分布：$$p(z_t|z_{t-1})=N(A{z}_{t-1}+B,Q)$$$$p(x_t|z_t)=N(C{z}_t+D,R)$$
对于模型的第一个变量，即隐变量$z_1$，我们直接令其服从高斯分布：$p(z_1)=N({\mu }_1,{\Sigma }_1)$，这时，已经可以明确卡尔曼滤波器的参数为：$$\theta =(A,B,C,D,Q,R,{\mu }_1,{\Sigma }_1)$$
模型明确后，我们需要关注推理问题中的滤波问题，即计算概率$p(z_t|x_1,x_2,...,x_t)$的过程。

滤波问题的解决方案

卡尔曼滤波器按照时间的推演顺序，我们依次观察到观测变量$x_1,x_2,...,x_T$，在观察到$x_t$前，我们会先观察到$x_{t-1}$，我们应该建立$p(z_t|x_1,x_2,...,x_t)$和$p(z_{t-1}|x_1,x_2,...,x_{t-1})$这前后相邻两个时刻的滤波表达式间的递推关系，关系建立后，我们就能从$p(z_1|x_1)$依次推导到$p(z_t|x_1,x_2,...,x_t)$。

首先依据贝叶斯公式有：$$p(z_t|x_1,x_2,...,x_t)=\frac{p(x_1,x_2,...,x_t,z_t)}{p(x_1,x_2,...,x_t)}$$
由于$p(x_1,x_2,...,x_t)$描述的是这一组观测变量的概率，因此是一个常数，所以就转换为以下正比关系：$$p(z_t|x_1,x_2,...,x_t)\propto p(x_1,x_2,...,x_t,z_t)$$
再次依据贝叶斯定理展开：$$p(x_1,x_2,...,x_t,z_t)=p(x_t|x_1,x_2,...,x_{t-1},z_t)p(x_1,x_2,...,x_{t-1},z_t)$$$$=p(x_t|z_t)p(x_1,x_2,...,x_{t-1},z_t)$$
再展开：$$p(x_1,x_2,...,x_{t-1},z_t)=p(z_t|x_1,x_2,...,x_{t-1})p(x_1,x_2,...,x_{t-1})$$
所以有：$$p(x_1,x_2,...,x_t,z_t)=p(x_t|z_t)p(z_t|x_1,x_2,...,x_{t-1})p(x_1,x_2,...x_{t-1})$$
该式子包含了两个概念：

• 第一：$p(z_t|x_1,x_2,...,x_{t-1})$是预测问题；
• 第二：$p(x_1,x_2,...,x_{t-1})$是关于观测变量的预测，是一个常数，因此式子可以用正比关系表达；

因此有：$$p(x_1,x_2,...,x_t,z_t)\propto p(x_t|z_t)p(z_t|x_1,x_2,...,x_{t-1})$$$$\Rightarrow p(z_t|x_1,x_2,...,x_t)\propto p(x_1,x_2,...,x_t,z_t)\propto p(x_t|z_t)p(z_t|x_1,x_2,...,x_{t-1})$$
然后思考预测问题的概率化简：$$p(z_t|x_1,x_2,...,x_{t-1})={\int }_{z_{t-1}}p(z_t,z_{t-1}|x_1,x_2,...,x_{t-1})d{z}_{t-1}$$$$={\int }_{z_{t-1}}p(z_t|z_{t-1},x_1,x_2,...,x_{t-1})p(z_{t-1}|x_1,x_2,...,x_{t-1})d{z}_{t-1}$$$$={\int }_{z_{t-1}}p(z_t|z_{t-1})p(z_{t-1}|x_1,x_2,...,x_{t-1})d{z}_{t-1}$$
此时，得到两个具有明显含义的式子：

• 基于马尔科夫假设化简得到的$p(z_t|z_{t-1})$；
• $t-1$时刻的滤波问题表达式$p(z_{t-1}|x_1,x_2,...,x_{t-1})$；

卡尔曼滤波的predict和update

在递推的过程中，$t-1$时刻，总是先利用$t-1$时刻的滤波结果，在$t-1$时刻的观测序列$x_1,x_2,...,x_{t-1}$的基础上，去预测下一时刻$t$的状态$z_t$的概率，这一步，称为predict：$$p(z_t|x_1,x_2,...,x_{t-1})={\int }_{z_{t-1}}p(z_t|z_{t-1})p(z_{t-1}|x_1,x_2,...,x_{t-1})d{z}_{t-1}$$
得到$t-1$时刻对$t$时刻状态的预测后，进入$t$时刻，此时，新增$t$时刻的观测值$x_t$，我们通过它来对$t-1$时刻的预测进行修正，也就是修正隐变量$z_t$的概率，这一步称为update：$$p(z_t|x_1,x_2,...,x_t)\propto p(x_t|z_t)p(z_t|x_1,x_2,...,x_{t-1})$$
直观地，从$t=1$演示以上过程：

$t=1$时刻：
update：计算$p(z_1|x_1)$；
predict：计算$p(z_2|x_1)$；

$t=2$时刻：
update：计算$p(z_2|x_2,x_1)$；
predict：计算$p(z_3|x_2,x_1)$；

然后不断按照$t=3,4,...,n-1,n$迭代；对于$t=1$的启动步update，$p(z_1|x_1)\propto p(x_1|z_1)p(z_1)$，隐状态$z_1$会是一个提前给定的正态分布。

基于正态分布得到解析结果

证明解析解存在

在上面看到，predict和update都是比较复杂的运算，实际处理时，我们将基于正态分布的性质得到解析结果。

卡尔曼滤波器中有重要假设，即服从带高斯分布噪声的线性关系：$$p(z_t|z_{t-1})=N(A{z}_{t-1}+B,Q)$$$$p(x_t|z_t)=N(C{z}_t+D,R)$$$$p(z_1)=N({\mu }_1,{\Sigma }_1)$$
另外已知，高斯分布的联合概率，边缘概率，条件概率依然是高斯分布。

从$t=1$时刻看：
update：$$p(z_1|x_1)\propto p(x_1|z_1)p(z_1)$$
其中，$p(x_1|z_1)$和$p(z_1)$都服从正态分布，它们联合概率的乘积$p(z_1|x_1)$也服从正态分布。

predict：$$p(z_2|x_1)={\int }_{z_1}p(z_2|z_1)p(z_1|x_1)d{z}_1$$
这里$p(z_1|x_1)$和$p(z_2|z_1)$都服从正态分布，所以联合概率$p(z_2|z_1)p(z_1|x_1)$也服从正态分布，边缘概率$p(z_2|x_1)={\int }_{z_1}p(z_2|z_1)p(z_1|x_1)d{z}_1$也服从正态分布。

过度到$t-1$时刻的predict步：$$p(z_t|x_1,...,x_{t-1})={\int }_{z_{t-1}}p(z_t|z_{t-1})p(z_{t-1}|x_1,x_2,...,x_{t-1})d{z}_{t-1}$$
$p(z_t|z_{t-1})$和$p(z_{t-1}|x_1,...,x_{t-1})$都服从正态分布，所以其联合概率，边缘概率也服从。

再进入$t$时刻的update步：$$p(z_t|x_1,...,x_t)\propto p(x_t|z_t)p(z_t|x_1,...,x_{t-1})$$
同样服从正态分布，所以卡尔曼滤波器的每一步都可以得到解析解。

解析解计算

假设$p(x)$和$p(y|x)$都服从正态分布，且随机变量$y$和$x$满足带噪声的线性关系，所以有：$$p(x)=N(X|\mu ,{\Lambda }^{-1})$$$$p(y|x)=N(y|Ax+b,L^{-1})$$
所以有：$$p(y)={\int }_{x}p(x)p(y|x)dx=N(y|A\mu +b,L^{-1}+A{\Lambda }^{-1}{A}^T)$$
这个过程于predict对应，所以有：$$p(x)\Rightarrow p(z_{t-1}|x_1,x_2,...,x_{t-1})=N({\mu }_{t-1},{\Sigma }_{t-1})$$$$p(y|x)\Rightarrow p(z_t|z_{t-1})=N(A{z}_{t-1}+B,Q)$$$$p(y)\Rightarrow p(z_t|x_1,...,x_{t-1})=N({\mu }_t^{\ast },{\Sigma }_t^{\ast })$$
其中，参数为：$${\mu }_t^{\ast }=A{\mu }_{t-1}+B$$$${\Sigma }_t^{\ast }=Q+A{\Sigma }_{t-1}{A}^T$$
所以predict的预测结果可以得到，由于服从正态分布，得到参数就是得到分布，即得到解析解。

另一个$p(x|y)$的分布解析解如下：$$p(x|y)=N(x|M(A^{T}L(y-b)+\Lambda \mu ),M)$$其中有：$$M=(\Lambda +A^{T}LA{)}^{-1}$$
注意到$p(x|y)\propto p(x)p(y|x)$与update对应：$$p(x)\Rightarrow p(z_t|x_1,...,x_{t-1})=N({\mu }_t^{\ast },{\Sigma }_t^{\ast })$$$$p(y|x)\Rightarrow p(x_t|z_t)=N(C{z}_t+D,R)$$$$p(x|y)\Rightarrow p(z_t|x_1,...,x_t)=N({\mu }_t,{\Sigma }_t)$$
对应着参数为：$${\mu }_t={\Sigma }_t(C^T{R}^{-1}(x_t-D)+({\Sigma }_t^{\ast }{)}^{-1}{\mu }_t^{\ast })$$其中$${\Sigma }_t=(({\Sigma }_t^{\ast }{)}^{-1}+C^T{R}^{-1}C{)}^{-1}$$
其中，${\mu }_t^{\ast }$和${\Sigma }_t^{\ast }$是$t-1$时刻的预测概率，即$p(z_t|x_1,...,x_{t-1})=N({\mu }_t^{\ast },{\Sigma }_t^{\ast })$的参数；

所以，我们按照以上解析解，一轮一轮进行滤波，即卡尔曼滤波。对于$t$轮的 update 步，从分布中取期望作为该时刻的滤波结果。卡尔曼滤波器的参数通常根据具体控制系统的建模确定。