贝叶斯滤波完了之后，按顺序就是粒子滤波，在这次拜读了两个大神的博客，看了些视频后，在此做个整理，博客大部分都摘录自 https://heyijia.blog.csdn.net/article/details/40899819，另外老王的视频也很不错哦。
再次声明，博客原文地址为https://heyijia.blog.csdn.net/article/details/40899819，我这边只是整理了下格式，添加了点自己的见解而已。
首先，把原作者文章的架构说一下，文章先由贝叶斯估计开始，然后引出了蒙特卡洛采样，重要性采样，SIS滤波，重采样，基本粒子滤波，SIR粒子滤波，各个概念原作者环环相扣，均解决了上一个概念中的问题。
最后，作者还给出了几个在Matlab和Python中的应用，包括图像跟踪、滤波、机器人定位。
一、贝叶斯滤波
我的上一篇文章中，已经很详细的介绍了贝叶斯滤波，如果按顺序看的话，这里就当复习了。
假设有一个系统，其状态方程和测量方程为：
{
x
k
=
f
k
(
x
k
−
1
,
v
k
−
1
)
y
k
=
h
k
(
x
k
,
n
k
)
(1)
\begin{cases} x_{k}=f_{k}(x_{k-1},v_{k-1}) \\ y_{k}=h_{k}(x_k,n_k) \end{cases} \tag{1}
{xk=fk(xk−1,vk−1)yk=hk(xk,nk)(1)
其中，
x
k
x_{k}
xk为第
k
k
k次系统状态，
y
k
y_{k}
yk为第
k
k
k次的测量数据，
f
k
(
x
k
−
1
,
v
k
−
1
)
f_{k}(x_{k-1},v_{k-1})
fk(xk−1,vk−1)为状态转移函数，
h
k
(
x
k
,
n
k
)
h_{k}(x_k,n_k)
hk(xk,nk)为测量函数，
v
k
v_{k}
vk为状态转移过程噪声，
n
k
n_{k}
nk为测量噪声。
从贝叶斯理论的观点来看，状态估计问题（目标跟踪、信号滤波）就是根据一系列的已有数据 y 1 : k y_{1:k} y1:k（后验知识）以及系统状态转移方程，递推出当前系统状态 x k x_{k} xk的可信度。这个可信度就是概率公式 p ( x k ∣ y 1 : k ) p(x_{k}|y_{1:k}) p(xk∣y1:k)，需要通过预测和更新两个步骤来进行递推计算。
预测过程是根据系统当前状态，并结合状态转移方程，来预测下一时刻的系统状态的先验概率密度，既是 p ( x k ∣ x k − 1 ) p(x_{k}|x_{k-1}) p(xk∣xk−1)。
更新过程则是利用最新的测量值 y k y_{k} yk对先验概率密度 p ( x k ∣ x k − 1 ) p(x_{k}|x_{k-1}) p(xk∣xk−1)进行修正，得到后验概率密度。
1.预测
在处理这些系统的时候，一般都会先假设系统的状态转移服从一阶马尔可夫模型，即当前时刻的系统状态 x k x_k xk仅由上一时刻的系统状态 x k − 1 x_{k-1} xk−1有关。
为了便于递推，假设已知
k
−
1
k-1
k−1时刻的概率密度为
p
(
x
k
−
1
∣
y
1
:
k
−
1
)
p(x_{k-1}|y_{1:k-1})
p(xk−1∣y1:k−1)，根据
k
−
1
k-1
k−1时刻的概率密度，结合状态转移方程，可以得到先验概率密度
p
(
x
k
∣
y
1
:
k
−
1
)
p(x_{k}|y_{1:k-1})
p(xk∣y1:k−1)，如下
p
(
x
k
∣
y
1
:
k
−
1
)
=
∫
p
(
x
k
,
x
k
−
1
∣
y
1
:
k
−
1
)
d
x
k
−
1
=
∫
p
(
x
k
∣
x
k
−
1
,
y
1
:
k
−
1
)
p
(
x
k
−
1
∣
y
1
:
k
−
1
)
d
x
k
−
1
=
∫
p
(
x
k
∣
x
k
−
1
)
p
(
x
k
−
1
∣
y
1
:
k
−
1
)
d
x
k
−
1
(2)
\begin{aligned} p(x_{k}|y_{1:k-1})&=\int p(x_{k},x_{k-1}|y_{1:k-1})dx_{k-1} \\ &=\int p(x_{k}|x_{k-1},y_{1:k-1})p(x_{k-1}|y_{1:k-1})dx_{k-1} \\ &=\int p(x_{k}|x_{k-1})p(x_{k-1}|y_{1:k-1})dx_{k-1} \tag{2} \end{aligned}
p(xk∣y1:k−1)=∫p(xk,xk−1∣y1:k−1)dxk−1=∫p(xk∣xk−1,y1:k−1)p(xk−1∣y1:k−1)dxk−1=∫p(xk∣xk−1)p(xk−1∣y1:k−1)dxk−1(2)
式中，第一步到第二步，就是纯粹的贝叶斯公式的应用；
第二步到第三步，使用的是一阶马尔科夫过程的假设，状态
x
k
x_k
xk只由
x
k
−
1
x_{k-1}
xk−1决定。
这里便于理解，做一下补充和说明：
对于
p
(
x
k
∣
x
k
−
1
)
=
p
(
x
k
∣
x
k
−
1
,
y
1
:
k
−
1
)
p(x_{k}|x_{k-1})=p(x_{k}|x_{k-1},y_{1:k-1})
p(xk∣xk−1)=p(xk∣xk−1,y1:k−1)，是因为
x
k
x_k
xk只由
x
k
−
1
x_{k-1}
xk−1决定， 与
y
1
…
y
k
−
1
y_1 \ldots y_{k-1}
y1…yk−1无关，那么
p
(
x
k
∣
y
1
:
k
−
1
)
p(x_{k}|y_{1:k-1})
p(xk∣y1:k−1)是什么意思呢？
这两个公式的意义是不一样的， p ( x k ∣ x k − 1 ) p(x_{k}|x_{k-1}) p(xk∣xk−1)表示根据 x k − 1 x_{k-1} xk−1进行模型预测可以得到 x k x_{k} xk， x k x_k xk只由 x k − 1 x_{k-1} xk−1决定；而 p ( x k ∣ y 1 : k − 1 ) p(x_{k}|y_{1:k-1}) p(xk∣y1:k−1)的意思为，我们可以根据已经测量到的值 y 1 … y k − 1 y_1 \ldots y_{k-1} y1…yk−1对 x k x_k xk进行预测，而并不能决定 x k x_k xk。
2.更新
由先验概率 p ( x k ∣ y 1 : k − 1 ) p(x_{k}|y_{1:k-1}) p(xk∣y1:k−1)得到后验概率 p ( x k ∣ y 1 : k ) p(x_{k}|y_{1:k}) p(xk∣y1:k)，这个后验概率才是真正有用的数据。先验概率仅仅只是当当前时刻的状态 x k x_k xk进行了预测，这里把先验概率结合当前时刻的观测量 y k y_k yk，对先验概率进行了修正，这就是滤波。这里计算出的后验概率也将是下一时刻的先验，形成了递推（这里说的并不是很详细，上一篇文章更详细）。
p ( x k ∣ y 1 : k ) = p ( y k ∣ x k , y 1 : k − 1 ) p ( x k ∣ y 1 : k − 1 ) p ( y k ∣ y 1 : k − 1 ) = p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 ) p ( y k ∣ y 1 : k − 1 ) (3) \begin{aligned} 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})} \end{aligned} \tag{3} p(xk∣y1:k)=p(yk∣y1:k−1)p(yk∣xk,y1:k−1)p(xk∣y1:k−1)=p(yk∣y1:k−1)p(yk∣xk)p(xk∣y1:k−1)(3)
其中，可以将分母归一化为常数：
p
(
y
k
∣
y
1
:
k
−
1
)
=
∫
p
(
y
k
∣
x
k
)
p
(
x
k
∣
y
1
:
k
−
1
)
d
x
k
p(y_{k}|y_{1:k-1})=\int p(y_{k}|x_k)p(x_{k}|y_{1:k-1}) dx_{k}
p(yk∣y1:k−1)=∫p(yk∣xk)p(xk∣y1:k−1)dxk
其中， p ( y k ∣ x k ) p(y_{k}|x_k) p(yk∣xk)称之为似然函数，从式（1）可以看出， y ( k ) y(k) y(k)只与 x ( k ) x(k) x(k)有关，由测量方程决定，又因为 x ( k ) x(k) x(k)是常数，所以只和测量噪声 n ( k ) n(k) n(k)有关，注意这里也将为后面的SIR粒子滤波里权重的采样提供重要的理论依据。
第一部分就是简单的复习了一下贝叶斯滤波，贝叶斯滤波最大的问题，就是不可避免的要求积分，这样对于非线性，非高斯系统，很难得到后验概率的解析解，为了解决这个问题，下面将引入蒙特卡洛采样。
二、蒙特卡洛采样
假设我们可以从一个目标概率分布
p
(
x
)
p(x)
p(x)中采样到一系列的样本（粒子）
x
1
,
x
2
,
…
,
x
n
x_{1},x_{2},\ldots ,x_{n}
x1,x2,…,xn（至于怎么生成服从
p
(
x
)
p(x)
p(x)概率分布的样本，这个问题先放一放），那么就能利用这些样本去估计这个概率分布的期望值，例如
E
(
f
(
x
)
)
=
∫
a
b
f
(
x
)
p
(
x
)
d
x
V
a
r
(
f
(
x
)
)
=
E
(
f
(
x
)
−
E
(
f
(
x
)
)
)
2
=
∫
a
b
[
f
(
x
)
−
E
(
f
(
x
)
)
]
2
p
(
x
)
d
x
(4)
\begin{aligned} E(f(x))&=\displaystyle\int_{a}^{b}f(x)p(x)dx \\ Var(f(x))&=E(f(x)-E(f(x)))^{2}=\displaystyle\int_{a}^{b}[f(x)-E(f(x))]^{2}p(x)dx \end{aligned} \tag{4}
E(f(x))Var(f(x))=∫abf(x)p(x)dx=E(f(x)−E(f(x)))2=∫ab[f(x)−E(f(x))]2p(x)dx(4)
上面的式子其实都是计算期望的问题，只是被积分的函数不同。
蒙特卡洛采样的思想就是，使用平均值来代替积分，求期望：
E
(
f
(
x
)
)
≈
f
(
x
1
)
+
…
+
f
(
x
N
)
N
(5)
E(f(x)) \approx \frac{f(x_1)+\ldots + f(x_{N})}{N} \tag{5}
E(f(x))≈Nf(x1)+…+f(xN)(5)
也可以从大数定律的角度去理解。我们用这中思想去指定不同的 f ( x ) f(x) f(x)以便达到估计不同东西的目的。比如说，需要估计一批同龄人的体重，不分男女，在大样本中有男性1000个，女性200个，为了减少工作量，可以直接按比例随机选取100个男性，20个女性，并计算这120个人的平均值就可以了。注意这里的按比例抽取，就可以看成从概率分布 p ( x ) p(x) p(x)中进行抽样。
贝叶斯遗留的问题是，贝叶斯的后验计算要用到积分，为了解决这个问题，可以使用蒙特卡洛采样来代替计算后验概率。
假设可以从后验概率中采样带N个样本，那么后验概率的计算可以表示为：
p
^
(
x
n
∣
y
1
:
k
)
=
1
N
∑
i
=
1
N
δ
(
x
n
−
x
n
(
i
)
)
≈
p
(
x
n
∣
y
1
:
k
)
(6)
\hat{p}(x_n|y_{1:k})=\frac{1}{N}\displaystyle\sum_{i=1}^{N}\delta(x_{n}-x_{n}^{(i)}) \approx p(x_{n}|y_{1:k}) \tag{6}
p^(xn∣y1:k)=N1i=1∑Nδ(xn−xn(i))≈p(xn∣y1:k)(6)
其中，在这个蒙特卡洛方法中，定义 f ( x ) = δ ( x n − x n ( i ) ) f(x)=\delta(x_{n}-x_{n}^{(i)}) f(x)=δ(xn−xn(i))，是狄拉克delta函数。
看到这里，既然可以用蒙特卡洛方法直接估计后验概率，那么现在估计出了后验概率，那到底怎么用来做图像跟踪或者滤波呢？
其实要做图像跟踪或者滤波，其实就是想知道当前状态的期望值：
E
[
f
(
x
n
)
]
≈
∫
f
(
x
n
)
p
^
(
x
n
∣
y
1
:
k
)
d
x
n
=
1
N
∑
i
=
1
N
∫
f
(
x
n
)
δ
(
x
n
−
x
n
(
i
)
)
d
x
n
=
1
N
∑
i
=
1
N
f
(
x
n
(
i
)
)
(7)
\begin{aligned} E[f(x_n)]& \approx \int f(x_n)\hat{p}(x_n|y_{1:k})dx_{n} \\ &=\frac{1}{N} \displaystyle \sum_{i=1}^{N} \int f(x_n)\delta(x_{n}-x_{n}^{(i)})dx_{n} \\ &=\frac{1}{N} \displaystyle \sum_{i=1}^{N}f(x_{n}^{(i)}) \end{aligned} \tag{7}
E[f(xn)]≈∫f(xn)p^(xn∣y1:k)dxn=N1i=1∑N∫f(xn)δ(xn−xn(i))dxn=N1i=1∑Nf(xn(i))(7)
上式就是用采样得到的粒子的状态直接平均就得到了期望值，也就是滤波后的值，这里的 f ( x ) f(x) f(x)就是每个粒子的状态函数。
上述就是粒子滤波了，只要从后验概率中采样很多的粒子，用他们的状态求平均值就可以得到滤波结果了。
思路看似简单，但是我们并不知道后验概率分布是啥，那怎么从后验概率中采样？所以下面就引入了重要性采样这个方法来解决这个问题。
三、重要性采样
因为无法知道目标的概率分布，可以从一个已知的可以采样的概率分布里去采样，如
q
(
x
∣
y
)
q(x|y)
q(x∣y)，这样上面的求期望的问题就变成了：
E
[
f
(
x
k
)
]
=
∫
f
(
x
k
)
p
(
x
k
∣
y
1
:
k
)
q
(
x
k
∣
y
1
:
k
)
q
(
x
k
∣
y
1
:
k
)
d
x
k
=
∫
f
(
x
k
)
p
(
y
1
:
k
∣
x
k
)
p
(
x
k
)
p
(
y
1
:
k
)
1
q
(
x
k
∣
y
1
:
k
)
q
(
x
k
∣
y
1
:
k
)
d
x
k
=
∫
f
(
x
k
)
W
k
(
x
k
)
p
(
y
1
:
k
)
q
(
x
k
∣
y
1
:
k
)
d
x
k
(8)
\begin{aligned} E[f(x_k)]&=\int f(x_k) \frac{p(x_{k}|y_{1:k})}{q(x_{k}|y_{1:k})}q(x_{k}|y_{1:k})dx_{k} \\ &=\int f(x_k) \frac{p(y_{1:k}|x_{k})p(x_k)}{p(y_{1:k})} \frac{1}{q(x_{k}|y_{1:k})} q(x_{k}|y_{1:k})dx_{k} \\ &=\int f(x_k) \frac{W_{k}(x_k)}{p(y_{1:k})} q(x_{k}|y_{1:k})dx_{k} \end{aligned} \tag{8}
E[f(xk)]=∫f(xk)q(xk∣y1:k)p(xk∣y1:k)q(xk∣y1:k)dxk=∫f(xk)p(y1:k)p(y1:k∣xk)p(xk)q(xk∣y1:k)1q(xk∣y1:k)dxk=∫f(xk)p(y1:k)Wk(xk)q(xk∣y1:k)dxk(8)
其中，
W
k
(
x
k
)
=
p
(
y
1
:
k
∣
x
k
)
p
(
x
k
)
q
(
x
k
∣
y
1
:
k
)
∝
p
(
x
k
∣
y
1
:
k
)
q
(
x
k
∣
y
1
:
k
)
W_{k}(x_k)=\frac{p(y_{1:k}|x_{k})p(x_k)}{q(x_{k}|y_{1:k})} \propto \frac{p(x_k|y_{1:k})}{q(x_{k}|y_{1:k})}
Wk(xk)=q(xk∣y1:k)p(y1:k∣xk)p(xk)∝q(xk∣y1:k)p(xk∣y1:k)
所以，式(8)可以进一步写成：
E
[
f
(
x
k
)
]
=
1
p
(
y
1
:
k
)
∫
f
(
x
k
)
W
k
(
x
k
)
q
(
x
k
∣
y
1
:
k
)
d
x
k
=
∫
f
(
x
k
)
W
k
(
x
k
)
q
(
x
k
∣
y
1
:
k
)
d
x
k
∫
p
(
y
1
:
k
∣
x
k
)
p
(
x
k
)
d
x
k
=
∫
f
(
x
k
)
W
k
(
x
k
)
q
(
x
k
∣
y
1
:
k
)
d
x
k
∫
W
k
(
x
k
)
q
(
x
k
∣
y
1
:
k
)
d
x
k
=
E
q
(
x
k
∣
y
1
:
k
)
[
W
k
(
x
k
)
f
(
x
k
)
]
E
q
(
x
k
∣
y
1
:
k
)
[
W
k
(
x
k
)
]
(9)
\begin{aligned} E[f(x_{k})]&=\frac{1}{p(y_{1:k})}\int f(x_{k})W_{k}(x_{k}) q(x_{k}|y_{1:k}) dx_{k} \\ &=\frac{\int f(x_{k})W_{k}(x_{k}) q(x_{k}|y_{1:k}) dx_{k}}{\int p(y_{1:k}|x_{k})p(x_k) dx_{k}} \\ &=\frac{\int f(x_{k})W_{k}(x_{k}) q(x_{k}|y_{1:k}) dx_{k}}{\int W_{k}(x_k) q(x_{k}|y_{1:k}) dx_{k}} \\ &=\frac{E_{q(x_{k}|y_{1:k})}[W_{k}(x_k)f(x_{k})]}{E_{q(x_{k}|y_{1:k})}[W_{k}(x_k)]} \end{aligned} \tag{9}
E[f(xk)]=p(y1:k)1∫f(xk)Wk(xk)q(xk∣y1:k)dxk=∫p(y1:k∣xk)p(xk)dxk∫f(xk)Wk(xk)q(xk∣y1:k)dxk=∫Wk(xk)q(xk∣y1:k)dxk∫f(xk)Wk(xk)q(xk∣y1:k)dxk=Eq(xk∣y1:k)[Wk(xk)]Eq(xk∣y1:k)[Wk(xk)f(xk)](9)
上面的期望计算都可以通过蒙特卡洛方法来解决，也就是说，通过采样Ｎ个样本
x
k
(
i
)
∼
q
(
x
k
∣
y
1
:
k
)
x_{k}^{(i)} \sim q(x_{k}|y_{1:k})
xk(i)∼q(xk∣y1:k)，用样本的平均值求它们的期望，所以式(9)可以近似为：
E
[
f
(
x
k
)
]
≈
1
N
∑
i
=
1
N
W
k
(
x
k
(
i
)
)
f
(
x
k
(
i
)
)
1
N
∑
i
=
1
N
W
k
(
x
k
(
i
)
)
=
∑
i
=
1
N
W
~
k
(
x
k
(
i
)
)
f
(
x
k
(
i
)
)
(10)
\begin{aligned} E[f(x_{k})]& \approx \frac{\frac{1}{N}\sum_{i=1}^{N} W_{k}(x_k^{(i)})f(x_{k}^{(i)})}{\frac{1}{N}\sum_{i=1}^{N} W_{k}(x_k^{(i)})} \\ &=\sum_{i=1}^{N} \tilde{W}_{k}(x_k^{(i)})f(x_{k}^{(i)}) \end{aligned} \tag{10}
E[f(xk)]≈N1∑i=1NWk(xk(i))N1∑i=1NWk(xk(i))f(xk(i))=i=1∑NW~k(xk(i))f(xk(i))(10)
其中，
W
~
k
(
x
k
(
i
)
)
=
W
k
(
x
k
(
i
)
)
∑
i
=
1
N
W
k
(
x
k
(
i
)
)
\tilde{W}_{k}(x_k^{(i)})=\frac{W_{k}(x_k^{(i)})}{\sum_{i=1}^{N} W_{k}(x_k^{(i)})}
W~k(xk(i))=∑i=1NWk(xk(i))Wk(xk(i))
这就是归一化的权重，而之前在式(8)中的那个权重是没有归一化的。
注意上面的式(10)，不再是式(7)中所有的粒子状态直接相加求平均了，而是一种加权求和的形式。不同的粒子都有它们相应的权重，如果粒子的权重越大，就说明对该粒子更加信任。
到此为止，已经解决了不能从后验概率直接采样的问题，但是上面这种每个粒子的权重都直接计算的方法，效率很低，因为每增加一个采样， p ( x k ∣ y 1 : k ) p(x_{k}|y_{1:k}) p(xk∣y1:k)全部都需要重新计算，并且这个还不太好计算，所以求权重时能否避开计算 p ( x k ∣ y 1 : k ) p(x_{k}|y_{1:k}) p(xk∣y1:k)？ 而最佳的形式是能够以递推的方式去计算权重，这就是序贯重要性采样(SIS)，粒子滤波的原型。
下面开始权重 W W W递推形式的推导：
假设重要性概率密度函数
q
(
x
0
:
k
∣
y
1
:
k
)
q(x_{0:k}|y_{1:k})
q(x0:k∣y1:k)，这里
x
x
x的下标是
0
:
k
0:k
0:k，也就是说粒子滤波是估计过去所有时刻的状态的后验。假设它可以分解为：
q
(
x
0
:
k
∣
y
1
:
k
)
=
q
(
x
0
:
k
−
1
∣
y
1
:
k
−
1
)
q
(
x
k
∣
x
0
:
k
−
1
,
y
1
:
k
)
q(x_{0:k}|y_{1:k})=q(x_{0:k-1}|y_{1:k-1})q(x_{k}|x_{0:k-1},y_{1:k})
q(x0:k∣y1:k)=q(x0:k−1∣y1:k−1)q(xk∣x0:k−1,y1:k)
后验概率密度函数的递归形式可以表示为：
p
(
x
0
:
k
∣
Y
k
)
=
p
(
y
k
∣
x
0
:
k
,
Y
k
−
1
)
p
(
x
0
:
k
∣
Y
k
−
1
)
p
(
y
k
∣
Y
k
−
1
)
=
p
(
y
k
∣
x
0
:
k
,
Y
k
−
1
)
p
(
x
k
∣
x
0
:
k
−
1
,
Y
k
−
1
)
p
(
x
0
:
k
−
1
∣
Y
k
−
1
)
p
(
y
k
∣
Y
k
−
1
)
=
p
(
y
k
∣
x
k
)
p
(
x
k
∣
x
k
−
1
)
p
(
x
0
:
k
−
1
∣
Y
k
−
1
)
p
(
y
k
∣
Y
k
−
1
)
∝
p
(
y
k
∣
x
k
)
p
(
x
k
∣
x
k
−
1
)
p
(
x
0
:
k
−
1
∣
Y
k
−
1
)
(11)
\begin{aligned} p(x_{0:k}|Y_{k})&=\frac{p(y_{k}|x_{0:k},Y_{k-1})p(x_{0:k}|Y_{k-1})}{p(y_{k}|Y_{k-1})} \\ &=\frac{p(y_{k}|x_{0:k},Y_{k-1}) p(x_{k}|x_{0:k-1},Y_{k-1})p(x_{0:k-1}|Y_{k-1})}{p(y_{k}|Y_{k-1})} \\ &=\frac{p(y_{k}|x_{k}) p(x_{k}|x_{k-1})p(x_{0:k-1}|Y_{k-1})}{p(y_{k}|Y_{k-1})} \\ & \propto p(y_{k}|x_{k}) p(x_{k}|x_{k-1})p(x_{0:k-1}|Y_{k-1}) \end{aligned} \tag{11}
p(x0:k∣Yk)=p(yk∣Yk−1)p(yk∣x0:k,Yk−1)p(x0:k∣Yk−1)=p(yk∣Yk−1)p(yk∣x0:k,Yk−1)p(xk∣x0:k−1,Yk−1)p(x0:k−1∣Yk−1)=p(yk∣Yk−1)p(yk∣xk)p(xk∣xk−1)p(x0:k−1∣Yk−1)∝p(yk∣xk)p(xk∣xk−1)p(x0:k−1∣Yk−1)(11)
其中，为了表示方便，将
y
1
:
k
y_{1:k}
y1:k用
Y
k
Y_{k}
Yk表示。同时，式(11)与贝叶斯滤波中后验概率的推导是一样的，只是贝叶斯滤波中的
x
k
x_k
xk变成了这里的
x
0
:
k
x_{0:k}
x0:k，正因为这个不同，才使得贝叶斯滤波里需要积分，而这里的后验概率的分解形式则不用积分。
粒子权重的递归形式可以表示为：
w
k
(
i
)
∝
p
(
x
0
:
k
(
i
)
∣
Y
k
)
q
(
x
0
:
k
(
i
)
∣
Y
k
)
=
p
(
y
k
∣
x
k
(
i
)
)
p
(
x
k
(
i
)
∣
x
k
−
1
(
i
)
)
p
(
x
0
:
k
−
1
(
i
)
∣
Y
k
−
1
)
q
(
x
k
(
i
)
∣
x
0
:
k
−
1
(
i
)
,
Y
k
)
q
(
x
0
:
k
−
1
(
i
)
∣
Y
k
−
1
)
=
w
k
−
1
(
i
)
p
(
y
k
∣
x
k
(
i
)
)
p
(
x
k
(
i
)
∣
x
k
−
1
(
i
)
)
q
(
x
k
(
i
)
∣
x
0
:
k
−
1
(
i
)
,
Y
k
)
(12)
\begin{aligned} w_{k}^{(i)} & \propto \frac{p(x_{0:k}^{(i)}|Y_{k})}{q(x_{0:k}^{(i)}|Y_{k})} \\ &=\frac{p(y_{k}|x_{k}^{(i)})p(x_{k}^{(i)}|x_{k-1}^{(i)})p(x_{0:k-1}^{(i)}|Y_{k-1})}{q(x_{k}^{(i)}|x_{0:k-1}^{(i)},Y_{k})q(x_{0:k-1}^{(i)}|Y_{k-1})} \\ &=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_{k})} \end{aligned} \tag{12}
wk(i)∝q(x0:k(i)∣Yk)p(x0:k(i)∣Yk)=q(xk(i)∣x0:k−1(i),Yk)q(x0:k−1(i)∣Yk−1)p(yk∣xk(i))p(xk(i)∣xk−1(i))p(x0:k−1(i)∣Yk−1)=wk−1(i)q(xk(i)∣x0:k−1(i),Yk)p(yk∣xk(i))p(xk(i)∣xk−1(i))(12)
注意，这种权重递推形式的推导是在式(8)的形式下进行推导出来的，也就是没有归一化。而式(10)这个公式中的权重是归一化以后进行推导的，所以在实际应用中，递推计算出 w k w_{k} wk，才能够带入式(10)中去计算期望。
同时，式(12)中的分子看起来是不是挺熟悉的，因为在第一部分贝叶斯滤波中已经做了介绍， p ( y k ∣ x k ( i ) ) p(y_{k}|x_{k}^{(i)}) p(yk∣xk(i))和 p ( x k ( i ) ∣ x k − 1 ( i ) ) p(x_{k}^{(i)}|x_{k-1}^{(i)}) p(xk(i)∣xk−1(i))的概率分布形状和状态方程中噪声的概率分布形状是一样的，只是均值不同。
因此，递推形式的式(12)与前面的非递推形式相比，公式里的概率都是已知的，权重的计算已经没有编程方面的难度了。权重的计算方法有了以后，只要进行稍微的总结就可以得到序贯重要性采样滤波(SIS Filter)。
四、Sequential Important Sampling(SIS) Filter
在实际应用中，我们假设重要性分布
q
(
)
q()
q()满足：
q
(
x
k
∣
x
0
:
k
−
1
,
y
1
:
k
)
=
q
(
x
k
∣
x
k
−
1
,
y
k
)
(13)
q(x_{k}|x_{0:k-1},y_{1:k})=q(x_{k}|x_{k-1},y_{k}) \tag{13}
q(xk∣x0:k−1,y1:k)=q(xk∣xk−1,yk)(13)
这个假设说明，重要性分布只和前一时刻的状态
x
k
−
1
x_{k-1}
xk−1以及当前的测量量
y
k
y_{k}
yk有关，那么公式(12)就可以转化为：
w
k
(
i
)
∝
w
k
−
1
(
i
)
p
(
y
k
∣
x
k
(
i
)
)
p
(
x
k
(
i
)
∣
x
k
−
1
(
i
)
)
q
(
x
k
(
i
)
∣
x
k
−
1
(
i
)
,
y
k
)
(14)
w_{k}^{(i)} \propto 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{14}
wk(i)∝wk−1(i)q(xk(i)∣xk−1(i),yk)p(yk∣xk(i))p(xk(i)∣xk−1(i))(14)
下面整理一下整个算法的伪代码：
[
{
x
k
(
i
)
,
w
k
(
i
)
}
i
=
1
N
]
=
S
I
S
(
{
x
k
(
i
)
,
w
k
(
i
)
}
i
=
1
N
,
Y
k
)
F
o
r
i
=
1
:
N
(
1
)
采样：
x
k
(
i
)
∼
q
(
x
k
(
i
)
∣
x
k
−
1
(
i
)
,
y
k
)
;
(
2
)
根据
w
k
(
i
)
∝
w
k
−
1
(
i
)
p
(
y
k
∣
x
k
(
i
)
)
p
(
x
k
(
i
)
∣
x
k
−
1
(
i
)
)
q
(
x
k
(
i
)
∣
x
k
−
1
(
i
)
,
y
k
)
递推计算各个例子的权重
;
E
n
d
F
o
r
粒子权值归一化
\begin{align*} &[\{ x_{k}^{(i)}, w_{k}^{(i)} \}_{i=1}^{N}]=SIS(\{ x_{k}^{(i)}, w_{k}^{(i)} \}_{i=1}^{N},Y_{k}) \\ & \enspace \\ &For \enspace i=1:N \\ &(1)采样：x_{k}^{(i)} \sim q(x_{k}^{(i)}|x_{k-1}^{(i)},y_k); \\ &(2)根据 w_{k}^{(i)} \propto 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})} 递推计算各个例子的权重; \\ & End For \\ & \enspace \\ &粒子权值归一化 \end{align*}
[{xk(i),wk(i)}i=1N]=SIS({xk(i),wk(i)}i=1N,Yk)Fori=1:N(1)采样：xk(i)∼q(xk(i)∣xk−1(i),yk);(2)根据wk(i)∝wk−1(i)q(xk(i)∣xk−1(i),yk)p(yk∣xk(i))p(xk(i)∣xk−1(i))递推计算各个例子的权重;EndFor粒子权值归一化
现在粒子有了，粒子的权重也有了，就可以由式(10)对每个粒子的状态进行加权去估计目标的状态了。
目前这个算法就是粒子滤波的前身了，只是在实际应用中，又发现了很多的问题，比如说，粒子权重退化的问题，因此就有了重采样，也就有了基本的粒子滤波算法，另一个问题就是重要性概率密度 q ( ) q() q()的选择问题等。
五、重采样
在应用SIS滤波的过程中，存在一个权重退化的问题，就是经过了几次迭代后，很多粒子的权重都变得很小，可以忽略了，只有少数粒子的权重比较大，并且随着粒子权值的方差随着时间的增长，状态空间的有效粒子数较少。随着无效采样粒子数目的增加，使得大量的计算浪费在对估计后验滤波概率分布几乎起不到作用的粒子上，使得估计性能下降，如图所示：
通常采用有效粒子数目来衡量粒子权值的退化程度，即：
N
e
f
f
=
N
/
[
1
+
v
a
r
(
w
k
∗
(
i
)
)
]
w
k
∗
(
i
)
=
p
(
x
k
(
i
)
∣
y
1
:
k
)
q
(
x
k
(
i
)
∣
x
k
−
1
(
i
)
,
y
1
:
k
)
(15)
\begin{aligned} N_{eff}&=N/[1+var(w_{k}^{*(i)})] \\ w_{k}^{*(i)}&=\frac{p( x_{k}^{(i)}|y_{1:k})}{q(x_{k}^{(i)}|x_{k-1}^{(i)},y_{1:k})} \end{aligned} \tag{15}
Neffwk∗(i)=N/[1+var(wk∗(i))]=q(xk(i)∣xk−1(i),y1:k)p(xk(i)∣y1:k)(15)
这个公式的含义是，有效粒子数越小，即权重的方差越大，也就是说权重大和权重小的之间差距大，表明权值退化越严重。
在实际计算中，有效粒子数可以近似为：
N
^
e
f
f
≈
1
∑
i
=
1
N
[
w
k
(
i
)
]
2
(16)
\hat{N}_{eff} \approx \frac{1}{\displaystyle \sum_{i=1}^{N}[w_{k}^{(i)}]^{2}} \tag{16}
N^eff≈i=1∑N[wk(i)]21(16)
在进行序贯重要性采样时，若上式小于事先设定的某一阈值，则应当采取一些措施加以控制。克服序贯重要性采样权值退化现象最直接的方法就是增加粒子数，而这会造成计算量的相应增加，影响计算的实时性。
因此，一般采用一下两种途径：
- 选择合适的重要性概率密度函数；
- 在序贯重要性采样之后，采用重采样方法。
对于第一种方法：选取重要性概率密度函数的一个标准就是使得粒子权值的方差最小。这部分内容，还是推荐百度文库的那篇文章《粒子滤波理论》，这里就不赘述了。
这里重点讲第二种方法，重采样。重采样的思路是，既然那些权重小的粒子不起作用，那就直接舍弃掉，要保持粒子的数目不变，得用一些新的粒子来取代他们。找新粒子最简单的方法就是将权重大的粒子多复制几个出来，至于复制了几个？那就在权重大的粒子里面让它们根据自己权重所占的比例去分配，也就是权重最大的复制的最多，权重第二大的复制的第二多，以此类推。
下面以数学的形式来进行说明。前面已经说明了求某种期望问题变成了这种加权和的形式：
p
(
x
k
∣
y
1
:
k
)
=
∑
i
=
1
N
w
k
(
i
)
δ
(
x
k
−
x
k
(
i
)
)
(17)
p(x_k|y_{1:k})=\displaystyle \sum_{i=1}^{N}w_{k}^{(i)}\delta(x_{k}-x_{k}^{(i)}) \tag{17}
p(xk∣y1:k)=i=1∑Nwk(i)δ(xk−xk(i))(17)
通过采样以后，希望可以表示成
p
^
(
x
k
∣
y
1
:
k
)
=
∑
j
=
1
N
1
N
δ
(
x
k
−
x
k
(
j
)
)
=
∑
i
=
1
N
n
i
N
δ
(
x
k
−
x
k
(
j
)
)
(18)
\hat{p}(x_k|y_{1:k})=\displaystyle \sum_{j=1}^{N} \frac{1}{N} \delta(x_{k}-x_{k}^{(j)})=\displaystyle \sum_{i=1}^{N} \frac{n_i}{N} \delta(x_{k}-x_{k}^{(j)}) \tag{18}
p^(xk∣y1:k)=j=1∑NN1δ(xk−xk(j))=i=1∑NNniδ(xk−xk(j))(18)
注意对比式(17)和式(18)，式中
x
k
(
i
)
x_{k}^{(i)}
xk(i)是第
k
k
k时刻的粒子，
x
k
(
j
)
x_{k}^{(j)}
xk(j)是第
k
k
k时刻重采样以后的粒子，
n
i
n_{i}
ni是指粒子
x
k
(
i
)
x_{k}^{(i)}
xk(i)在产生新的粒子集
x
k
(
j
)
x_{k}^{(j)}
xk(j)时被复制的次数。
式(18)中第一个等号说明重采样以后，所有的粒子权重一样，都是
1
N
\frac{1}{N}
N1，只是有的粒子多出现了
n
i
n_{i}
ni次。
思路有了，就看具体的操作方法了。在《On resampling algorithms for particle filters》 这篇paper里讲了四种重采样的方法。这四种方法大同小异。如果你接触过遗传算法的话，理解起来就很容易，就是遗传算法中那种轮盘赌的思想。
在这里用个简单的例子来说明：
假设有3个粒子，在第 k k k时刻的时候，他们的权重分别是0.1，0.1，0.8，然后计算他们的概率累计和（在MATLAB中使用函数cumsum()）得到：[0.1,0.2,1.0]。接着我们用服从[0,1]之间的均匀分布随机采样3个值，假设为0.15,0.38和0.54，也就是说，第二个粒子被复制了一次，第三个粒子被复制了两次。
在MATLAB中一句命令就可以方便的实现这个过程：
[~, j] = histc(rand(N,1), [0 cumsum(w')]);
对于上面的过程，可以对着下面的图进行加深理解：
将重采样的方法结合之前的SIS算法，便形成了基本的粒子滤波算法。
标准的粒子滤波算法流程为：
- 粒子集初始化， k = 0 k=0 k=0：对于 i = 1 , 2 , … , N i=1,2,\ldots ,N i=1,2,…,N，由先验 p ( x 0 ) p(x_0) p(x0)生成采样粒子 { x 0 ( i ) } i = 1 N \{ x_{0}^{(i)} \}_{i=1}^{N} {x0(i)}i=1N
- 对于 k = 1 , 2 , … k=1,2,\ldots k=1,2,…，循环执行以下步骤：
-
重要性采样：对于 i = 1 , 2 , … , N i=1,2,\ldots ,N i=1,2,…,N，从重要性概率密度中生成采样粒子 { x ~ k ( i ) } i = 1 N \{ \tilde{x}_{k}^{(i)} \}_{i=1}^{N} {x~k(i)}i=1N，计算粒子权值 w ~ k ( i ) \tilde{w}_{k}^{(i)} w~k(i)，并进行归一化；
-
重采样：对粒子集 { x ~ k ( i ) , w ~ k ( i ) } \{ \tilde{x}_{k}^{(i)},\tilde{w}_{k}^{(i)} \} {x~k(i),w~k(i)}进行重采样，重采样后的粒子集为 { x k ( i ) , 1 N } \{ x_{k}^{(i)},\frac{1}{N} \} {xk(i),N1}；
-
输出：计算 k k k时刻的状态估计值： x ^ k = ∑ i = 1 N x ~ k ( i ) w ~ k ( i ) \hat{x}_{k}=\displaystyle\sum_{i=1}^{N}\tilde{x}_{k}^{(i)}\tilde{w}_{k}^{(i)} x^k=i=1∑Nx~k(i)w~k(i)。
重采样的思路很简单，但是仔细分析权重的计算公式时：
W
k
(
x
k
)
=
p
(
y
1
:
k
∣
x
k
)
p
(
x
k
)
q
(
x
k
∣
y
1
:
k
)
∝
p
(
x
k
∣
y
1
:
k
)
q
(
x
k
∣
y
1
:
k
)
W_{k}(x_k)=\frac{p(y_{1:k}|x_{k})p(x_k)}{q(x_{k}|y_{1:k})} \propto \frac{p(x_k|y_{1:k})}{q(x_{k}|y_{1:k})}
Wk(xk)=q(xk∣y1:k)p(y1:k∣xk)p(xk)∝q(xk∣y1:k)p(xk∣y1:k)
会有疑问，权重大的就多复制几次，这一定是正确的吗？权重大，如果是分子大，即后验概率大，那说明确实应该在后验概率大的地方多放几个粒子。但权重大也有可能是分母小造成的，这时候的分子也可能小，也就是实际的后验概率也可能小，这时候的大权重可能就没那么优秀了。何况，这种简单的重采样会使得粒子的多样性丢失，到最后可能都变成了只剩一种粒子的分身。在遗传算法中好歹还引入了变异来解决多样性的问题。当然，粒子滤波里也有专门的方法：正则粒子滤波，有兴趣的可以查阅相关资料。
至此，整个粒子滤波的流程已经清晰明朗了，在实际应用中还有一些不确定的就是重要性概率密度的选择。在下一章中，首先引出 SIR 粒子滤波，接着用SIR滤波来进行实践应用。
六、Sampling Importance Resampling Filter (SIR)
SIR滤波器很容易由前面的基本粒子滤波推导出来，只要对粒子的重要性概率密度函数做出特定的选择即可。
在SIR中，选取
p
(
x
k
∣
x
k
−
1
)
p(x_{k}|x_{k-1})
p(xk∣xk−1)为先验概率（在第一部分贝叶斯滤波预测部分中已经说过如何得到这个先验），将这个式子带入到SIS推导出的权重公式（14）中，可以得到
w
k
(
i
)
∝
w
k
−
1
(
i
)
p
(
y
k
∣
x
k
(
i
)
)
(19)
w_{k}^{(i)} \propto w_{k-1}^{(i)} p(y_{k}|x_{k}^{(i)}) \tag{19}
wk(i)∝wk−1(i)p(yk∣xk(i))(19)
另外，由之前的重采样我们知道，实际上每次重采样以后，有
w
k
−
1
(
i
)
=
1
N
w_{k-1}^{(i)}=\frac{1}{N}
wk−1(i)=N1，所以式(19)可以进一步简化为：
w
k
(
i
)
∝
1
N
p
(
y
k
∣
x
k
(
i
)
)
w_{k}^{(i)} \propto \frac{1}{N} p(y_{k}|x_{k}^{(i)})
wk(i)∝N1p(yk∣xk(i))
但是这里又引出来了一个概率采样 p ( y k ∣ x k ( i ) ) p(y_{k}|x_{k}^{(i)}) p(yk∣xk(i))，实际怎么得到这个概率，程序里面又怎么采样呢？
先搞清楚这个概率 p ( y k ∣ x k ( i ) ) p(y_{k}|x_{k}^{(i)}) p(yk∣xk(i))的含义，其意义为在状态 x x x出现的条件下，测量出 y y y的概率。在机器人定位里面就是，在机器人处于位姿 x x x时，此时传感器数据 y y y出现的概率。举个更简单的粒子，要找到一个年龄是14岁的男孩(状态 x x x)，身高为170cm(测量量)的概率。由系统状态方程可知，测量值就是在真实值附近添加了一个高斯噪声。因此，测量量 y y y的分布就是以真实测量值为均值，以噪声方差为方差的一个高斯分布。
因此，权重的采样过程就是：当粒子处于
x
x
x状态时，能够得到该粒子的测量量
y
y
y。要知道这个测量量
y
y
y出现的概率，就只要把它放到以真实值为均值，噪声方差为方差的高斯分布里去计算就行了：
w
=
η
(
2
π
∑
)
−
1
/
2
exp
{
−
1
2
(
y
t
r
u
e
−
y
)
∑
−
1
(
y
t
r
u
e
−
y
)
}
(20)
w=\eta (2\pi \sum)^{-1/2}\exp \{ -\frac{1}{2}(y_{true}-y)\sum^{-1}(y_{true}-y) \} \tag{20}
w=η(2π∑)−1/2exp{−21(ytrue−y)∑−1(ytrue−y)}(20)
到这里，就可以看成SIR滤波只和系统状态方程有关了，不用自己另外去设计概率密度函数，所以在很多程序中都是用这种方法。
下面以伪代码的形式给出SIR滤波器：
---------------------------------------------------------------------------------------------------------------------------
[
{
x
k
(
i
)
,
w
k
(
i
)
}
i
=
1
N
]
=
S
I
R
[
{
x
k
(
i
)
,
w
k
(
i
)
}
i
=
1
N
,
y
k
]
[\{ x_{k}^{(i)},w_{k}^{(i)} \}_{i=1}^{N}]=SIR[\{ x_{k}^{(i)},w_{k}^{(i)} \}_{i=1}^{N},y_{k}]
[{xk(i),wk(i)}i=1N]=SIR[{xk(i),wk(i)}i=1N,yk]
- FOR i=1:N
（1）采样粒子： x k ( i ) ∼ p ( x k ∣ x k − 1 ( i ) ) x_{k}^{(i)} \sim p(x_{k}|x_{k-1}^{(i)}) xk(i)∼p(xk∣xk−1(i))
（2）计算粒子权重： w k ( i ) = p ( y k ∣ x k ( i ) ) w_{k}^{(i)}=p(y_{k}|x_{k}^{(i)}) wk(i)=p(yk∣xk(i)) - End FOR
- 计算粒子权重和， t = s u m ( w ) t=sum(w) t=sum(w)
- 对每个粒子，用上面的权重和进行归一化 w = w / t \enspace w=w/t w=w/t
- 现在有了粒子，也有了粒子权重，进行状态估计
- 重采样
---------------------------------------------------------------------------------------------------------------------------
在上面的算法中，每进行一次，都必须重采样一次，这是由于权重的计算方式决定的。
分析上面算法中的采样，发现它并没有加入测量量 y ( k ) y(k) y(k)。只是凭先验知识 p ( x k ∣ x k − 1 ) p(x_{k}|x_{k-1}) p(xk∣xk−1)进行的采样，而不是用的修正了的后验概率，所以SIR算法存在效率不高和对奇异点敏感的问题，但是不管怎样，SIR确实简单实用。
七、粒子滤波的应用
这里主要以下面的状态方程为例子进行演示：
{
x
k
=
x
k
−
1
2
+
25
x
k
−
1
1
+
x
k
−
1
2
+
8
cos
[
1.2
(
k
−
1
)
]
+
v
k
y
k
=
x
k
2
20
+
n
k
\begin{cases} x_{k}&=\frac{x_{k-1}}{2}+\frac{25x_{k-1}}{1+x_{k-1}^{2}}+8\cos[1.2(k-1)] +v_{k} \\ y_{k}&=\frac{x_{k}^{2}}{20}+n_{k} \end{cases}
{xkyk=2xk−1+1+xk−1225xk−1+8cos[1.2(k−1)]+vk=20xk2+nk
在这个存在过程噪声和测量噪声的系统中，估计系统状态
x
k
x_{k}
xk。
使用的MATLAB进行的仿真。
%% SIR粒子滤波的应用，算法流程参见博客http://blog.csdn.net/heyijia0327/article/details/40899819
clear all
close all
clc
%% initialize the variables
x = 0.1; % initial actual state
x_N = 1; % 系统过程噪声的协方差 (由于是一维的，这里就是方差)
x_R = 1; % 测量的协方差
T = 75; % 共进行75次
N = 100; % 粒子数，越大效果越好，计算量也越大
%initilize our initial, prior particle distribution as a gaussian around
%the true initial value
V = 2; %初始分布的方差
x_P = []; % 粒子
% 用一个高斯分布随机的产生初始的粒子
for i = 1:N
x_P(i) = x + sqrt(V) * randn;
end
z_out = [x^2 / 20 + sqrt(x_R) * randn]; %实际测量值
x_out = [x]; %the actual output vector for measurement values.
x_est = [x]; % time by time output of the particle filters estimate
x_est_out = [x_est]; % the vector of particle filter estimates.
for t = 1:T
x = 0.5*x + 25*x/(1 + x^2) + 8*cos(1.2*(t-1)) + sqrt(x_N)*randn;
z = x^2/20 + sqrt(x_R)*randn;
for i = 1:N
%从先验p(x(k)|x(k-1))中采样
x_P_update(i) = 0.5*x_P(i) + 25*x_P(i)/(1 + x_P(i)^2) + 8*cos(1.2*(t-1)) + sqrt(x_N)*randn;
%计算采样粒子的值，为后面根据似然去计算权重做铺垫
z_update(i) = x_P_update(i)^2/20;
%对每个粒子计算其权重，这里假设量测噪声是高斯分布。所以 w = p(y|x)对应下面的计算公式
P_w(i) = (1/sqrt(2*pi*x_R)) * exp(-(z - z_update(i))^2/(2*x_R));
end
% 归一化.
P_w = P_w./sum(P_w);
%% Resampling这里没有用博客里之前说的histc函数，不过目的和效果是一样的
for i = 1 : N
x_P(i) = x_P_update(find(rand <= cumsum(P_w),1)); % 粒子权重大的将多得到后代
end % find( ,1) 返回第一个 符合前面条件的数的 下标
%状态估计，重采样以后，每个粒子的权重都变成了1/N
x_est = mean(x_P);
% Save data in arrays for later plotting
x_out = [x_out x];
z_out = [z_out z];
x_est_out = [x_est_out x_est];
end
t = 0:T;
figure(1);
clf
plot(t, x_out, '.-b', t, x_est_out, '-.r','linewidth',3);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time step'); ylabel('flight position');
legend('True flight position', 'Particle filter estimate');
最后，再次奉上文章的链接http://blog.csdn.net/heyijia0327。向大佬膜拜。
1394
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
