本文介绍了概率机器人中的粒子滤波方法，首先阐述了概率分布的两种描述方式，然后讲解了非线性贝叶斯滤波的基础，包括系统方程和预测更新过程。接着，引入了蒙特卡洛积分、重要性采样以及序列化重要性采样（SIS），讨论了如何确定采样点位置和权重。接着，文章详细探讨了Condensation Filter，分析了其最优解和次优解。最后，解释了重采样在避免粒子退化中的作用，以及粒子滤波的完整算法流程。
文章目录
1 前言
-
在介绍粒子滤波之前大家要知道一个重要观念：一个概率分布可以如何描述？
- 可以通过概率密度函数(pdf)函数，表示某一区间的概率就是概率密度函数在这一区间的积分
- 还可以通过离散的点的位置和权重，表示某一区间的概率就是这一区间上点的位置与权重加权平均和
-
用概率密度函数(pdf)函数表示一个分布: 高斯分布 N ( 0 , 4 ) N(0, 4) N(0,4), 求这个分布在 [ 0 , 4 ] [0,4] [0,4]上的概率：
p ( x ) = 1 σ 2 π e − ( x − μ ) 2 2 σ 2 = 1 2 2 π e − x 2 8 ↓ p 0 : 4 = ∫ 0 4 p ( x ) d x = ∫ 0 4 1 2 2 π e − x 2 8 d x ≈ 0.48 \begin{aligned} p(x) &= \frac{1}{\sigma\sqrt{2 \pi }} e^{\frac{-(x - \mu)^2}{2\sigma^2}}\\ & = \frac{1}{2\sqrt{2 \pi}} e^{\frac{-x^2}{8}}\\ \end{aligned}\\ \downarrow\\ p_{0:4} = \int_0^4 p(x) dx = \int_0^4 \frac{1}{2\sqrt{2 \pi}} e^{\frac{-x^2}{8}} dx \approx 0.48 p(x)=σ2π1e2σ2−(x−μ)2=22π1e8−x2↓p0:4=∫04p(x)dx=∫0422π1e8−x2dx≈0.48
-
用离散的点的位置和权重表示一个分布: 高斯分布 N ( 0 , 4 ) N(0, 4) N(0,4), 求这个分布在 [ 0 , 4 ] [0,4] [0,4]上的概率：
散点的位置： x k = − 7.0 , − 6.9 , − 6.8 , ⋯ , 0 , 0.1 , 0.2 , ⋯ , 6.8 , 6.9 , 7.0 散点的权重： ω k = p ( x k ) = 1 2 2 π e − x k 2 8 = ⋯ ↓ p 0 : 4 = 4 n ∑ x k = 0 4 w k = 4 40 ∑ x k = 0 4 w k = 0.49 \text{散点的位置：}x_k = -7.0, -6.9, -6.8, \cdots, 0, 0.1, 0.2, \cdots,6.8, 6.9, 7.0\\ \text{散点的权重：} \omega_k = p(x_k) = \frac{1}{2\sqrt{2 \pi}} e^{\frac{-x_k^2}{8}} = \cdots\\ \downarrow\\ p_{0:4} = \frac{4}{n}\sum_{x_k=0}^4 w_k = \frac{4}{40}\sum_{x_k=0}^4 w_k = 0.49 散点的位置：xk=−7.0,−6.9,−6.8,⋯,0,0.1,0.2,⋯,6.8,6.9,7.0散点的权重：ωk=p(xk)=22π1e8−xk2=⋯↓p0:4=n4xk=0∑4wk=404xk=0∑4wk=0.49
-
大家比较一下一个是积分计算，一个是权重求和，但是都以得出近似的结果，而且上述散点越多，结果与积分的误差越小
-
这里透露一下，上述的散点其实就是粒子滤波的粒子
-
再说一下，介绍上面的内容是为了说明离散的粒子的位置+权重可以表示一个概率密度函数，需要强调的是一个位置有1个粒子，权重为0.5；与一个相同位置有2个粒子，权重均为0.25；这两种情况对于计算概率和分析来说是等价的，这就是粒子滤波中重采样的思想
-
下面说一下本片博客的介绍流程：
- 因为任何滤波都是基于贝叶斯滤波的，所以先介绍非线性贝叶斯滤波
- 但是非线性贝叶斯滤波需要计算积分，有些积分函数是不存在的，所以介绍一种数值积分方法：蒙特卡洛积分
- 当原函数未知的时候，利用蒙特卡洛积分也无法计算积分，那么就引出重要性采样：即利用与原函数类似的分布代替原函数。
- 但是重要性采样面临一个问题如何确定粒子的位置和粒子的权重？这就引出序列化重要性采样
- 有了序列化重要性采样其实就有了粒子滤波的雏形
- 但是在实施的过程中存在粒子退化的现象，那么就引出重采样
- 其实粒子滤波 = 序列化重要性采样 + 重采样
- 这时仅有一个问题没有解决：与原函数类似的分布怎么确定？为了解决这个问题，就有了Condensation Filter,它是序列化重要性采样的一种方法。
- 最后用Condensation Filter代替序列化重要性采样就有了粒子滤波 = Condensation Filter + 重采样
2 非线性贝叶斯滤波
- 说到贝叶斯滤波前面已经介绍过：
- 详细推导和讲解过程见：贝叶斯滤
- 这里简单介绍一下主要部分
- 贝叶斯滤波的思想是：基于测量信号 z 1 : k z_{1:k} z1:k，评估 x k x_k xk
- 也就是计算置信度（条件概率）： p ( x k ∣ z 1 : k ) p(x_k | z_{1:k}) p(xk∣z1:k)
- 类似数学归纳法：
- 初始时刻： p ( x 0 ∣ z 0 ) = p ( x 0 ) p(x_0 | z_{0}) = p(x_0) p(x0∣z0)=p(x0)已知
- 已知后验概率： p ( x k − 1 ∣ z 1 : k − 1 ) p(x_{k-1}|z_{1:k-1}) p(xk−1∣z1:k−1)
- 计算后验概率： p ( x k ∣ z 1 : k ) p(x_{k}|z_{1:k}) p(xk∣z1:k)
- 在进行贝叶斯滤波前需要知道系统方程和预测方程：
系统方程： x k = g k ( x k − 1 , ϵ k − 1 ) ( ϵ k − 1 : 系统噪声 ) 测量方程： z k = h k ( x k , δ k ) ( δ k : 测量噪声 ) \text{系统方程：} x_k = g_k(x_{k-1}, \epsilon_{k-1}) \qquad (\epsilon_{k-1}: \text{系统噪声})\\ \text{测量方程：}z_k = h_k(x_k, \delta_k) \qquad (\delta_k: \text{测量噪声}) 系统方程：xk=gk(xk−1,ϵk−1)(ϵk−1:系统噪声)测量方程：zk=hk(xk,δk)(δk:测量噪声) - 滤波主要分为两个步骤：
- 已知后验概率： p ( x k − 1 ∣ z 1 : k − 1 ) p(x_{k-1}|z_{1:k-1}) p(xk−1∣z1:k−1)
- 根据系统方程进行预测：计算 p ( x k ∣ z 1 : k − 1 ) p(x_{k}|z_{1:k-1}) p(xk∣z1:k−1)
- 根据测量方程进行更新：计算 p ( x k ∣ z 1 : k ) p(x_{k}|z_{1:k}) p(xk∣z1:k)
- 那么具体怎么预测和更新呢？
-
- 预测计算
p ( x k ) = ∫ p ( x k , x k − 1 ) d x k − 1 = ∫ p ( x k ∣ x k − 1 ) p ( x k − 1 ) d x k − 1 ↓ p ( x k ∣ z 1 : k − 1 ) = ∫ p ( x k ∣ x k − 1 , z 1 : k − 1 ) p ( x k − 1 ∣ z 1 : k − 1 ) d x k − 1 ↓ p ( x k ∣ x k − 1 , z 1 : k − 1 ) = p ( x k ∣ x k − 1 ) = ∫ p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ z 1 : k − 1 ) d x k − 1 \begin{aligned} p(x_k) &= \int p(x_k, x_{k-1})dx_{k-1}\\ &= \int p(x_k|x_{k-1})p(x_{k-1})dx_{k-1} \end{aligned}\\ \downarrow\\ \begin{aligned} p(x_k|z_{1:k-1}) &= \int p(x_k|x_{k-1},z_{1:k-1})p(x_{k-1}|z_{1:k-1})dx_{k-1}\\ &\downarrow p(x_k|x_{k-1},z_{1:k-1})=p(x_k|x_{k-1})\\ &= \int p(x_k|x_{k-1})p(x_{k-1}|z_{1:k-1})dx_{k-1}\\ \end{aligned} p(xk)=∫p(xk,xk−1)dxk−1=∫p(xk∣xk−1)p(xk−1)dxk−1↓p(xk∣z1:k−1)=∫p(xk∣xk−1,z1:k−1)p(xk−1∣z1:k−1)dxk−1↓p(xk∣xk−1,z1:k−1)=p(xk∣xk−1)=∫p(xk∣xk−1)p(xk−1∣z1:k−1)dxk−1
- 预测计算
-
- 更新计算
p ( x k ∣ z 1 : k ) = p ( x k ∣ z k , z 1 : k − 1 ) = p ( z k ∣ x k , z 1 : k − 1 ) p ( x k ∣ z 1 : k − 1 ) p ( z k ∣ z 1 : k − 1 ) = p ( z k ∣ x k ) p ( x k ∣ z 1 : k − 1 ) p ( z k ∣ z 1 : k − 1 ) 分子为归一化常数： p ( z k ∣ z 1 : k − 1 ) = ∫ p ( z k ∣ x k , z 1 : k − 1 ) p ( x k ∣ z 1 : k − 1 ) d x k = ∫ p ( z k ∣ x k ) p ( x k ∣ z 1 : k − 1 ) d x k \begin{aligned} p(x_k|z_{1:k}) &= p(x_k|z_k,z_{1:k-1})\\ &= \frac{p(z_k|x_k,z_{1:k-1})p(x_k|z_{1:k-1})}{p(z_k|z_{1:k-1})}\\ &= \frac{p(z_k|x_k)p(x_k|z_{1:k-1})}{p(z_k|z_{1:k-1})}\\ \end{aligned}\\ \text{分子为归一化常数：} \begin{aligned} p(z_k|z_{1:k-1}) &= \int p(z_k|x_k,z_{1:k-1}) p(x_k|z_{1:k-1}) d x_k\\ &= \int p(z_k|x_k) p(x_k|z_{1:k-1}) d x_k \end{aligned} p(xk∣z1:k)=p(xk∣zk,z1:k−1)=p(zk∣z1:k−1)p(zk∣xk,z1:k−1)p(xk∣z1:k−1)=p(zk∣z1:k−1)p(zk∣xk)p(xk∣z1:k−1)分子为归一化常数：p(zk∣z1:k−1)=∫p(zk∣xk,z1:k−1)p(xk∣z1:k−1)dxk=∫p(zk∣xk)p(xk∣z1:k−1)dxk
- 更新计算
-
- 总结
预测： p ( x k ∣ z 1 : k − 1 ) = ∫ p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ z 1 : k − 1 ) d x k − 1 更新： p ( x k ∣ z 1 : k ) = p ( z k ∣ x k ) p ( x k ∣ z 1 : k − 1 ) p ( z k ∣ z 1 : k − 1 ) p ( z k ∣ z 1 : k − 1 ) = ∫ p ( z k ∣ x k ) p ( x k ∣ z 1 : k − 1 ) d x k \text{预测：}p(x_k|z_{1:k-1}) = \int p(x_k|x_{k-1})p(x_{k-1}|z_{1:k-1})dx_{k-1}\\ \text{更新：} \qquad p(x_k|z_{1:k}) = \frac{p(z_k|x_k)p(x_k|z_{1:k-1})}{p(z_k|z_{1:k-1})}\\ p(z_k|z_{1:k-1}) = \int p(z_k|x_k) p(x_k|z_{1:k-1}) d x_k 预测：p(xk∣z1:k−1)=∫p(xk∣xk−1)p(xk−1∣z1:k−1)dxk−1更新：p(xk∣z1:k)=p(zk∣z1:k−1)p(zk∣xk)p(xk∣z1:k−1)p(zk∣z1:k−1)=∫p(zk∣xk)p(xk∣z1:k−1)dxk
- 总结
- 上述计算概率需要积分计算，那么有些函数是很难积分的，所以下面介绍一种数值积分方法
3 蒙特卡洛积分
- 求函数 f ( x ) f(x) f(x)在区间 [ a , b ] [a,b] [a,b]上的积分 ∫ a b f ( x ) d x \int_a^b f(x) dx ∫abf(x)dx
- 如果函数 f ( x ) f(x) f(x)的解析积分形式很容易求得，那么积分 ∫ a b f ( x ) d x \int_a^b f(x) dx ∫abf(x)dx很简单
- 如果函数 f ( x ) f(x) f(x)的解析积分形式很难计算，那么积分 ∫ a b f ( x ) d x \int_a^b f(x) dx ∫abf(x)dx怎么算呢？
- 方法是我们在区间 [ a , b ] [a,b] [a,b]上均匀取 N N N个点 { x 1 , x 2 , ⋯ , x N } \{x_1, x_2, \cdots, x_N\} { x1,x2,⋯,xN},并计算这些点所对应的函数值 { f ( x 1 ) , f ( x 2 ) , ⋯ , f ( x N ) } \{f(x_1), f(x_2), \cdots, f(x_N)\} { f(x1),f(x2),
最低0.47元/天 解锁文章
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
