初探粒子滤波


粒子滤波算法的核心思想就是通过利用一系列随机样本的加权和表示后验概率密度

数学模型

我们建立了一个状态的空间模型。
用$x_i$ 表示运动系统当前的状态，$y_i$表示测量到的数据,$v_k$表示当前系统的过程噪音，$n_i$表示测量噪声.
再分别用函数f和h表示出状态方程和测量方程:
$$x_k = f_k(x_{k-1},v_{k-1}) \tag{1} $$
$$y_k = h_k(x_k,n_k) \tag{2}$$

贝叶斯滤波

在跟踪注册领域，上述数学模型可以表述为：已知$t$时刻的状态(通常用旋转矩阵和平移矩阵表示)，估计出$t+1$的状态.

从贝叶斯的观点来看，状态估计问题就是根据目前知道的信息(先验知识)推导出后验概率密度$P(x_k | y_{1:k})$的值。

贝叶斯滤波原理的实质是试图用所有已知信息来构造系统状态变量的后验概率密度, 即用系统模型预测状态的先验概率密度, 再使用最近的量测值进行修正, 得到后验概率密度。这样通过量测数据$y_{1:k}$, 来递推计算状态$x_k$取不同值时的置信度$P(x_{1:k}|y_{1:k})$, 由此获得状态的最优估计[6].

贝叶斯滤波通过两个过程来计算这个值,一个过程是预测过程，一个过程是更新过程。

  • 预测过程
    根据状态方程(1)预测出下一状态的概率密度(又被称为先验概率密度).也就是由上一刻的概率密度$P(x_{k-1},y_{1:k-1})$得到$P(x_k,y_{1:k-1})$
    预测公式：
    $$
    P(x_k, x_{k-1}|y_{1:k-1})
    = P(x_k | x_{k-1}, y_{1:k-1})P(x_{k-1}, x_{k-1}|y_{1:k-1})
    = P(x_k | x_{k-1})P(x_{k-1}, x_{k-1}|y_{1:k-1}
    )
    $$
    • 联合分布概率公式
    • 一般都先假设系统的状态转移服从一阶马尔科夫模型，即当前时刻的状态$x_k$只与上一个时刻的状态$x_{k-1}$有关。

对上式两边对${\rm d} x_{k-1}$积分有:

$$
P(x_k,y_{1:k-1})
= \int P(x_k | x_{k-1})P(x_{k-1}, x_{k-1}|y_{1:k-1}) {\rm d}x_{k-1}
\tag{3}
$$

  • 更新过程
    利用测量值$y_k$对预测过程计算出来的先验概率密度进行修正,也就是根据$P(x_k, y_{1:k-1})$得到后验概率$P(x_k |y_{1:k})$
    这个后验概率就是我们真正想要的一个东西。
    $$
    P(x_k,y_{1:k})
    = \frac {P(y_k|x_k, y_{1:k-1})P(x_k|y_{1:k-1})}
    {P(y_k|y_{1:k-1})}
    = \frac {P(y_k|x_k)P(x_k|y_{1:k-1})}
    {P(y_k|y_{1:k-1})}
    \tag{4}
    $$
    • 归一常数:$
      P(y_k|y_{1:k-1}) = \int {P(y_k|x_k)P(x_k|y_{1:k-1})}{\rm d}x_k
      $
    • $y_k$ 只与$x_k$有关

我们知道了k时刻的后验概率密度$P(y_k|y_{1:k})$,在重复预测和更新过程就可以递归的求解出k+1时刻的后验概率密度$P(y_{k+1}|y_{1:k+1})$

目标状态的最优估计值可以通过后验密度函数进行计算，通常我们根据最小均方误差准则，将具有极大后验概率密度的状态或条件均值作为系统状态的估计值[2],即：
$$
\hat{x_k}^{MMSE} = E(f(x_k)|Y_k) = \int f(x_k) P(x_k|Y_k) {\rm d}x_k
\tag{5}
$$

  • $Y_k 等同于y_{1:k}$

贝叶斯以递推的方式给出了后验滤波概率密度函数的最优解,然而，这种循环传播最多是一种概念上的解决方案[2]。现有的非线性滤波器多采用近似的计算方法解决积分的问题，以此来获取估计的次优解。

蒙特卡洛模拟

蒙特卡洛模拟提供了一种将积分问题转变成有限样本的求和问题。这使得我们能够避开(3)式中复杂的积分计算。
假设我们能够从后验概率密度$P(x_k, Y_k)$中抽取N个独立同分布的随机样本$x_k^{i}$ 则有:
$$后验概率P(x_k, Y_k) \approx \frac {1} {N} \sum_{i=1}^N \delta{(x_k - x_k^i)}
\tag{6}
$$

  • $\delta(.)$:狄拉克函数

至此，我们找到了一种方法使得，我们引入一个已知的密度函数，就可以利用式子(6)近似的逼近真实的后验概率密度函数(当我们采取的粒子数量足够大时)。由公式(5)和公式(6)有下面的公式(7):
$$
E(f(x_k)|Y_k) = \int f(x_k) P(x_k|Y_k) {\rm d}x_k
= \frac {1} {N} \sum_{i=1}^N f(x_k^i)
\tag{7}
$$

  • $x_k^i此时表示抽取出来的样本$
  • 函数f表示的是每个粒子的状态函数

然而在实际的计算中，我们通常无法从后验概率分布中抽取样本，这个时候重要性采样法为我们提供了另一种逼近后验概率密度的方法。

重要性采样

重要性采样提供了一种方法,使得我们能够在无法在原概率分布抽样时，引入了一个已知，容易采样的重要性概率密度函数$q(x_k, Y_k)$, 从中生成采样粒子，利用这些粒子的加权和来逼近后验概率密度(P(x_k, Y_k))。具体来说：重要性采样为上面的抽取的随机样本$x_k^i$分配一个重要性权重$w_k^i$
$$
P(x_k, Y_k) \approx \frac {1} {N} \sum_{i=1}^N w_k^i \delta{(x_k - x_k^i)} \tag{8}
$$

  • $w_k^i ∝
    \frac {P(x_k^i|Y_k)}{q(x_k^i|Y_k)}$
  • $\sum_i{w_k^i} = 1$
  • 具体推导可参考[3][4]

当采样的粒子数很大时，式(8)便可近似的逼近真实的后验概率密度函数，此时式(7)可表达为：
$$
E(f(x_k)|Y_k) = \frac {1} {N} \sum_{i=1}^N f(x_k^i)w_k^i
= \frac {1} {N} \sum_{i=1}^N f(x_k^i) \frac {P(x_k^i |Y_k)}{q(x_k^i|Y_k)}
\tag{9}
$$

此时的$x_k^i表示从密度函数q(x_k,Y_k)分布中抽取的样本$

序贯重要性采样(SIS)算法

序贯重要性抽样(SIS)算法是一种蒙特卡洛(MC)方法，它是过去几十年发展起来的大多数序贯重要性抽样滤波器的基础.[1]它将统计学中的序贯分析方法应用到蒙特卡洛中，从而实现后验滤波概率密度的递推估计。[2]

重要性采样的每个粒子的权重都采用的直接计算的方法，效率低，因为每增加一个采样$p( x(k) |y(1:k))$都得重新计算，并且还不好计算这个式子。所以求权重时能否避开计算$p(x_k|y_{1:k})$？而最佳的形式是能够以递推的方式去计算权重，这就是所谓的序贯重要性采样（SIS），粒子滤波的原型。[3]

我们对我们假设的重要性概率密度函数$q(x_{0:k},y_{1:k})$进行因式分解为:
$$
q(x_{0:k},y_{1:k})=q(x_k |x_{0:k-1},y_{1:k})q(x_{0:k-1}|y_{1:k-1})
$$
经过推导有:
$$
w_k^i∝
w_{k-1}^i
\frac {P(y_k|x_k^i)P(x_k^i|x_{k-1}^i)}
{q(x_k^i|x_{0:k-1}^i,y_{1:k})}
$$

详细推导过程见[1]
补一句:”计算机的尽头是数学,数学的尽头是哲学”.

一般情况下有$q(x_k|x_{0:k-1},y_{1:k})=q(x_k|x_{k-1},y_k)($这使得我们不在需要保存${x_0},….,x_{k-1}$),则由上式有:
$$
w_k^i∝
w_{k-1}^i
\frac {P(y_k|x_k^i)P(x_k^i|x_{k-1}^i)}
{q(x_k^i|x_{k-1}^i,y_k)}
\tag{10}
$$

这时重要性密度只依赖于$x_{k-1}和y_k$

$$
P(x_k, Y_k) \approx \sum_{i=1}^{N_s} w_k^i \delta{(x_k - x_k^i)} \tag{11}
(N_s → \infty)
$$
粒子滤波算法的核心思想就是通过利用一系列随机样本的加权和表示后验概率密度。
序贯重要性采样算法从重要性概率密度函数中生成采样粒子，并随着测量值的依次到来递推求得相应的权值，最终以粒子加权和的形式来描述后验滤波概率密度，进而得到状态估计。

SIS伪代码如下:
SIS.jpg

  1. $<!–swig0–>_{i=1}^N,Y_k)$

  2. $For i =1:N_s$
    $ 抽取粒子x_k^i 属于q(x_k|x_{k-1}^i, y_k)$
    $通过式(10)更新粒子权重w_k^i$

  3. End for

参考