第二十一课.粒子滤波器

粒子滤波

问题引入

卡尔曼滤波基于"高斯噪声+线性"的假设，由于高斯分布的特性，我们可以得到解析解，如果更一般化，变量之间服从任意关系，则称为粒子滤波。

粒子滤波的关注问题同样是$p(z_t|x_1,x_2,...,x_t)$的分布，回顾卡尔曼滤波器中介绍的解决方案：

• predict，利用$t-1$时刻的滤波结果估计$z_t$作为先验概率：$$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}$$
• update，在得到$t$时刻的观测变量$x_t$后，对预测的先验概率进行修正，得到$z_t$的后验概率：$$p(z_t|x_1,...,x_t)\propto p(x_t|z_t)p(z_t|x_1,...,x_{t-1})$$

现在抛弃卡尔曼滤波器中变量间的假设关系，我们不能得到解析解，为了实现滤波，我们利用蒙特卡洛方法获取数值解。

采样获取数值解

具体采样方法为重要性采样。在predict和update不断迭代的过程中，都在计算$z$的概率分布，获取到分布（获取到参数）后，我们用期望作为滤波的结果。对于数值解方法，如果$z_t$服从概率分布$p(z_t|x_1,...,x_t)$，则数值解的结果即直接得到$z_t$的期望。

首先回顾重要性采样，例如变量$z$服从分布$p(z)$，现在要获得$z$的期望。

按照连续型随机变量期望的定义：$E[z]=\int zp(z)dz$，根据大数定理，从分布$p(z)$中采样$N$个样本$z_i$，样本均值与期望近似，所以期望表示为：$$E[z]=\int zp(z)dz\approx \frac{1}{N}\sum _{i=1}^N{z}_i$$
但问题是$p(z)$本来就是未知的分布，我们难以采样得到样本$z_i$。为了解决该问题，我们引入建议分布$q(z)$，建议分布是我们已知且熟悉的分布，即得到重要性采样的期望：$$E[z]=\int zp(z)dz=\int z\frac{p(z)}{q(z)}q(z)dz\approx \frac{1}{N}\sum _{i=1}^N{z}_i\frac{p(z_i)}{q(z_i)}$$
其中，$\frac{p(z_i)}{q(z_i)}$称为$z_i$的权重，记作$w_i$，并且，现在的$z_i$不是从$p(z)$中采样得到的，而是来自$q(z)$采样。

现在回到粒子滤波问题，在$t$时刻，用数值解法计算滤波的近似结果，实际上就是求$p(z_t|x_{1:t})$中变量$z$的期望，利用重要性采样方法，引入建议分布$q(z_t|x_{1:t})$，每一轮过程中，比如在$t$时刻，我们用建议分布$q(z_t|x_{1:t})$生成$N$个样本：$z_t^1,z_t^2,...,z_t^N$；

再利用权重公式获得权重：$$w_t^i=\frac{p(z_t^i|x_{1:t})}{q(z_t^i|x_{1:t})}$$
从而计算滤波结果：$$\frac{1}{N}\sum _{i=1}^N{z}_t^i{w}_t^i$$
目前又面临一个实际问题，在以往的重要性采样中，虽然不能便利地从$p(z)$中采样，但毕竟是知道$p(z)$的解析式的，因为这样才能计算$p(z)$的概率值，对于粒子滤波问题，我们不知道$p(z_t^i|x_{1:t})$的解析式，只有当$t=1$时，$p(z_1^i|x_1)$可以由$p(z_1^i|x_1)\propto p(z_1^i)p(x_1|z_1^i)$获得。所以，我们利用递推法解决该问题。

权重递推关系推导

在$t-1$时刻，$N$个样本$z_{t-1}^1,z_{t-1}^2,z_{t-1}^3,...,z_{t-1}^N$所对应的权重为$w_{t-1}^1,w_{t-1}^2,w_{t-1}^3,...,w_{t-1}^N$；

在$t$时刻，$N$个样本$z_t^1,z_t^2,z_t^3,...,z_t^N$所对应的权重为$w_t^1,w_t^2,w_t^3,...,w_t^N$；

我们只要求出样本两个对应位置的样本$z_{t-1}^i$和$z_t^i$权重的递推关系，即$w_{t-1}^i$和$w_t^i$之间的递推关系，从$t=1$开始，一步一步采样$z_t^i$，一轮一轮迭代$w_t^i$，就实现了滤波。

下面是具体求解过程：

为了简化计算，使用概率$p(z_{1:t}|x_{1:t})$，这是一种简化运算的假设，权重的表达式变为：$$w_t=\frac{p(z_{1:t}|x_{1:t})}{q(z_{1:t}|x_{1:t})}$$
现在研究如何化简该表达式，首先对于分子：$$p(z_{1:t}|x_{1:t})=\frac{p(z_{1:t},x_{1:t})}{p(x_{1:t})}=\frac{1}{C}p(z_{1:t},x_{1:t})$$
其中，由于$p(x_{1:t})$是观测变量$x_{1:t}$的概率，所以可以记作常数$C$，然后：$$p(z_{1:t}|x_{1:t})=\frac{1}{C}p(z_{1:t},x_{1:t})=\frac{1}{C}p(x_t|z_{1:t},x_{1:t-1})p(z_{1:t},x_{1:t-1})$$$$=\frac{1}{C}p(x_t|z_{1:t},x_{1:t-1})p(z_t|z_{1:t-1},x_{1:t-1})p(z_{1:t-1},x_{1:t-1})$$$$=\frac{1}{C}p(x_t|z_{1:t},x_{1:t-1})p(z_t|z_{1:t-1},x_{1:t-1})p(z_{1:t-1}|x_{1:t-1})p(x_{1:t-1})$$
根据观测独立性假设有：$$p(x_t|z_{1:t},x_{1:t-1})=p(x_t|z_t)$$
根据齐次马尔科夫假设有：$$p(z_t|z_{1:t-1},x_{1:t-1})=p(z_t|z_{t-1})$$
同时，$p(x_{1:t-1})$是一组观测变量的概率，记作常数$D$，最终化简为：$$p(z_{1:t}|x_{1:t})=\frac{D}{C}p(x_t|z_t)p(z_t|z_{t-1})p(z_{1:t-1}|x_{1:t-1})$$
再研究分母的化简，先按照条件概率公式转化一下：$$q(z_{1:t}|x_{1:t})=q(z_t|z_{1:t-1},x_{1:t})q(z_{1:t-1}|x_{1:t})$$
此处有一个技巧，时刻$t$以前的隐变量取值显然不受时刻$t$的观测变量取值的影响，于是有：$$q(z_{1:t-1}|x_{1:t})=q(z_{1:t-1}|x_{1:t-1})$$
所以：$$q(z_{1:t}|x_{1:t})=q(z_t|z_{1:t-1},x_{1:t})q(z_{1:t-1}|x_{1:t-1})$$
此时，权重的表达式为：$$w_t=\frac{p(z_{1:t}|x_{1:t})}{q(z_{1:t}|x_{1:t})}\propto \frac{p(x_t|z_t)p(z_t|z_{t-1})p(z_{1:t-1}|x_{1:t-1})}{q(z_t|z_{1:t-1},x_{1:t})q(z_{1:t-1}|x_{1:t-1})}$$
即：$$w_t\propto \frac{p(x_t|z_t)p(z_t|z_{t-1})}{q(z_t|z_{1:t-1},x_{1:t})}{w}_{t-1}$$
$q$分布是可以任意选择的提议分布，我们不妨让$q$分布与$p$分布相同，则有：$$q(z_t|z_{1:t-1},x_{1:t})=p(z_t|z_{1:t-1},x_{1:t})=p(z_t|z_{t-1})$$
进一步化简为：$$w_t\propto p(x_t|z_t)w_{t-1}$$
发现$p(x_t|z_t)$反映的是$t$时刻隐变量$z_t$到观测变量$x_t$的转移概率，这个是模型的已知内容，因此，$t$时刻和$t-1$时刻的权重递推关系就可以方便地计算出来。

粒子滤波过程

粒子滤波采样流程

在上面内容中，已经得到了$t-1$时刻和$t$时刻的权重递推关系：$w_t=p(x_t|z_t)w_{t-1}$，那么粒子滤波中的采样过程具体描述如下，这是一个迭代的过程：

前提：在$t-1$时刻采样过程已完成；

在$t$时刻，我们采样$N$个样本$z_t^i$

$fori=1,2,...,N:$
$z_t^i\sim q(z_t|z_{t-1}^i,x_{1:t})$
$w_t^i=\frac{p(x_t|z_t^i)p(z_t^i|z_{t-1}^i)}{q(z_t^i|z_{1:t-1}^i,x_{1:t})}{w}_{t-1}^i$
$end:$对$w_t^i$进行归一化，使得${\sum }_{i=1}^N{w}_t^i=1$

由于我们让建议分布$q=p$，则有：$$q(z_t^i|z_{1:t-1}^i,x_{1:t})=p(z_t^i|z_{1:t-1}^i,x_{1:t})=p(z_t^i|z_{t-1}^i)$$
此时，采样和权重迭代的过程简化为：

$fori=1,2,...,N:$
$z_t^i\sim p(z_t|z_{t-1}^i)$
$w_t^i=p(x_t|z_t^i)w_{t-1}^i$
$end:$对$w_t^i$进行归一化，使得${\sum }_{i=1}^N{w}_t^i=1$

实际上在$t$时刻，我们需要从分布$p(z_t|z_{t-1}^i)$中采样$N$个样本$z_t^i$，同时我们在上一步$t-1$时刻曾经也得到了一轮采样样本$z_{t-1}^1,...,z_{t-1}^N$，其中每一个采样样本$z_{t-1}^i$对应的权重就是$w_{t-1}^i$，那么在时刻$t$的采样中，我们对每一个样本$z_t^i$，仅通过$p(x_t|z_t^i)w_{t-1}^i$的迭代计算，我们就能获得样本$z_t^i$的权重$w_t^i$。

对于初始时刻$w_1^i$的获取，当$t=1$时，代入$w_1^i=p(x_1|z_1^i)w_0^i$，令$w_0^i=1$，并令$t=1$时，$z_t^i\sim p(z_t|z_{t-1}^i)$中的$p(z_t|z_{t-1}^i)$为初始概率分布$p(z_1)$；

所以粒子滤波过程为：

$t=1:$
$fori=1,2,...,N:$
$z_1^i\sim p(z_1)$
$w_1^i=p(x_1|z_1^i)$
$end:$对$w_1^i$进行归一化，使得${\sum }_{i=1}^N{w}_1^i=1$

$t\ge 2:$
$fori=1,2,...,N:$
$z_t^i\sim p(z_t|z_{t-1}^i)$
$w_t^i=p(x_t|z_t^i)w_{t-1}^i$
$end:$对$w_t^i$进行归一化，使得${\sum }_{i=1}^N{w}_t^i=1$

观测变量与隐变量的关系分析

初始概率$p(z_1)$，状态转移概率$p(z_t|z_{t-1})$和发射概率$p(x_t|z_t)$，这三种概率在隐马尔科夫模型，卡尔曼滤波器和粒子滤波器中都有，但表现形式不同，回顾一下：

对于HMM，初始概率$p(z_1)$取自概率向量$\pi$，状态转移概率$p(z_t|z_{t-1})$和发射概率$p(x_t|z_t)$取自状态转移概率矩阵$A$和发射概率矩阵$B$中的对应项；

对于卡尔曼滤波器，三个概率均服从高斯分布：$$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)$$
粒子滤波器没有约束，三个概率都可以定义为任意函数：$$p(z_t|z_{t-1})=f(z_{t-1})$$$$p(x_t|z_t)=g(z_t)$$$$p(z_1)=f_0(z_1)$$

滤波中的分布变化

比较卡尔曼滤波和粒子滤波，比较从时间$t-1$时刻到$t$时刻的迭代。对于卡尔曼滤波，关于隐变量$z_t$的估计，实际是一个高斯分布：

我们知道，在$t-1$时刻，通过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}$$
这本质上是先获得$z_t$的先验高斯分布，然后在$t$时刻进行update步，利用观察到$t$时刻的观测变量$x_t$，对先验分布进行修正，得到后验分布：$$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$时刻的predict步到$t$时刻的update步，就是从一个高斯分布变化到另一个高斯分布的过程，高斯分布仅由均值和方差决定，因此前后需要发生改变的就是均值和方差，如下图：

而粒子滤波，因为没有高斯分布的解析形式，我们没有办法在每一轮迭代中获得其解析式，所以采用蒙特卡洛方法采样。

即在$t-1$时刻，我们获得$N$个采样的样本：$z_{t-1}^1,z_{t-1}^2,...,z_{t-1}^N$，对应了各自不同的权重$w_{t-1}^1,w_{t-1}^2,...,w_{t-1}^N$，这实际是一个分布列，这个分布列近似表示了$z_t$的后验分布，而我们正是通过这个分布列求得期望，作为这一轮滤波的估计值。到了$t$时刻，我们通过$z_t^i\sim p(z_t|z_{t-1}^i)$和$w_t^i=w_{t-1}^{i}p(x_t|z_t^i)$来获取$t$时刻的分布列和各自对应的权重；

如果我们明确了概念，即离散型随机变量的分布列就是连续型随机变量概率分布的一种近似，则粒子滤波从$t-1$时刻到$t$时刻也是从一个分布到另一个分布的变化过程，这里变化的是所有随机变量$z_t^i$的取值和对应的权重，而分布列的两个要素就是随机变量的取值和其对应的权重。

从图中可以看出，从$t-1$时刻到$t$时刻，虚线箭头表示采样过程$z_t^i\sim p(z_t|z_{t-1}^i)$和权重更新$w_t^i=w_{t-1}^{i}p(x_t|z_t^i)$：每一个采样点$z_i$的取值发生了变化，权重也发生了变化。

粒子滤波的权重衰减问题

粒子滤波还存在一个问题，即权重衰减问题，一般粒子滤波在经过迭代几轮后，会出现以下情况，在其分布列中，某一个取值的权重很大，其他都很小：

也就是当某一个采样点的权重很大，而其他采样点的权值很小的情况下，后续迭代都会保持这个趋势，而导致采样点的权值出现衰退，首先这种情况是没有意义的，因为一枝独秀的情况不能很好反映整体分布。

在考虑如何解决这种问题前，先来想想为什么，在前面的粒子滤波算法中，$N$个样本进行迭代，每个样本都被处理一次，这一次处理通过$z_t^i\sim p(z_t|z_{t-1}^i)$确定$t$时刻采样点的值，通过$w_t^i=w_{t-1}^{i}p(x_t|z_t^i)$确定权重。

因此，那个权重极大的采样点，下一轮权重依然很大，而权重很小的（近似0），总体上看，下一轮权重依旧接近0，即：在$t-1$时刻到$t$时刻，$N$个样本，每个采样点只处理一次，权重随之一起转移了。

重采样解决权重衰减问题

为了解决权重衰减问题，应该让采样点的值转移过程与权值脱钩，但又要反映各个采样点的权重属性，这个"既要又要"可以由重采样实现。

考虑一种极端情况，比如$z_{t-1}^j$的权重是0.85，$z_{t-1}^k$的权重是0.1，$z_{t-1}^m$的权重是0.05，其他采样点的权重都为0（实际情况应该是每个采样点都有权重，此处只是为了简化描述）。

在$t-1$时刻到$t$时刻的采样过程中，我们还是进行$N$次采样，但在每次采样中，我们让其有0.85的概率去处理$z_{t-1}^j$，0.1的概率去处理$z_{t-1}^k$，0.05的概率去处理$z_{t-1}^m$，假设$N=100$，那么按照大数定理，$z_{t-1}^j$的处理次数就为85次，这样它同样照顾到$t-1$时刻各个采样点的权重属性，其好处体现在：每次都是依照$z_t^j\sim p(z_t|z_{t-1}^j)$的概率分布进行采样确定下一轮的采样值，而85次采样的结果不会再集中于一点。

对于权重的处理，在$t-1$时刻，$N$次采样过程中，我们每一次所选中的采样点，他的权重都是$\frac{1}{N}$，在这个例子中，$N=100$，每一次采样的权重就是$0.01$，但是每次采样又0.85的概率处理$z_{t-1}^j$，依照大数定理，$z_{t-1}^j$的处理次数就为85次，仍然包含了原始采样点的权重信息，那么迭代到下一轮$w_t^j=\frac{1}{N}p(x_t|z_t^j)$，决定了$t$时刻这个采样点的权重；

我们依照$z_t^i\sim p(z_t|z_{t-1}^i)$概率取出$t$时刻的样本$z_t^i$，如果这个样本对于生成观测变量$x_t$是合适的，则$p(x_t|z_t^i)$取值会更大，那么这个样本在$t$时刻所对应的权重$w_t^i=\frac{1}{N}p(x_t|z_t^i)$就更大。

因此在前面基本的粒子滤波算法上，我们在$t-1$轮生成新的$N$个样本点$z_{t-1}^i$以及它们的权重$w_{t-1}^i$后，在向$t$时刻迭代的过程中，目标还是采样$N$个样本，首先更新$z_{t-1}^i$的权重，将其变成$\frac{1}{N}$，但是在每次采样的过程中，我们依照更新前的权重$w_{t-1}^1,w_{t-1}^2,...,w_{t-1}^N$，依概率选择一个采样点进行处理，利用$z_t^i\sim p(z_t|z_{t-1}^i)$获得下一轮$t$时刻的新采样点，并更新权重$w_t^i=\frac{1}{N}p(x_t|z_t^i)$得到$t$时刻第$i$个采样点的权重。