粒子滤波算法源于 Montecarlo 的思想，即以某事件出现的频率来指代该事件的
概率。因此在滤波过程中，需要用到概率如 P(x)的地方，一概对变量 x 采样，
以大量采样的分布近似来表示 P(x)。因此，采用此一思想，在滤波过程中粒子
滤波可以处理任意形式的概率，而不像 Kalman 滤波只能处理高斯分布的概率
问题。他的一大优势也在于此。
再来看对任意如下的状态方程
x(t)为 t 时刻状态
RRRRRRR
x(t)=f(x(t-1),u(t),w(t))(1)状态转移方程，u(t)为控制量，
w(t)为模型噪声
y(t)=h(x(t),e(t))(2)观测方程，e(t)为观测噪声
其中的 x(t)为 t 时刻状态，u(t)为控制量，w(t)和 e(t)分别为模型噪声和观测噪声。
(1)当然是状态转移方程，(2)是观测方程。那么对于这么一个问题粒子滤波怎么
来从观测 y(t)，和 x(t-1),u(t)滤出真实状态 x(t)呢？
看看滤波的预估阶段：粒子滤波首先根据 x(t-1)和他的概率分布生成大量的采
样，这些采样就称之为粒子。那么这些采样在状态空间中的分布实际上就是
x(t-1)的概率分布了。好，接下来依据状态转移方程加上控制量可以对每一粒子
得到一个预测粒子。所有的预测粒子就代表了涉及哪些参数化的东西）。
进入校正阶段来：有了预测粒子，当然不是所有的预测粒子都能得到我们的时
间观测值 y，越是接近真实状态的粒子，当然获得越有可能获得观测值 y。于是
对所有的粒子得有个评价了，这个评价就是一个条件概率 P(y|xi),直白的说，这
个条件概率代表了假设真实状态 x(t)取第 i 个粒子 xi 时获得观测 y 的概率。令这
个条件概率为第 i 个粒子的权重。对所有粒子都进行这么一个评价，那么越有
可能获得观测 y 的粒子，当然获得的权重越高。好了预测信息融合在粒子的分
布中，观测信息又融合在了每一粒子的权重中。
最后采用重采样算法，去除低权值的粒子，复制高权值的粒子。所得当然是需
要的真实状态 x(t)了，而这些重采样后的粒子，就代表了真实状态的概率分布
了。
下一轮滤波，再将重采样过后的粒子集输入到状态转移方程中，直接就能够获
得预测粒子了。
初始状态的问题：可以认为 x(0)在全状态空间内平均分布。于是初始采样就平
均分布在整个状态空间中。然后将所有采样输入状态转移方程，得到预测粒子。
再评价下所有预测粒子的权重，当然我们在整个状态空间中只有部分粒子能够
获的高权值。重采样算法去除低权值的，将下一轮滤波的考虑重点缩小到高权
值粒子附近。
下面是我最后改写和精简的一个粒子滤波 Matlab 算法。
评论0