粒子滤波原理
粒子滤波是基于蒙特卡洛仿真的近似贝叶斯滤波算法。
我们可以从贝叶斯滤波的过程来相应的给出粒子滤波的过程。
贝叶斯滤波公式推导分为两步，详细推导过程请见：崔岩的笔记——粒子滤波原理及应用（2）蒙特卡洛法与贝叶斯网络。
第一步为状态预测，即通过上一时刻的状态量和当前时刻的控制量预测当前时刻的状态量：
第二步为量测更新，即通过当前时刻的观测量来修正当前时刻状态量的预测量：
式中
代表归一化常数。
对照贝叶斯滤波给出粒子滤波算法：
①生成初代粒子
通过先验概率生成初代粒子N个，各粒子之间相互独立，各个粒子的初始状态量分布服从先验概率分布。
②对于每个粒子进行状态量更新
对比贝叶斯滤波中的状态预测步骤，我们对每一个粒子上一时刻状态量
按照控制方程
进行更新，得到
。
③对于每个粒子进行权重计算
对比贝叶斯滤波中的量测更新步骤，我们根据观测量
，结合观测方程
计算每个粒子的权重
，与贝叶斯滤波中的量测更新不同的是，我们在这一步不对粒子的状态量进行更新，而是引入一个新的参数——粒子权重，通过权重大小来表明各个粒子中，哪一个粒子的状态量估计值同真值更接近，其接近的程度又是多少。
④优胜劣汰
根据每个粒子的权重通过重采样步骤进行粒子的删除与复制，淘汰权重低的粒子，留下权重高的粒子。具体可以参考轮盘赌算法。
粒子重采样后需要对粒子权重进行重置，再依次循环状态更新、权重计算、优胜劣汰三步。最终会得到一个粒子集，我们通常把这个粒子集的状态量均值作为粒子滤波的结果。
在上述的整个过程中，我们使用粒子分布对概率进行表示、包括采样过程的理论依据就是蒙特卡洛法
粒子滤波算法流程
①初始化，t = 0
For i = 1:N，从先验分布
中抽取初始化状态
②For i = 1:T
（a）重要性采样阶段
For i = 1:N，采样
For i = 1:N，为每个粒子计算权重
For i = 1:N，归一化权重
（b）选择阶段（重采样）
根据归一化权值
的大小对粒子进行复制和淘汰For i = 1:N，重新设置权重
（c）输出
粒子滤波的输出为一组样本点，以样本点近似表示后验分布
计算均值
end
