粒子滤波算法

本文为《蒙特卡罗方法理论和应用》笔记

$\text{1.动态空间模型}$

动态空间模型包括过程模型和观测模型，分别由状态方程和观测方程描述，状态方程和观测方程为：
$$\begin{array}{rl}\mathbf{x}(t)& =a(\mathbf{x}(t-1),\mathbf{u}(t))\\ \mathbf{y}(t)& =b(\mathbf{x}(t),\mathbf{v}(t))\end{array}$$
当过程噪音和观测噪音可分离时，状态方程和观测方程为：
$$\begin{array}{rl}{\mathbf{x}}_k& =a\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)=a\left({\mathbf{x}}_{k-1}\right)+{\mathbf{u}}_k\\ {\mathbf{y}}_k& =b\left({\mathbf{x}}_k,{\mathbf{v}}_k\right)=b\left({\mathbf{x}}_k\right)+{\mathbf{v}}_k\end{array}$$
式中，所有下标$k$表示时刻；$a({\mathbf{x}}_{k-1},{\mathbf{u}}_k)$和$a({\mathbf{x}}_{k-1})$为过程函数； $b({\mathbf{x}}_k,{\mathbf{v}}_k)$和$b({\mathbf{x}}_k)$为观测函数；${\mathbf{x}}_k$和${\mathbf{x}}_{k-1}$为状态，${\mathbf{y}}_k$为观测；${\mathbf{u}}_k$为过程噪声； ${\mathbf{v}}_k$为观测噪声。过程噪声的统计参数为${\mathbf{Q}}_k$ ，观测噪声的统计参数为 ${\mathbf{R}}_k$ ，对单变量模型${\mathbf{Q}}_k$ 和 ${\mathbf{R}}_k$表示方差，对多变量模型${\mathbf{Q}}_k$ 和 ${\mathbf{R}}_k$表示协方差。可用统计参数${\mathbf{Q}}_k$ 和 ${\mathbf{R}}_k$唯一地描述高斯噪声，但不能唯一地描述非高斯噪声。

$\text{2.隐马尔可夫随机过程}$

序贯随机过程的下一状态不仅与当前状态有关，而且还与以前所有状态都有关。序贯随机过程的概率分布为：
$$f(\mathbf{x})=f\left({\mathbf{x}}_1|{\mathbf{x}}_{1:0}\right)f\left({\mathbf{x}}_2|{\mathbf{x}}_{1:1}\right)f\left({\mathbf{x}}_3|{\mathbf{x}}_{1:2}\right)\cdots f\left({\mathbf{x}}_k|{\mathbf{x}}_{1:k-1}\right)\cdots f\left({\mathbf{x}}_m|{\mathbf{x}}_{1:m-1}\right)$$
隐马尔可夫随机过程的概率分布为：
$$f(\mathbf{x})=f\left({\mathbf{x}}_1|{\mathbf{x}}_0\right)f\left({\mathbf{x}}_2|{\mathbf{x}}_1\right)\cdots f\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1}\right)\cdots f\left({\mathbf{x}}_m|{\mathbf{x}}_{m-1}\right)$$
隐马尔可夫随机过程本身具有马尔可夫性，即无后效性，下一状态只与当前状态有关，与以前状态无关。${\mathbf{x}}_{k+1}$只与${\mathbf{x}}_k$ 有关，与${\mathbf{x}}_{0:k-1}$无关，${\mathbf{y}}_k$只与${\mathbf{x}}_k$有关，状态$\mathbf{x}$接受的过程噪声$\mathbf{u}$，观测$\mathbf{y}$观测噪声$\mathbf{v}$.

$\text{3.贝叶斯递推滤波}$

状态变量$\mathbf{X}$ 的先验概率分布为$f(\mathbf{x})$ ，观测变量$\mathbf{Y}$ 的概率分布为 $f(\mathbf{y}|\mathbf{x})$
故状态变量$\mathbf{X}$ 的后验概率分布为
$$f(\mathbf{x}|\mathbf{y})=\frac{f(\mathbf{y}|\mathbf{x})f(\mathbf{x})}{\int f(\mathbf{y}|\mathbf{x})f(\mathbf{x})d\mathbf{x}}$$
状态变量的估计值为
$$\hat{\mathbf{x}}=\int \mathbf{x}f(\mathbf{x}|\mathbf{y})d\mathbf{x}$$
如果后验概率分布$f(\mathbf{x}|\mathbf{y})$的分母积分和状态变量估计值的积分都能求解，则可得到状态变量的估计值，称为$\text{贝叶斯估计}$.

$\text{4.贝叶斯递推滤波描述}$

$0:k$时刻的状态值用${\mathbf{x}}_{0:k}$表示，
${\mathbf{x}}_{0:k}={\mathbf{x}}_0,{\mathbf{x}}_1,\cdots ,{\mathbf{x}}_k$;
$1:k$时刻的观测值用${\mathbf{y}}_{1:k}$表示，
${\mathbf{y}}_{1:k}={\mathbf{y}}_1,{\mathbf{y}}_2,\cdots ,{\mathbf{y}}_k$.
贝叶斯递推滤波是给定所有观测值${\mathbf{y}}_{1:k}$构造状态${\mathbf{x}}_{0:k}$的后验概率分布，贝叶斯递推滤波分为$\underline{预测阶段}$和$\underline{更新阶段}$.

$\text{预测阶段}$，预测阶段主要基于${\mathbf{x}}_{k-1}$预测${\mathbf{x}}_k$ 预测方程为：$$f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k-1}\right)=\int f\left({\mathbf{x}}_{0:k}|{\mathbf{x}}_{0:k-1}\right)f\left({\mathbf{x}}_{0:k-1}|{\mathbf{y}}_{1:k-1}\right)\mathrm{d}{\mathbf{x}}_{0:k-1}$$

$\text{更新阶段}$，更新阶段主要根据${\mathbf{y}}_k$更新${\mathbf{x}}_k$. 根据贝叶斯定理，后验概率分布为
$$f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)=f\left({\mathbf{y}}_{1:k}|{\mathbf{x}}_{0:k}\right)f\left({\mathbf{x}}_{0:k}\right)/f\left({\mathbf{y}}_{1:k}\right)$$
式中，$$f\left({\mathbf{y}}_{1:k}\right)=\int f\left({\mathbf{y}}_{1:k}|{\mathbf{x}}_{0:k}\right)f\left({\mathbf{x}}_{0:k}\right)\mathrm{d}{\mathbf{x}}_{0:k}(1)$$

$\text{粒子滤波}$

$\text{1.后验概率分布变换}$

由于$f({\mathbf{y}}_{1:k}=f({\mathbf{y}}_{1:k-1},{\mathbf{y}}_k)$, 使用条件概率分布变换规则：$f(a,b|c)=f(a|b,c)f(b|c)$及$f(a,b)=f(a|b)f(b)$，后验概率分布变换为
$$f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)=\frac{f\left({\mathbf{y}}_{1:k-1},{\mathbf{y}}_k|{\mathbf{x}}_{0:k}\right)f\left({\mathbf{x}}_{0:k}\right)}{f\left({\mathbf{y}}_{1:k-1},{\mathbf{y}}_k\right)}=\frac{f\left({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1},{\mathbf{x}}_{0:k}\right)f\left({\mathbf{y}}_{1:k-1}|{\mathbf{x}}_{0:k}\right)f\left({\mathbf{x}}_{0:k}\right)}{f\left({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1}\right)f\left({\mathbf{y}}_{1;k-1}\right)}$$
再次使用贝叶斯定理：
$$f\left({\mathbf{y}}_{1:k-1}|{\mathbf{x}}_{0:k}\right)=f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k-1}\right)f\left({\mathbf{y}}_{1:k-1}\right)/f\left({\mathbf{x}}_{0:k}\right)$$
后验概率分布变换为
$$f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)=\frac{f\left({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1},{\mathbf{x}}_{0:k}\right)f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k-1}\right)f\left({\mathbf{y}}_{1:k-1}\right)f\left({\mathbf{x}}_{0:k}\right)}{f\left({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1}\right)f\left({\mathbf{y}}_{1:k-1}\right)f\left({\mathbf{x}}_{0:k}\right)}$$
由于$\mathbf{y}$的独立性，${\mathbf{y}}_k$ 与${\mathbf{y}}_{1:k-1}$ 无关，因此有$f\left({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1},{\mathbf{x}}_{0:k}\right)=f\left({\mathbf{y}}_k|{\mathbf{x}}_{0:k}\right)$ ，后验概率分布变换为$$f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)=f\left({\mathbf{y}}_k|{\mathbf{x}}_{0:k}\right)f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k-1}\right)/f\left({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1}\right)(2)$$
式中, $f\left({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1}\right)=\int f\left({\mathbf{y}}_k|{\mathbf{x}}_{0:k}\right)f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k-1}\right)\mathrm{d}{\mathbf{x}}_{0:k}$

$\text{2.蒙特卡罗状态估计}$

序贯重要抽样方法，由于采用重要抽样技巧，需要对结果纠偏，权重$w({\mathbf{X}}_k^i)$是纠偏因子，统计量估计值为
$${\hat{h}}_k=(1/n)\sum _{i=1}^{n}h({\mathbf{X}}_k^i)w({\mathbf{X}}_K^I)$$
概率分布为$f({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k})$ ，重要概率分布为$g({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k})$ ，使用式(2),得到统计量$h({\mathbf{x}}_{0:k})$ 的数学期望为
$$\begin{array}{rl}E[h({\mathbf{x}}_{0:k})]& =\int h({\mathbf{x}}_{0:k})f({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k})d{\mathbf{x}}_{0:k}\\ & =\int h\left({\mathbf{x}}_{0:k}\right)\left(f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)/g\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)\right)g\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)d{\mathbf{x}}_{0:k}\\ & =\int h\left({\mathbf{x}}_{0:k}\right)\frac{f\left({\mathbf{y}}_k|{\mathbf{x}}_{0:k}\right)f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k-1}\right)}{f\left({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1}\right)g\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)}g\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)d{\mathbf{x}}_{0:k}\\ & =\int h\left({\mathbf{x}}_{0:k}\right)\left(w\left({\mathbf{x}}_{0:k}\right)/f\left({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1}\right)\right)g\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)\mathrm{d}{\mathbf{x}}_{0:k}\end{array}$$
真实权重应为
$$f\left({\mathbf{y}}_k|{\mathbf{x}}_{0:k}\right)f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k-1}\right)/f\left({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1}\right)/g\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)$$
由于$f({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1})$是多维积分，实际上是未知的，为了得到可计算的权重，引入伪权重，伪权重为$$w\left({\mathbf{x}}_{0:k}\right)=f\left({\mathbf{y}}_k|{\mathbf{x}}_{0:k}\right)f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k-1}\right)/g\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)(3)$$
由于忽略了$f({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1})$,伪权重不是真正意义上的权重，已经失去纠偏的意义，不能用它对模拟结果进行纠偏。于是得到统计量$h({\mathbf{x}}_{0:k})$的数学期望为
$$\begin{array}{rl}E\left[h\left({\mathbf{x}}_{0:k}\right)\right]& =\int h\left({\mathbf{x}}_{0:k}\right)\left(w\left({\mathbf{x}}_{0:k}\right)/f\left({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1}\right)\right)g\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)\mathrm{d}{\mathbf{x}}_{0:k}\\ & =\frac{\int h\left({\mathbf{x}}_{0:k}\right)w\left({\mathbf{x}}_{0:k}\right)g\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)\mathrm{d}{\mathbf{x}}_{0:k}}{\int f\left({\mathbf{y}}_k|{\mathbf{x}}_{0:k}\right)f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k-1}\right)\mathrm{d}{\mathbf{x}}_{0:k}}\\ & =\frac{\int h\left({\mathbf{x}}_{0:k}\right)w\left({\mathbf{x}}_{0:k}\right)g\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)\mathrm{d}{\mathbf{x}}_{0:k}}{\int w\left({\mathbf{x}}_{0:k}\right)g\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)\mathrm{d}{\mathbf{x}}_{0:k}}\\ & =\frac{(1/n){\sum }_{i=1}^{n}h\left({\mathbf{X}}_{0:k}^i\right)w\left({\mathbf{X}}_{0:k}^i\right)}{(1/n){\sum }_{i=1}^{n}w\left({\mathbf{X}}_{0:k}^i\right)}=\sum _{i=1}^{n}h\left({\mathbf{X}}_{0:k}^i\right)\tilde{w}\left({\mathbf{X}}_{0:k}^i\right)\end{array}$$
式中归一化伪权重为
$$\tilde{w}\left({\mathbf{X}}_{0:k}^i\right)=w\left({\mathbf{X}}_{0:k}^i\right)/\sum _{i=1}^{n}w\left({\mathbf{X}}_{0:k}^i\right)$$
蒙特卡罗状态估计可写为
$${\hat{\mathbf{X}}}_{0:k}=\sum _{i=1}^n{\mathbf{X}}_k^i\tilde{w}\left({\mathbf{X}}_{0:k}^i\right)$$这是由于采用伪权重，蒙特卡罗状态估计的形式，与以前的形式不同，前面少了因子$1/n$，而且采用归一化伪权重.

$\text{3.递推伪权重}$

为了得到递推伪权重，需要对式（2）的后验概率分布作变换，由于$f({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k-1})=f({\mathbf{x}}_{0:k-1},{\mathbf{x}}_k|{\mathbf{y}}_{1:k-1})$,并使用条件概率分布变换规则$f(a,b|c)=f(a|b,c)\cdot f(b|c)$,后验概率分布变换为
$$f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)=f\left({\mathbf{y}}_k|{\mathbf{x}}_{0:k}\right)f\left({\mathbf{x}}_k|{\mathbf{x}}_{0:k-1},{\mathbf{y}}_{1:k-1}\right)f\left({\mathbf{x}}_{0:k-1}|{\mathbf{y}}_{1:k-1}\right)/f\left({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1}\right)$$
由于${\mathbf{y}}_k$只与${\mathbf{x}}_k$有关，${\mathbf{x}}_k$只与${\mathbf{x}}_{k-1}$有关，因此有
$$f\left({\mathbf{y}}_k|{\mathbf{x}}_{0:k}\right)f\left({\mathbf{x}}_k|{\mathbf{x}}_{0:k-1},{\mathbf{y}}_{1:k-1}\right)=f\left({\mathbf{y}}_k|{\mathbf{x}}_k\right)f\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1}\right)$$于是得到后验概率分布为
$$f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)=f\left({\mathbf{y}}_k|{\mathbf{x}}_k\right)f\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1}\right)f\left({\mathbf{x}}_{0:k-1}|{\mathbf{y}}_{1:k-1}\right)/f\left({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1}\right)(4)$$
各个时刻状态${\mathbf{X}}_{0:k}$的重要概率分布可写为
$$g\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)=g\left({\mathbf{x}}_k|{\mathbf{x}}_{0:k-1},{\mathbf{y}}_{1:k}\right)g\left({\mathbf{x}}_{0:k-1}|{\mathbf{y}}_{1:k-1}\right)$$
由于${\mathbf{x}}_k$只与${\mathbf{x}}_{k-1}$有关
$$g\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)=g\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{y}}_{1:k}\right)g\left({\mathbf{x}}_{0:k-1}|{\mathbf{y}}_{1:k-1}\right)$$
根据伪权重定义，并注意到式（4），得到各个时刻递推伪权重为
$$\begin{array}{rl}w\left({\mathbf{x}}_{0:k}\right)& =\frac{f\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)f\left({\mathbf{y}}_k|{\mathbf{y}}_{1:k-1}\right)}{g\left({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k}\right)}=\frac{f\left({\mathbf{y}}_k|{\mathbf{x}}_k\right)f\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1}\right)f\left({\mathbf{x}}_{0:k-1}|{\mathbf{y}}_{1:k-1}\right)}{g\left({\mathbf{x}}_{0:k-1}|{\mathbf{y}}_{1:k-1}\right)g\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{y}}_{1:k-1}\right)}\\ & =\frac{f\left({\mathbf{x}}_{0:k-1}|{\mathbf{y}}_{1:k-1}\right)}{g\left({\mathbf{x}}_{0:k-1}|{\mathbf{y}}_{1:k-1}\right)}\frac{f\left({\mathbf{y}}_k|{\mathbf{x}}_k\right)f\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1}\right)}{g\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{y}}_{1:k}\right)}\\ & =w\left({\mathbf{x}}_{0:k-1}\right)\frac{f\left({\mathbf{y}}_k|{\mathbf{x}}_k\right)f\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1}\right)}{g\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{y}}_{1:k}\right)}\end{array}(5)$$ 初始时刻伪权重 $w({\mathbf{x}}_0)=f({\mathbf{x}}_0)/g({\mathbf{x}}_0)$

$\text{4.标准粒子滤波过程}$

使用重要抽样技巧的关键是如何选取最佳重要概率分布$g({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k})$,从式（5）看出，选取重要概率分布$g({\mathbf{x}}_{0:k}|{\mathbf{y}}_{1:k})$变为选取概率分布$g({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{y}}_{1:k-1})$
通常可选取与观测值无关的先验概率分布作为重要概率分布，先验概率分布为
$$g({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{y}}_{1:k-1})=f({\mathbf{x}}_k|{\mathbf{x}}_{k-1})$$
由式（5）得到第k时刻伪权重为
$$w\left({\mathbf{x}}_k\right)=w\left({\mathbf{x}}_{k-1}\right)f\left({\mathbf{y}}_k|{\mathbf{x}}_k\right)$$由观测方程，得到观测函数$b({\mathbf{x}}_k)$，伪权重可写为$$w\left({\mathbf{x}}_k\right)=w\left({\mathbf{x}}_{k-1}\right)f\left({\mathbf{y}}_k-b\left({\mathbf{x}}_k\right)\right)$$
令${\mathbf{x}}_{\mathbf{k}},{\mathbf{X}}_k^{\mathbf{i}},{\mathbf{u}}_{\mathbf{k}},{\mathbf{U}}_{\mathbf{k}}^{\mathbf{i}}$分别表示状态变量，状态样本变量，过程噪声变量，过程噪声样本值。令${\mathbf{y}}_k,{\mathbf{Y}}_k^i,{\mathbf{v}}_k,{\mathbf{V}}_k^i$分别比哦啊是观测变量，观测样本值，观测噪声变量，观测噪声样本值。标准粒子滤波模拟过程包括初始化、序贯重要抽样、重抽样和状态值估计.

(1)初始化. 从初始概率分布$f({\mathbf{x}}_0)$ 抽样产生初始时刻样本值${\mathbf{X}}_0$ 给出初始伪权重 $w({\mathbf{X}}_0)$.

（2）序贯重要抽样. 序贯过程是迭代过程，在粒子滤波中是时间的迭代过程.从过程噪声概率分布$f({\mathbf{u}}_k)$ 抽样产生过程噪声样本值${\mathbf{U}}_k^i$ 。从观测噪声概率分布$f({\mathbf{v}}_k)$ 抽样产生观测噪声样本值${\mathbf{V}}_k^i$ 。从先验概率分布$f({\mathbf{x}}_k|{\mathbf{x}}_{k-1})$ 抽样产生状态样本值${\mathbf{X}}_k^i$ 。伪权重为
$$w\left({\mathbf{X}}_k^i\right)=w\left({\mathbf{X}}_{k-1}^i\right)f\left({\mathbf{Y}}_k^i|{\mathbf{X}}_k^i\right)=w\left({\mathbf{X}}_{k-1}^i\right)f\left({\mathbf{Y}}_k^i-b\left({\mathbf{X}}_k^i\right)\right)$$

(3)重抽样. 重抽样算法有随机重抽样算法、残差重抽样算法、系统重抽样算法、分层重抽样算法和优化组合重抽样算法等. 归一化伪权重为
$$\tilde{w}\left({\mathbf{X}}_k^i\right)=w\left({\mathbf{X}}_k^i\right)/\sum _{i=1}^{n}w\left({\mathbf{X}}_k^i\right)$$
有效样本数为
$$N_{\mathrm{e}\mathrm{f}\mathrm{f}}=1/\sum _{i=1}^n{\left(\tilde{w}\left({\mathbf{X}}_k^i\right)\right)}^2$$
如果有效样本数小于阈值，则进行重抽样。例如，随机重抽样的概率分布为
$$f(i)=\tilde{w}\left({\mathbf{X}}_k^i\right)$$
利用直接抽样方法的列表查找算法抽样，得到i的样本值为
$$I=\min \left\{i:F_{i-1}<U⩽F_i\right\}$$
式中，累积分布函数为
$$F_i=\sum _{i=1}^n\tilde{w}\left({\mathbf{X}}_k^i\right)$$每个时刻模拟次数为n，重抽样随机变量$\mathbf{X}$的样本值和伪权重为
$${\tilde{\mathbf{X}}}_k^i={\mathbf{X}}_k^I,w\left({\tilde{\mathbf{X}}}_k^i\right)=1/n$$
(4)状态值的估计值和误差.每个时刻的状态估计值为
$${\hat{\mathbf{X}}}_k=\sum _{i=1}^n{\mathbf{X}}_k^i\tilde{w}\left({\mathbf{X}}_k^i\right)$$
有$m$个时刻数，粒子滤波状态值估计的误差用均方根误差表示，均方根误差为
$$\mathrm{R}\mathrm{M}\mathrm{S}\mathrm{E}={\left((1/m)\sum _{k=1}^m{\left({\hat{\mathbf{X}}}_k-{\mathbf{X}}_k\right)}^2\right)}^{1/2}$$

    参考：康崇禄《蒙特卡罗方法理论和应用》