概率机器人总结——粒子滤波先实践再推导
如果仅仅是将《概率机器人》这本书上的推导过程看一遍的话，其实很难理解粒子滤波到底是个什么玩意儿的，像我的话还去看了徐亦达老师的讲解视频和白巧克力亦唯心的博客和一些其他资料，在稿纸上从头到尾推一遍之后才有了初步的一些认识，按道理来说，我们应该是先推到公式再进行实践，但是我连粒子滤波是神马都不知道的话我也没法好好推到，因此，这里我结合上面的视频和博客进行了一个简单的总结，先具体描述下粒子滤波是如何使用的，然后再简单介绍下我理解的推导过程，具体的推导过程大家可以参看上面两个链接的任何一个，我觉得都推得很棒！
推导之前，先来一波实践
整个推导过程其实是一个两头小中间大的过程，怎么说呢，就是从贝叶斯滤波开始，到最后Sampling Importance Resampling Filter(SIR)算法结束，中间推导过程其实主要就是应用了条件概率公式、贝叶斯概率公式、马尔科夫性、大数定理这样一些基础知识，其实我觉得在推导之前我们可以先看看到底什么是粒子滤波，也就是结果到底是什么。
伪代码分析
经典的SIR算法伪代码如下：
-----------------------SIR FILTER---------------------------------
[{xk(i),wk(i)}i=1N]=SIR[{xk−1(i),wk−1(i)}i=1N,yk]
遍历所有粒子执行如下操作：
（1）第一步：采样粒子xk(i)∼p(xk∣xk−1(i))
（2）第二步：计算粒子权重wk(i)=p(yk∣xk(i))
第三步：计算粒子的权重和t=sum(w)，然后粒子权重归一化w=w/t
第四步：进行状态估计∑i=1NW~k(xk(i))f(xk(i))
第五步：重采样
----------------------------END-------------------------------------
其中xi代表粒子（这个所谓的粒子其实就是我们观察对象可能存在的某一种状态，例如对于一台机器人而言，这个粒子可能就是表示一个具体的位置，速度，如[x,y,v,w]这样）而wi粒子权重，这里第一行指的是我们通过SIR算法从前一个状态过渡到下一个状态，怎么做呢?
先遍历所有的粒子，对于其中某一个粒子，我们的做法是：
第一步：采样粒子，先将这个粒子（刚刚提到了其实就是对象可能存在的某一种状态）代入到状态转移方程中，这个就是采样粒子的过程，这样其实就是获得了p(xk∣xk−1(i))分布的一种具体可能。
第二步：更新这个粒子的权重，例如这个p(yk∣xki)满足高斯分布（也可以理解为观测方程的噪声满足高斯分布），现在我们有了一个实际的观测值和一个通过状态转移方程获得的先验值，这中间肯定存在误差，然后这个误差又是满足高斯分布的，我们将这个实际存在的误差带入高斯分布的方程中就获得了权重w。
第三步：归一化权重
第四步：进行状态估计
第五步：进行粒子的重采样，第三步和第四步操作起来没什么难度的，具体为什么要归一化就得参照推导过程进行理解了，第五步的重采样过程是SIR算法中必须的操作，其实就是一个复制粒子的过程，但是这个复制是要根据已经存在的粒子的权重大小进行复制的，怎么能够根据不同粒子权重的不同选择谁复制得多谁复制得少呢？假设现在有三个粒子权重分别是[1,2,3]，我们先对其进行累加变成[1,3,6]，这样相当于形成了三个区间[0,1],[1,3],[3,6]，然后我们在这[0,6]这个区间内按照均匀分布进行随机采样，例如采样点为[1.2,3.4,5.8]，这三个点落在不同的区间内，因此第一个粒子不采样，第二个粒子采一次，第三个粒子采两次。这样就实现了按照权重进行重采样的效果，这里注意一个结论，重采样之后所有的粒子权重都变为1/N。
真代码解析
伪代码看完了我们来看下真代码加深理解，这个代码是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
其实注释已经写的很清楚了，就是五个步骤对应起来看就好了
下面是重采样函数的代码，很巧妙的～
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的环境下，效果如下：
红色的轨迹估计值，蓝色的轨迹（被红色盖住）是真实值，黑色的轨迹是没有观测的情况下的航迹值，效果很明显。
推导过程
因为在白巧克力亦唯心的Particle Filter Tutorial 粒子滤波：从推导到应用（一）这个系列中已经推导得非常详细了，我不想当一个搬运工，因此我在这里仅仅加入一些我的个人理解，详细的推到过程大家可以参看上述博客。
推到过程如下：
1. 贝叶斯滤波
贝叶斯滤波两步，分别是预测和更新，推导如下：
预测： 由p(xk−1∣y1:k−1)得到p(xk∣y1:k−1)，其实就是状态转移的过程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
更新： 由p(xk∣y1:k−1)得到p(xk∣y1:k)，其实就是观测方程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)
可以结合隐马尔科夫模型图理解（其实就是基于这个模型设计的）
假如上述的更新过程中的p(xk∣xk−1)不是一个不可积分的分布，则问题需要通过粒子滤波算法来解决
2. 蒙特卡罗采样
用蒙特卡洛采样来代替计算后验概率，只要从后验概率中采样很多粒子，用它们的状态求平均就得到了滤波结果。E[f(xn)]≈∫f(xn)p^(xn∣y1:k)dxn=N1i=1∑N∫f(xn)δ(xn−xn(i))dxn=N1i=1∑Nf(xn(i))
这一步就对应着上面实践过程中第四步（状态估计过程），解释了为什么这样做就可以估计状态。
3. 重要性采样
这个是最重要也是最复杂的推导过程，其解决的就是，之前描述的是我的后验分布积分过程较为难实现，在实际操作过程中我们可能连后验分布怎么表达都不知道，就没办法直接从后验分布中进行采样。因此我们需要从一个已知的分布中去采样，中间通过某一种变换的方式让其等价于在后验分布上采样，这就是重要性采样。
推导结果在已知分布q(xk∣y1:k)中采样xk(i)，状态估计变为：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))
其中，W~k(xk(i))=∑i=1NWk(xk(i))Wk(xk(i))而Wk(xk)=q(xk∣y1:k)p(y1:k∣xk)p(xk)∝q(xk∣y1:k)p(xk∣y1:k)
这里又会发现一个问题，这里面不还是有我们想要避免的p(xk∣y1:k)，上述工作不是白做了吗，当然不是，我们换成递推形式就可以避免：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∣x0k,Yk−1)p(xk∣x0k−1,Yk−1)p(x0k−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)
这里其实就是在后验概率上用了一个条件概率公式，接下里就有了重要性采样最后的结论：
wk(i)∝q(xk∣y1:k)p(xk∣y1:k)=q(xk(i)∣x0k−1(i),Yk)q(x0k−1(i)∣Yk−1)p(yk∣xk(i))p(xk(i)∣xk−1(i))p(x0k−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))
这里面p(xk∣xk−1)（状态转移）和p(yk∣xk)（观测）都是知道的呀，而且不涉及到积分之类的复杂过程，因此现在所有的分布都知道了，就可以进行顺畅的滤波了。
4. Sequential Importance Sampling (SIS) Filter
SIS的算法伪代码如下：
-----------------------SIR FILTER---------------------------------
[{xk(i),wk(i)}i=1N]=SIS({xk−1(i),wk−1(i)}i=1N,Yk)
遍历所有粒子执行如下操作：
（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))
第三步：粒子权重归一化
----------------------------END-------------------------------------
5. 重采样
重采样的目的就是就是为了解决下面这种情况：
由蓝色圆点变黄色圆点指的是采样的过程，粒子的位置或者说状态发生了变化，从黄色原点到蓝色圆点指的是权值更新的过程，上面这种情况就会使得大量计算浪费在对后验估计几乎不起作用的粒子上，因此需要重采样。重采样后的结果如下：p~(xk∣y1:k)=j=1∑NN1δ(xk−xk(j))=i=1∑NNniδ(xk−xk(i))
重采样的具体操作实践部分已经说明过了，不再赘述
6. Sampling Importance Resampling (SIR) Filter
在前面已经讲了SIR算法的具体流程，那么SIR算法是怎么来的呢，其实就是在SIS算法的基础上，取
q(xk(i)∣xk−1(i),yk)=p(xk(i)∣xk−1(i))
得到
wk(i)∝wk−1(i)p(yk∣xk(i))
然后每次都进行重采样之后，权重wk−1i=1/N，因此
wk(i)∝p(yk∣xk(i))
如上就变成了我们所见的在状态转移（方程）分布上采样，在观测（方程）分布上更新权值。
上面就是粒子滤波的实践和推到过程，全部推导出来还是很爽的，可能会有理解有误的地方，欢迎指正交流～