【SLAM基础入门】贝叶斯滤波、卡尔曼滤波、粒子滤波笔记（4）

基于老王的BILIBILI视频

第七部分：粒子滤波PF

1. 应用：处理非线性函数f和h，用于电池电量估算，视频跟踪，封闭环境导航gmapping。

2. 大数定律：设X为随机变量，E(X)存在，对X采样（做n次随机试验），试验结果记为$x_1,x_2,...,x_n$，则有$\underset{n\to \infty }{\lim }P(|\frac{1}{n}\sum _i{x}_i-E(X)|<ϵ)=1$

   当n足够大时，均值收敛于期望，$\frac{1}{n}\sum _i{x}_i\approx E(X)={\int }_{-\infty }^{\infty }xf(x)dx$。

   狄拉克函数：${\int }_c^{d}f(x)\delta (x-a)=f(a),a\in [c,d]$，将一个函数收缩成一个点。

   $\frac{1}{n}\sum _i{x}_i=\frac{1}{n}(x_1+x_2+...+x_n)$，$x_1,x_2,...,x_n$为数，是可以排序的。则设$-\infty <x_1<a_1<x_2<a_2<x_3<...<x_n<+\infty$

   即$x_1={\int }_{-\infty }^{a_1}x\delta (x-x_1)dx$，$x_n={\int }_{a_{n-1}}^{+\infty }x\delta (x-x_n)dx$

   $\Rightarrow \frac{1}{n}\sum _i{x}_i=\frac{1}{n}({\int }_{-\infty }^{a_1}+{\int }_{a_1}^{a_2}+...+{\int }_{a_1}^{+\infty })=\frac{1}{n}{\int }_{-\infty }^{+\infty }x[\sum _i\delta (x-x_i)]dx={\int }_{-\infty }^{\infty }xf(x)dx$，其中$f(x)$是X的PDF。

   由大数定律，当$n\to \infty$，$f(x)\approx \frac{1}{n}\sum _i\delta (x-x_i)$，狄拉克函数是非常容易积分的。即大数定律暗示了概率密度可以用一堆带权重的粒子来近似（采样）。

   

   缺点：需要大量粒子。

   如何用少量粒子表示pdf

• 在概率密度大的地方升高得快一点：引入权重，让少量粒子有较高的权重。权重$\frac{1}{n}\to w_i$

  $f(x)\approx \sum _i\frac{1}{n}\delta (x-x_i)\Rightarrow \sum _i{w}_i\delta (x-x_i)$，$\sum _i{w}_i=1$

• 粒子的数量、位置和权重完全决定了 pdf，$x_1,...,x_n$是样本，权重$w_i=1/n$

  • 位置影响

+ 权重影响：原则——pdf大的，$w_i$高

• 处理方法

  • 比例分配权重：$w_i=\frac{f(x_i)}{f(x_1)+f(x_2)+f(x_3)}$，满足归一化$\sum w_i=1$
  • 大粒子数：也可以$w_i=1/n$，但是n要足够大（50个以上）
  • 综合：大粒子数，且$w_i=\frac{f(x_i)}{\sum _{i}f(x_i)}$

3. 贝叶斯滤波中的粒子滤波：

   设$x_0$的pdf为$f_0(x)$，在$f_0(x)$中采了n个样本。

   如何采样：假设$X_0$是一个正态分布，采集到样本$x_0^{(1)},x_0^{(2)},...,x_0^{(n)}$，设$f_0(x)=\sum _i{w}_0^{(i)}\delta [x-x_0^{(i)}]$，$w_0^{(i)}$可以为$1/n$也可以按比例分配。

   贝叶斯滤波：

   • $f_0(x)=\sum _i{w}_i\delta [x-x_0^{(i)}]$

   • $f_1^{-}(x)={\int }_{-\infty }^{+\infty }{f}_Q[x-f(v)]f_0(v)dv=\sum _i{w}_i{f}_Q[x-f(x_0^{(i)})]$，概率分布由一堆粒子近似，多个正态分布按照权重进行叠加。

   • $f_1^{+}(x)=\eta f_R[y-h(x)]f_1^{-}(x)$

     到这一步已无狄拉克函数无法无穷积分，且很难采样

     解决办法：

     • 通过$f_1^{-}(x)$生成一堆粒子，理论上$f_1^{-}(x)=\sum _i{w}_i{f}_Q[x-f(x_0^{(i)})]$也可以采样，也可以计算出新的权重w。

     • 假设$Q\sim N(0,Q)$则，$f_Q[x-f(x_0^{(i)})]\sim N(f(x_0^{(i)}),Q)$ ，则下面要对这一堆概率密度进行采样（找到独立事件）

     • 对预测$f_1^{-}(x)$做傅里叶变换，$N(f(x_o^{(i)}),Q)\stackrel{F.T}{\to }{e}^{if(x_0^{(i)})t-Q{t}^2/2}=e^{if(x_0^{(i)})t}{e}^{-Q{t}^2/2}$

     • 对两个拆分出来的独立事件做傅里叶逆变换

       • $e^{if(x_0^{(i)})t}\stackrel{I.F.T}{\to }\delta (x-f(x_0^{(i)}))$，是必然事件$X=f(x_0^{(i)})$时的概率密度
       • $e^{-Q{t}^2/2}\stackrel{I.F.T}{\to }N(0,Q)$，是Q的概率密度

     • 定理：若X的pdf为f，Y的pdf为g，X,Y独立，则$Z=X+Y$的$pdf=f\ast g$，设Z的概率密度为h，则$h=f\ast g$（卷积），设$G$为FT，则$G(h)=G(f)G(g)$

     • 设事件X的pdf为$f_X=\delta (x-f(x_0^{(i)}))$，Y的pdf为$f_Y=N(0,Q)$，X，Y独立，$G(f_A)=G(f_X)G(f_Y)\Rightarrow A=X+Y$。想对事件A做随机试验，则只需要对事件X和事件Y分别做随机试验然后把它们加起来。

       分析两个拆分出来的事件，$X$为必然事件，$Y\sim N(0,Q)$，且X，Y独立

     • 对$f_1^{-}(x)$生成粒子：$f_1^{-}(x)=\sum _i{w}_i{f}_Q[x-f(x_0^{(i)})]$，对每一个$f_Q[x-f(x_0^{(i)})]$可以看做是一个必然事件$X=f(x_0^{(i)})$与一个随机数$Y\sim N(0,Q)$的叠加。

       则可生成粒子$X_1^{-(1)},X_1^{-(2)},...,X_1^{-(n)}$，$X_1^{-(i)}=f(x_0^{(i)})+v$，其中$v\sim N(0,Q)$的一个随机数。

       例子：$X_1=2X_0+Q,Q\sim N(0,1)$，设$X_0\sim N(0,1)$，样本$x_0^{-(1)}=0,x_0^{-(2)}=0.1,x_0^{-(3)}=-0.1$

       ​ 则 $x_1^{-(1)}=2\cdot 0+0.12,x_1^{-(2)}=2\cdot 0.1+0.08,x_1^{-(2)}=2\cdot (-0.1)+0.3$

     • 综上$f_1^{-}(x)=\sum _i{w}_i{f}_Q[x-f(x_0^{(i)})]$，对每一个$f_Q[x-f(x_0^{(i)})]$生成一个粒子即可。此时$x_1^{-(i)}=f(x_0^{(i)})+Q$，本质是改变了粒子的位置，并未改变粒子的权重。
       

4. 粒子滤波算法推导

   • 设置初值$X\sim N(\mu ,{\sigma }^2)$

   • 生成$X_0$的采样样本$x_0^{(1)},....,x_0^{(n)}$，生成$X_0$的样本对应权重$w_0^{(i)}$，可以1/n，也可以按比例分配$\frac{f(x_i)}{\sum _{i}f(x_i)}$，其中$f(x)$为$X_0$的pdf

   • 预测步：生成$X_1^{-}$的样本，$x_1^{(i)}=f(x_0^{(i)})+v$，$v$为一个服从$N(0,Q)$的正态分布的随机数。

     $f^{-}(x)=\sum _i{w}_0^{(i)}\delta (x-x_1^{-(i)})$，预测步改变了粒子的位置，并未改变粒子的权重

   • 更新步：$f_1^{+}(x)=\eta f_R[y-h(x)]f_1^{-}(x)=\sum _{i=1}^n\eta f_R[y-h(x)]w_0^{(i)}\delta (x-x_1^{-(i)})$

   

   $\Rightarrow f_1^{+}(x)=\sum _{i=1}^n\eta f_R[y-h(x_1^{-(i)})]w_0^{(i)}\delta (x-x_1^{-(i)})$

   设$w_1^{(i)}=f_R[y-h(x_1^{-(i)})]w_0^{(i)}$，则

   $\Rightarrow f_1^{+}(x)=\eta \sum _{i=1}^n{w}_1^{(i)}\delta (x-x_1^{-(i)})$ ，其中$\eta =(\sum _i{w}_1^{(i)}{)}^{-1}$起到归一化作用

   更新步并未改变粒子位置，但是改变了粒子权重

   • 对后验概率分布$f_1^{+}(x)=\sum _{i=1}^n{w}_1^{(i)}\delta (x-x_i)$估计期望：${\hat{X}}_k^{+}=E(X_1^{+})={\int }_{-\infty }^{\infty }x\sum _{i=1}^n{w}_1^{(i)}\delta (x-x_i)dx=\sum _{i=1}^n{w}_1^{(i)}{x}_i$

     方差$D(X_1^{+})=E(X_1^{2+})-[E(X_1^{+}){]}^2=\sum _{i=1}^n(w_1^{(i)}{x}_i^2)-({\hat{X}}_k^{+}{)}^2$

5. 粒子滤波算法流程

   • 设置初值$X\sim N(\mu ,{\sigma }^2)$
   • 生成$X_0$的采样样本$x_0^{(1)},....,x_0^{(n)}$，生成$X_0$的样本对应权重$w_0^{(i)}$，可以1/n，也可以按比例分配$\frac{f(x_i)}{\sum _{i}f(x_i)}$，其中$f(x)$为$X_0$的pdf
   • 预测步：生成$X_1^{-}$的样本，$x_1^{(i)}=f(x_0^{(i)})+v$，$v$为一个服从$N(0,Q)$的正态分布的随机数。更新粒子位置。
   • 更新步：设观测值$y_1$，生成$w_1^{(i)}=f_R[y_1-h(x_1^{-(i)})]w_0^{(i)}$。更新粒子权重。
   • 将$w_1^{(i)}$归一化，$w_1^{(i)}=\frac{w_1^{(i)}}{\sum _i{w}_1^{(i)}}$
   • 此时有新的粒子$X_1^{(i)}$和新的权重$w_1^{(i)}$
   • 再由预测步生成$X_2^{(i)}=f(x_1^{(i)})+v$
   • 再由更新步生成$w_2^{(i)}=f_R[y_2-h(x_2^{-(i)})]w_0^{(i)}$，归一化$w_2^{(i)}=\frac{w_2^{(i)}}{\sum _i{w}_2^{(i)}}$
   • $.....$

6. 重采样：为了解决粒子退化问题，针对下一步更新失效的问题，但必然导致粒子多样性丧失，且减慢粒子滤波的速度。重采样判据$N=\frac{1}{\sum w_i^2}$。

   • 粒子退化问题：只有少数粒子具有较高的权重，大量粒子权重极低。$\to$下一步更新失效

     • 原因1：粒子数太多

     • 原因2：$w_k^{(i)}=f_R[y_k-h(x_k^{-(i)})]w_{k-1}^{(i)}$，$f_R[y_k-h(x_k^{(i)})]=(2\pi R{)}^{-1/2}{e}^{-\frac{[y_k-h(x_k^{(i)}){]}^2}{2R}}$是$e^{-\alpha x^2}$型

       

   • 例子退化的坏处：$f_k^{+}=\delta (x-x_2)$，除了$w_k^{(2)}=1$，其余权重都是0。进行预测步$x_{k+1}^{(i)}=f(x_k^{(i)})+v_k\surd$；进行更新步$w_k^{(2)}=f_R[y_k-h(x_k^{-(2)})]w_{k-1}^{(2)}$其余权重都还是0；归一化$w_{k+1}^{(2)}=\frac{w_{k+1}^{(2)}}{\sum _i{w}_{k+1}^{(i)}}=1$，权重没有更新，失去了权重更新的作用。

• 重采样的流程：

  • 按概率进行复制和淘汰，权重高的粒子更有可能被多次复制，从而保证整个粒子数不变。

  • 复制后把所有粒子的权重设为$\frac{1}{n}$

• 例子：

  • $x_1:w_1=0.1,x_2:w_2=0.1,x_3:w_3=0.7,x_4:w_4=0.1$，在[0,1]上按$w_i$的大小生成区间，权重越大区间越长

    

    每个区间为$(0,w_1)(w_1,w_1+w_2),...,(\sum _{i-1}{w}_{i-1},\sum _i{w}_i)$，即$(0,0.1)(0.1,0.2)(0.2,0.9)(0.9,1)$

  • 随机生成随机数a， $a\sim U(0,1)$

  • 看a落在哪个区间，就把该区间对应粒子进行复制。若a取4次，$x_1$复制一次，$x_3$复制三次，则重采样结果是$x_1,x_3,x_3,x_3$，即按概率进行复制。

  • 所有粒子权重设为$\frac{1}{n}$

  • 重采样代码：

    Xold=[x1 x2 x3 x4];
    Wold=[w1 w2 w3 w4];
    for i=1:4
        a = unifrnd(0,1);
        C[4] = (w1,w1+w2,w1+w2+w3,1);
        for j = 1:4
            if(a<C[j])
                Xnew[i]=Xold[j];
                break;
             end
        end
    end
    

7. [粒子滤波算法代码实现](

   注意：滤波之前一定要写成状态方程与观测方程的形式$x_k=f(x_{k-1})$，大多数情况下不能直接得到$x_k$和$x_{k-1}$的关系，而是$x=f(t)$，要用各种模型（改进欧拉、龙格库塔、泰勒展开等）将$x=f(t)$转化为**$x_k=f(x_{k-1})$**。

8. 如何在复杂pdf上采样

   采样粒子的特点：概率密度大的地方粒子密度大，概率密度小的地方粒子密度小。

   采样方法：高pdf 的地方粒子有更大的概率被保留，低pdf的地方粒子有更大的概率被去掉。做减法，结果粒子数不可控。

均匀分布$\to$任意分布

• 均匀分布生成粒子

• 取直线M，使$M\ge f(x)$

• 对每一个粒子$x_1,,...,x_n$做判断：生成随机数$a\sim U(0,M)$，看a落在哪个区间，若$a\in (0,f(x_i))$则$x_i$保留，否则去掉。

  

任意分布$\to$均匀分布

• 设$g(x)$为正态分布

• 在g(x)采样

• 找到一个常数M，使得$Mg(x)\ge f(x)$

• 对于每一个$x_i$，生成一个随机数$a\sim U(0,Mg(x_i))$，看a落在哪个区间，若$a\in (0,f(x_i))$保留，反之拒绝。

接受-拒绝采样法：设待采样分布$f(x)$，容易采样分布$g(x)$（提议分布proposal distribute)。做加法，结果粒子数可控。

• 找到M，使得$Mg(x)\ge f(x)$
• 在$g(x)$采样一个粒子$x_1$
• 对于每一个$x_i$，生成一个随机数$a\sim U(0,Mg(x_1))$，看a落在哪个区间，若$a\in (0,f(x_1))$保留，反之拒绝。
• 在$g(x)$采样一个粒子$x_2$
• $....$

提议分布尽可能要与$f(x)$位置、形状贴近，形状越相似，拒绝率越低。

9. 改写预测方程：已知$X=f(t)$，如何得到高精度的$X_k=F(X_{k-1})$

   核心方程：$\{\begin{array}{l}\begin{array}{rl}{x}_k& =f(x_{k-1})+Q_k预测难写\\ z_{k,j}& =h(x_k)+R_k观测好写\end{array}\end{array}$

   解决方式：$x=f(t)\stackrel{discrete}{\to }{x}_k=f(t_k)\Rightarrow x_k=F(x_{k-1},t)$，如
   $\left[\begin{array}{c}{X}_k\\ {\dot{X}}_k\\ {\ddot{X}}_k\end{array}\right]=\left[\begin{array}{ccc}1& dt& d{t}^2/2\\ 0& 1& dt\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_{k-1}\\ {\dot{X}}_{k-1}\\ {\ddot{X}}_{k-1}\end{array}\right]+\left[\begin{array}{c}{Q}_{k11}\\ Q_{k2}\\ Q_{k3}{X}_k\end{array}\right]$ ，但是改变了维数，计算了许多不必要的量，计算缓慢。

   思路：转换为常微分方程数值解法 $x=f(t)\Rightarrow \dot{x}=F(x,t)$，使用欧拉法、龙格库塔法，在不改变维数的前提下提高精度。

   • 已知$\frac{dx}{dt}+p(t)x=0\Rightarrow x=e^{-\int p(t)dt+lnC}$，想用$\frac{dx}{dt}+p(t)x=0\Leftarrow x=e^{-\int p(t)dt+lnC}$，来得到$\dot{x}=F(x,t)$
   • $x=f(t)=e^{|lnf(t)|}$
   • 对比得到$p(t)=-\frac{f^{\prime }(t)}{f(t)}$
   • 即得到$x=f(t)\Rightarrow \dot{x}-\frac{f^{\prime }(t)}{f(t)}x=0\Rightarrow \dot{x}=F(x,t)=\frac{f^{\prime }(t)}{f(t)}x$

   举例：设$x=f(t)=e^{t^2}$，则$f^{\prime }(t)=2t{e}^{t^2}$，因此$F(x,t)=\frac{2t{e}^{t^2}}{e^{t^2}}x=2tx\Rightarrow \dot{x}=2tx$。

   • 欧拉法$x_k=x_{k-1}+2t_{k-1}{x}_{k-1}dt$
   • 改进欧拉法$x_k=x_{k-1}+dt[2t_{k-1}{x}_{k-1}+t{t}_k(x_{k-1}+2t_{k-1}{x}_{k-1}dt)]/2$
   • 龙格库塔法