粒子滤波方法

粒子滤波

根据B站教程总结

贝叶斯滤波

一些公式

贝叶斯公式,$\eta$可以看做归一化系数
$$\begin{gathered} X:状态,Y:观测 \\ 离散：P(X=x|Y=y)=\frac{P(Y=y|X=x)P(X=x)}{P(Y=y)}=\eta P(Y=y|X=x)P(X=x) \\ 连续:f_{X|Y}(x|y)=\frac{f_{Y|X}(y|x)f_X(x)}{f_Y(y)}=\eta f_{Y|X}(y|x)f_X(x) \end{gathered}$$
概率公式
$$P(X=x)=\underset{\epsilon \to 0}{\lim }f(x<X<x+\epsilon )\epsilon$$

贝叶斯滤波

$$\begin{gathered} 预测：X_k=f(X_{k-1})+Q_k \\ 观测：Y_k=h(X_k)+R_k \\ {Q}_k,R_k是预测噪声和观测噪声 \\ {Q}_k,R_k,X_0独立 \end{gathered}$$
对于连续变量，$X_0$假定其概率密度函数为$f_0$

预测(得到先验概率)

$$\begin{array}{rl}P(X_1=u)& =\sum _{v=-\infty }^{v=\infty }P(X_1=u|X_0=v)P(X_0=v)\\ & =\sum _{v=-\infty }^{v=\infty }P[X_1-f(X_0)=u-f(v)|X_0=v]P(X_0=v)X_{k-1}与Q_k独立\\ & =\sum _{v=-\infty }^{v=\infty }P[Q_1=u-f(v)]P(X_0=v)\\ & =\underset{\epsilon \to 0}{\lim }\sum _{v=-\infty }^{v=\infty }{f}_{Q_1}(u-f(v))\epsilon f_0(v)\epsilon \\ & =\underset{\epsilon \to 0}{\lim }{\int }_{-\infty }^{\infty }{f}_{Q_1}(u-f(v))f_0(v)dv\epsilon \end{array}$$
$$\begin{array}{rl}P(X_1<x)& =\sum _{u=-\infty }^{x}P(X_1=u)\\ & =\underset{\epsilon \to 0}{\lim }\sum _{u=-\infty }^x{\int }_{-\infty }^{\infty }{f}_{Q_1}(u-f(v))f_0(v)dv\epsilon \\ & ={\int }_{-\infty }^x{\int }_{-\infty }^{\infty }{f}_{Q_1}(u-f(v))f_0(v)dvdu\end{array}$$
预测得到X_1的概率密度函数：
$$f_1^{-}(x)=\frac{dP(X_1<x)}{dx}={\int }_{-\infty }^{\infty }{f}_{Q_1}[x-f(v))]f_0(v)dv$$

更新

观测到$Y_1=y_1$,似然函数为
$$\begin{array}{rl}{f}_{Y_1|X_1}(y|x)& =\underset{\epsilon \to 0}{\lim }\frac{P_{Y_1|X_1}(y_1<Y_1<y_1+\epsilon |X_1=x)}{\epsilon }\\ & =\underset{\epsilon \to 0}{\lim }\frac{P_{Y_1|X_1}(y_1-h(x)<Y_1-h(X_1)<y+\epsilon -h(x)|X_1=x)}{\epsilon }\\ & =\underset{\epsilon \to 0}{\lim }\frac{P_{Y_1|X_1}(y_1-h(x)<R_1<y_1+\epsilon -h(x)|X_1=x)}{\epsilon }(R_k与X_k独立)\\ & =f_{R_1}[y-h(x)]\end{array}$$
得到后验概率
$$f_1^{+}(x)=f_{X_1|Y_1}(x|y_1)=\eta f_{Y_1|X_1}(y_1|x)f_1^{-}(x)=\eta f_{R_1}[y_1-h(x)])f_1^{-}(x)$$
那么估计值为
$${\hat{x}}_1={\int }_{-\infty }^{\infty }x{f}_1^{+}(x)dx$$

总结

主要步骤如下：

• 假设$X_0$及其概率分布函数$f_0$
• 预测步：由上一步的后验概率得到这一步的先验概率$f_k^{-}(x)={\int }_{-\infty }^{\infty }{f}_{Q_k}[x-f(v))]f_{k-1}(v)dv$
• 更新步：得到本次的后验概率$f_k^{+}(x)=\eta f_{R_k}[y_k-h(x)])f_k^{-}(x)$
• 估值：${\hat{x}}_k={\int }_{-\infty }^{\infty }x{f}_k^{+}(x)dx$

问题在于用到无穷积分，多数情况无解析解。

粒子滤波

一些等式和定理

特征函数：逆傅里叶变换
N ( μ , σ 2 ) → 特征函数 / I F T e x p { j u t + σ 2 2 t 2 } 若 X , Y 的 p d f 为 f , g , 且 X , Y 独立，则 Z = X + Y 的 p d f 为 f ∗ g N(\mu,\sigma^2) \overset{特征函数/IFT}{\rightarrow}exp\{jut+\frac{\sigma^2}{2}t^2\}\\ 若X,Y的pdf为f,g,且X,Y独立，则Z=X+Y的pdf为f*g N(μ,σ2)→特征函数/IFTexp{jut+2σ2​t2}若X,Y的pdf为f,g,且X,Y独立，则Z=X+Y的pdf为f∗g

一些概念

根据大数定理知道，对于随机变量X的大量采样{$x_1$,…,$x_n$}的均值依概率收敛于它的期望E[X]，即
$$\underset{n\to \infty }{\lim }P(|\frac{1}{n}\sum _i^n{x}_i-E[X]|<\epsilon )=1,\forall \epsilon >0$$
那么，足够的采样值可以满足
$$\frac{1}{n}\sum x_i={\int }_{-\infty }^{\infty }\frac{1}{n}\sum _i^n{x}_i\delta (x-x_i)dx=E[X]={\int }_{-\infty }^{\infty }xf(x)dx$$
即
$$f(x)\approx \frac{1}{n}\sum _i^n\delta (x-x_i)$$
可以看做一个粒子的和，也就是粒子滤波的来源。
当然，由于粒子是从f(x)中采样出来的，可以给予f(x)大的粒子更大的权重以期使用较少的粒子得到较好的效果。
即
$$w_i=\frac{f(x_i)}{{\sum }_k^{n}f(x_k)}$$

过程

给出$X_0$，及其概率密度函数$f_0$，和采样{$x_0^1$,…,$x_0^n$}

预测

$$\begin{array}{rl}{f}_1^{-}(x)& ={\int }_{-\infty }^{\infty }{f}_{Q_1}[x-f(v))]f_0(v)dv\\ & =\sum _i^n{w}_i{f}_{Q_1}[x-f(x_0^i))]\end{array}$$
假设$Q_k$为正态分布，则
f Q [ x − f ( x 0 i ) ] = 1 2 π Q e x p { − [ x − f ( x 0 i ) ] 2 2 Q } ∼ N ( f ( x 0 i ) , Q ) f_Q[x-f(x_0^i)]=\frac{1}{\sqrt{2\pi Q}}exp\{-\frac{[x-f(x_0^i)]^2}{2Q}\} \sim N(f(x_0^i),Q)\\ fQ​[x−f(x0i​)]=2πQ ​1​exp{−2Q[x−f(x0i​)]2​}∼N(f(x0i​),Q)
其特征函数为
f Q [ x − f ( x 0 i ) ] → 特征函数 / I F T e x p { j f ( x 0 i ) t } ⋅ e x p { − Q 2 t 2 } f_Q[x-f(x_0^i)]\overset{特征函数/IFT}{\rightarrow}exp\{jf(x_0^i)t\}\cdot exp\{-\frac{Q}{2}t^2\} fQ​[x−f(x0i​)]→特征函数/IFTexp{jf(x0i​)t}⋅exp{−2Q​t2}
由于
e x p { j f ( x 0 i ) t } → F T δ ( x − f ( x 0 i ) ) e x p { − Q 2 t 2 } → F T N ( 0 , Q ) exp\{jf(x_0^i)t\}\overset{FT}{\rightarrow}\delta(x-f(x_0^i))\\ exp\{-\frac{Q}{2}t^2\}\overset{FT}{\rightarrow}N(0,Q) exp{jf(x0i​)t}→FTδ(x−f(x0i​))exp{−2Q​t2}→FTN(0,Q)
所以$f_Q[x-f(x_0^i)]$可以看做两个事件的和事件，必然事件$\delta (x-f(x_0^i))$和随机事件$N(0,Q)$。所以可以得到$f_1^{-}(x)$的采样粒子{$x_1^{-1}$,…,$x_1^{-n}$}
$$x_1^{-i}=f(x_0^i)+Q$$
即
$$f_1^{-}(x)=\sum _i^n{w}_i{f}_{Q_1}[x-f(x_0^i))]=\sum _i^n{w}_i\delta (x-x_1^{-i})$$

更新

观测数据$Y_1=y_1$
$$\begin{array}{rl}{f}_1^{+}(x)& =\eta f_{R_1}[y_1-h(x)])f_1^{-}(x)\\ & =\eta \sum _i^n{w}_i{f}_{R_1}[y_1-h(x_1^{-i})]\delta (x-x_1^{-i})\end{array}$$
可以更新权重
$$\begin{gathered} w_i=w_i{f}_{R_1}[y_1-h(x_1^{-i})] \\ {w}_i=\frac{f(x_i)}{{\sum }_k^{n}f(x_k)} \end{gathered}$$
得到
$$f_1^{+}(x)=\sum _i^n{w}_i\delta (x-x_1^{-i})$$
可以估计得到
$${\hat{x}}_1=\sum _i^n{w}_i{x}_1^{-i}$$

总结

主要步骤如下：

• 假设$X_0$及其概率分布函数$f_0$，以及生成的采样{$x_0^i$,…,$x_0^n$}
• 预测步：根据上一步的采样生成当前采样$x_k^{-i}=f(x_{k-1}^i)+Q$
• 更新步：更新权重$w_i=w_i{f}_{R_k}[y_k-h(x_k^{-i})]$,并归一化
• 估值：${\hat{x}}_k={\sum }_i^n{w}_i{x}_k^{-i}$

补充

重采样

在权重更新中，若假设噪声是高斯分布，会有大量粒子的权重趋于0，出现粒子退化，最终导致仅有几个或一个粒子有效，失去粒子更新的作用。
重采样方法和遗传算法类似。主要步骤如下：

• 根据不同粒子的权重，在(0,1)的区间分配区间
• 生成随机数
• 根据随机数，选取区间，并复制相应的粒子
• 权重重新赋为相同

可以依据$N=\frac{1}{{\sum }_i^n{w}_i^2}$进行重采样，N越小退化越严重，越要重采样。