本节目录如下：
目录
1. 推导之前，先看实践
1.1 伪代码分析
1.2 代码解析
2. 推导过程
2.1 贝叶斯滤波
2.2 蒙特卡罗采样
2.3 重要性采样
2.4 Sequential Importance Sampling（SIS）Filter
2.5 重采样
2.6 Sampling Importance Resampling（SIR）Filter
1. 推导之前，先看实践
整个推导过程其实是一个两头小中间大的过程，怎么说呢，就是从贝叶斯滤波开始，到最后Sampling Importance Resampling Filter(SIR)算法结束，中间推导过程其实主要就是应用了条件概率公式、贝叶斯概率公式、马尔科夫性、大数定理这样一些基础知识，其实我觉得在推导之前我们可以先看看到底什么是粒子滤波，也就是结果到底是什么。
1.1 伪代码分析
经典的SIR算法伪代码如下：
遍历所有粒子执行如下操作：
（1）第一步：采样粒子
~
（2）第二步：计算粒子权重
=
（3）第三步：计算粒子的权重和 t = sum(w)，然后粒子权重归一化w = w/t
（4）第四步：进行状态估计
（5）第五步：重采样
--------------------------------------------------------END---------------------------------------------------------
其中
代表粒子（这个所谓的粒子其实就是我们观察对象可能存在的某一种状态），例如对于一台机器人而言，这个粒子可能就是表示一个具体的位置，速度，如[x,y,v,w]这样，而
为粒子权重，这里第一行指的是我们通过SIR算法从前一个状态过渡到下一个状态，怎么做呢？
先遍历所有的粒子，对于其中某一个粒子，我们的做法是：
第一步：采样粒子，先将这个粒子（刚刚提到了就是对象可能存在的某一种状态）代入到状态转移方程中，这个就是采样粒子的过程，这样其实就是获得了
分布的一种具体可能。 第二步：更新这个粒子的权重，例如这个
满足高斯分布（也可以理解为观测方程的噪声满足高斯分布），现在我们有了一个实际的观测值和一个通过状态转移方程获得的先验值，这中间肯定存在误差，然后这个误差又是满足高斯分布的，我们将这个实际存在的误差带入高斯分布的方程中就获得了权重w。
第三步：归一化权重
第四步：进行状态估计
第五步：进行粒子的重采样，
第三步和第四步操作起来没什么难度的，具体为什么要归一化就得参照推导过程进行理解了，第五步的重采样过程是SIR算法中必须的操作，其实就是一个复制粒子的过程，但是这个复制是要根据已经存在的粒子的权重大小进行复制的，怎么能够根据不同的粒子权重的不同选择谁复制得多谁复制得少呢？假设现在有三个粒子权重分别是[1, 2, 3]，我们先对其进行累加变成[1, 3, 6]，这样就相当于形成了三个区间[0, 1], [1, 3], [3, 6]，然后我们在这[0, 6]这个区间内按照均匀分布进行随机采样，例如采样点为[1.2, 3.4, 5.8]，这三个点落在不同的区间内，因此第一个粒子不采样，第二个粒子采一次，第三个粒子采两次。这样就实现了按照权重进行重采样的效果，这里注意一个结论，重采样之后所有的粒子权重都变为1/N。
1.2 代码解析
伪代码看文了我们来看真代码加深理解，这个代码是PythonRobotic这个项目里的，利用粒子滤波进行定位的代码，我对其进行了注释，下面我截取一些片段进行分析：
def pf_localization(px, pw, xEst, PEst, z, u): # 这里输入的分别是粒子，粒子的权值，估计的位置，带噪声的测量和带噪声的控制
"""
Localization with Particle filter
"""
for ip in range(NP): # 遍历所有的粒子
x = np.array([px[:, ip]]).T # 提取出第ip个粒子
w = pw[0, ip] # 提取出第ip个粒子的权重
# 【---第一步---粒子采样过程---P(x_k|x_k-1)---】
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0] # 这里加噪声的原因是实现随机采样（个人理解，这里不一定对）
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
ud = np.array([[ud1, ud2]]).T
x = motion_model(x, ud) # 粒子在带噪声的状态转移方程下更新
# 【---第二步---权值更新过程---P(y_k|x_k)---】
for i in range(len(z[:, 0])): # 遍历所有的测量
dx = x[0, 0] - z[i, 1]
dy = x[1, 0] - z[i, 2]
prez = math.sqrt(dx**2 + dy**2) # 这里计算我采样后的状态（位置）距离RFID有多远
dz = prez - z[i, 0] # 这里是测量的误差
w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0])) # 跟新权重
px[:, ip] = x[:, 0] # 更新粒子的状态
pw[0, ip] = w # 跟新粒子的权重
# 【---第三步---权值归一化过程---】
pw = pw / pw.sum() # 对权值进行归一化
# 【---第四步---状态估计过程---】
xEst = px.dot(pw.T) # 加权平均就获得了估计的位置
PEst = calc_covariance(xEst, px, pw) # 获得粒子的方差
# 【---第五步---重采样过程---】
px, pw = resampling(px, pw) # 重采样，SIR粒子滤波中必须有的步骤
return xEst, PEst, px, pw其中均值为0的高斯分布代码如下：
def gauss_likelihood(x, sigma):
p = 1.0 / math.sqrt(2.0 * math.pi * sigma ** 2) * \
math.exp(-x ** 2 / (2 * sigma ** 2))
return p公式如下：
其实上面注释写得很清楚了，五个步骤对应起来看就好了，下面是重采样函数的代码：
def resampling(px, pw):
"""
low variance re-sampling
"""
Neff = 1.0 / (pw.dot(pw.T))[0, 0] # 按照公式判定那些粒子是低效的
if Neff < NTh:
wcum = np.cumsum(pw) # 将权值累加起来，例如 [1,2,3] 在进行cumsum之后变成 [1,3,6]，这其实就是轮盘采样的方法
base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP # 同上理解，会形成一个阶梯状的数列
resampleid = base + np.random.rand(base.shape[0]) / NP # 加上噪声，形成均匀分布的随机采样值
inds = []
ind = 0
for ip in range(NP):
while resampleid[ip] > wcum[ind]:
ind += 1
inds.append(ind) # 这里存储的是冲采样后的id
px = px[:, inds] # 将id对应的粒子提取出来
pw = np.zeros((1, NP)) + 1.0 / NP # 所有的粒子的权值初始化为1/N
return px, pw然后代码下载下来是可以直接运行的，在python3的环境下，效果如下：
红色的轨迹估计值，蓝色的轨迹（被红色盖住）是真实值，黑色的轨迹是没有观测的情况下的航迹值，效果很明显。
2. 推导过程
因为白巧克力亦唯心的Particle Filter Tutorial 粒子滤波：从推导到应用（一）这个系列中已经推导得非常详细了。这里参考Jichao_Peng的理解，详细地推导过程大家可以参考上述两篇博客。
2.1 贝叶斯滤波
贝叶斯滤波两步，分别是预测和更新，推导如下：
预测： 由
得到
，其实就是状态转移的过程：
更新：由
得到
，其实就是观测方程
可以结合隐马尔科夫模型图理解（其实就是基于这个模型设计的）：
假如上述的更新过程中的
不是一个不可积分的分布，则问题需要通过粒子滤波算法来解决。
2.2 蒙特卡罗采样
用蒙特卡洛采样来代替计算后验概率，只要从后验概率中采样很多粒子，用它们的状态求平均就得到了滤波结果。
这一步就对应着上面时间过程中第四步（状态估计过程），解释了为什么这样做就可以估计状态。
2.3 重要性采样
这个最重要也是最复杂的推导过程，其解决的就是，之前描述的是我的后延分布积分过程较为难实现，在实际操作过程中我们可能连后验分布怎么表达都不知道，就没办法直接从后验分布中进行采样。因此我们需要从一个已知的分布中去采样，中间通过某一种变换的方式让其等价于在后验分布上采样，这就是重要性采样。
推导结果在已知分布
中采样
，状态估计变为：
其中，
而
这里又会发现一个问题，这里面不还是有我们想要避免的
，上述工作不是白做了吗，当然不是，我们换成递推形式就可以避免：
这里其实就是在后验概率上用了一个条件概率公式，接下里就有了重要性采样最后的结论：
这里面
（状态转移）和
观测都是知道的呀，而且不涉及到积分之类的复杂过程，因此现在所有的分布都知道了，酒可以进行顺畅的滤波了。
2.4 Sequential Importance Sampling（SIS）Filter
2.5 重采样
重采样的目的就是就是为了解决下面这种情况：
由蓝色圆点变黄色圆点指的是采样的过程，粒子的位置或者说状态发生了变化，从黄色原点到蓝色圆点指的是权值更新的过程，上面这种情况就会使得大量计算浪费在对后验估计几乎不起作用的粒子上，因此需要重采样。重采样后的结果如下：
重采样的具体操作实践部分已经说明过了，不再赘述。
2.6 Sampling Importance Resampling（SIR）Filter
在前面已经讲了SIR算法的具体流程，那么SIR算法是怎么来的呢，其实就是在SIS算法的基础上，取：
得到：
然后每次都进行重采样之后，权重
，因此
如上就变成了我们所见的在状态转移（方程）分布上采样，在观测（方程）分布上更新权值。
