本文从贝叶斯滤波出发，探讨其在概率机器人中的应用，介绍了贝叶斯滤波的求解过程，并指出其在处理大量可能情况时的缺点。接着引入卡尔曼滤波，通过简化假设，得出卡尔曼滤波的线性递推公式，并展示了一个简单的卡尔曼滤波在噪声数据处理中的应用实例。
背景介绍
假设我们的模型是这个样子的
{ x k = f ( x k − 1 , u k ) + w k 预 测 方 程 z k = h ( x k ) + v k 观 测 方 程 \begin{cases} \ x_k=f(x_{k-1},u_k)+w_k & 预测方程 \\ \ z_k=h(x_k)+v_k& 观测方程 \end{cases} {
xk=f(xk−1,uk)+wk zk=h(xk)+vk预测方程观测方程
已知条件有在0时刻的分布 x 0 x_0 x0,所有时刻的运动输入 u 1 : k u_{1:k} u1:k,所有时刻的观测 z 1 : k z_{1:k} z1:k，然后我们要计算在k时刻的 x k x_k xk的分布
P ( x k ∣ x 0 , u 1 : k , z 1 : k ) P(x_k|x_0,u_{1:k},z_{1:k}) P(xk∣x0,u1:k,z1:k)
求解过程（贝叶斯滤波）
- 将 P ( x k ∣ x 0 , u 1 : k , z 1 : k ) P(x_k|x_0,u_{1:k},z_{1:k}) P(xk∣x0,u1:k,z1:k)通过贝叶斯展开
P ( x k ∣ x 0 , u 1 : k , z 1 : k ) = P ( x k , x 0 , u 1 : k , z 1 : k ) P ( x 0 , u 1 : k , z 1 : k ) = P ( z k ∣ x k , x 0 , u 1 : k , z 1 : k − 1 ) P ( x k ∣ x 0 , u 1 : k , z 1 : k − 1 ) P ( x 0 , u 1 : k , z 1 : k − 1 ) P ( z k ∣ x 0 , u 1 : k , z 1 : k − 1 ) P ( x 0 , u 1 : k , z 1 : k − 1 ) = P ( z k ∣ x k , x 0 , u 1 : k , z 1 : k − 1 ) P ( x k ∣ x 0 , u 1 : k , z 1 : k − 1 ) P ( z k ∣ x 0 , u 1 : k , z 1 : k − 1 ) \begin{aligned} & \ \ \ \ \ P(x_k|x_0,u_{1:k},z_{1:k})\\ & = \frac{P(x_k,x_0,u_{1:k},z_{1:k})}{P(x_0,u_{1:k},z_{1:k})} \\ &= \frac{P(z_k|x_k,x_0,u_{1:k},z_{1:k-1})P(x_k|x_0,u_{1:k},z_{1:k-1})P(x_0,u_{1:k},z_{1:k-1})}{P(z_k|x_0,u_{1:k},z_{1:k-1})P(x_0,u_{1:k},z_{1:k-1})} \\ &=\frac{P(z_k|x_k,x_0,u_{1:k},z_{1:k-1})P(x_k|x_0,u_{1:k},z_{1:k-1})}{P(z_k|x_0,u_{1:k},z_{1:k-1})} \\ \end{aligned} P(xk∣x0,u1:k,z1:k)=P(x0,u1:k,z1:k)P(xk,x0,u1:k,z1:k)=P(zk∣x0,u1:k,z1:k−1)P(x0,u1:k,z1:k−1)P(zk∣xk,x0,u1:k,z1:k−1)P(xk∣x0,u1:k,z1:k−1)P(x0,u1:k,z1:k−1)=P(zk∣x0,u1:k,z1:k−1)P(zk∣xk,x0,u1:k,z1:k−1)P(xk∣x0,u1:k,z1:k−1) - 我们假设世界是马尔可夫的，即当前的状态只和上一刻有关，这样的话在 x k x_k xk已知的情况下，就已经包含了 x 0 , u 1 : k , z 1 : k − 1 x_0,u_{1:k},z_{1:k-1} x0,u1:k,z1:k−1的所有信息,即符合条件独立(此时的 x k x_k xk是已知的了，是一个确定的状态了）
P ( z k ∣ x k , x 0 , u 1 : k , z 1 : k − 1 ) = P ( z k ∣ x k ) P(z_k|x_k,x_0,u_{1:k},z_{1:k-1})=P(z_k|x_k) P(zk∣xk,x0,u1:k,z1:k−1)=P(zk∣xk)
- 由于分母与 x k x_k xk的取值无关，即不管 x k x_k xk取什么值，分母都是定值，可以看成一个常量，即
η = P ( z k ∣ x 0 , u 1 : k , z 1 : k − 1 ) − 1 \eta ={P(z_k|x_0,u_{1:k},z_{1:k-1})}^{-1} η=P(zk∣x0,u1:k,z1:k−1)−1
综上，可以得到
P ( x k ∣ x 0 , u 1 : k , z 1 : k ) = η P ( z k ∣ x k ) P ( x k ∣ x 0 , u 1 : k , z 1 : k − 1 ) P(x_k|x_0,u_{1:k},z_{1:k})=\eta P(z_k|x_k)P(x_k|x_0,u_{1:k},z_{1:k-1}) P(xk∣x0,u1:k,z1:k)=ηP(zk∣xk)P(xk∣x0,u1:k,z1:k−1)
将 P ( z k ∣ x k ) P(z_k|x_k) P(zk∣xk)称为似然，将 P ( x k ∣ x 0 , u 1 : k , z 1 : k − 1 ) P(x_k|x_0,u_{1:k},z_{1:k-1}) P(xk∣x0,u1:k,z1:k−1)称为先验。到现在，我们可以通过观测方程求得似然，但是先验部分还没有办法直接求，接下来使用全概率公式引进 x k − 1 x_{k-1} xk−1
P ( x k ∣ x 0 , u 1 : k , z 1 : k − 1 ) = ∑ x k − 1 P ( x k , x k − 1 , u 1 : k , z 1 : k − 1 ) P ( x 0 , u 1 : k , z 1 : k − 1 ) = ∑ x k − 1 P ( x k ∣ x k − 1 , x 0 , u 1 : k , z 1 : k − 1 ) P ( x k − 1 ∣ x 0 , u 1 : k , z 1 : k − 1 ) P ( x 0 , u 1 : k , z 1 : k − 1 ) P ( x 0 , u 1 : k , z 1 : k − 1 ) = ∑ x k − 1 P ( x k ∣ x k − 1 , x 0 , u 1 : k , z 1 : k − 1 ) P ( x k − 1 ∣ x 0 , u 1 : k , z 1 : k − 1 ) 离 散 形 式 = ∫ P ( x k ∣ x k − 1 , x 0 , u 1 : k , z 1 : k − 1 ) P ( x k − 1 ∣ x 0 , u 1 : k , z 1 : k − 1 ) d x k − 1 连 续 形 式 \begin{aligned} & \ \ \ \ \ P(x_k|x_0,u_{1:k},z_{1:k-1})\\ & = \frac{\sum_{x_k-1} P(x_k,x_{k-1},u_{1:k},z_{1:k-1} )}{P(x_0,u_{1:k},z_{1:k-1})}\\ &= \sum_{x_k-1} \frac{P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})P(x_0,u_{1:k},z_{1:k-1})}{P(x_0,u_{1:k},z_{1:k-1})} \\ &=\sum_{x_k-1} P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})P(x_{k-1}|x_0,u_{1:k},z_{1:k-1}) \ 离散形式\\ &=\smallint P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})dx_{k-1} \ 连续形式 \end{aligned} P(xk∣x0,u1:k,z1:k−1)=P(x0,u1:k,z1:k−1)∑xk−1P(xk,xk−1,u1:k,z1:k−1)=xk−1∑P(x0,u
最低0.47元/天 解锁文章
扫一扫
214
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
