RBPF

最新推荐文章于 2022-05-02 14:15:12 发布

hjwang1

最新推荐文章于 2022-05-02 14:15:12 发布

阅读量2.7k

点赞数

分类专栏：算法人工智能

本文链接：https://blog.csdn.net/hjwang1/article/details/45951123

版权

算法同时被 2 个专栏收录

277 篇文章 7 订阅

订阅专栏

人工智能

255 篇文章 7 订阅

订阅专栏

粒子滤波

假设系统的状态转移服从一阶马尔科夫模型 $p(x_k|x_{k-1})$
$\mathscr{ABCDEFGHIJKLMNOPQRSTUVWXYZ}$
$\mathcal{ABC}$
$\pmb{\mathscr{ABCDEFGHIJKLMNOPQRSTUVWXYZ}}$

The following factorization

p (x 1 : t, m | z 1 : t, u 1 : t - 1) = p (m | x 1 : t, z 1 : t) \cdot p (x 1 : t | z 1 : t, u 1 : t - 1)

$\begin{align} p(x_{1:t},m|z_{1:t},u_{1:t-1}) & = p(m|x_{{1:t}},z_{{1:t}}) \cdot p(x_{1:t}|z_{1:t},u_{1:t-1}) \\ \end{align}$

Rao-Blackwellized SIR(sampling importance resampling)

w (i) t = p ( x ( i ) 1 : t | z 1 : t , u 1 : t - 1 ) π ( x ( i ) 1 : t | z 1 : t , u 1 : t - 1 )

$\begin{align} w_t^{( i )} = \frac {p(x_{1:t}^{( i )}|z_{1:t},u_{1:t-1})}{\pi(x_{1:t}^{( i )}|z_{1:t},u_{1:t-1})} \\ \end{align}$

the observation model $p(z_t|m,x_t)$ ，即 $p(z_t|m_{t-1}^{(i)},x_{t}^{(i)})$
the motion model $p(x_t|x_{t-1},u_{t-1})$ ，即 $p(x_{t}^{(i)}|x_{t-1}^{(i)},u_{t-1})$
Improved RBPF for Map Learning
Require:
$S_{t-1}$ ，the sample set of the previous time step
$z_t$ ，the most recent laser scan
$u_{t-1}$ ，the most recent odometry measurement
Ensure:
$S_t$ ，the new sample set

$S_t=\{\}$
for all $s_{t-1}^{(i)} \in S_{t-1}$ do
<x(i)t−1,w(i)t−1,m(i)t−1>=s(i)t−1 <script type="math/tex" id="MathJax-Element-928"> =s_{t-1}^{(i)}</script>

// scan matching
$x_{t}^{'(i)}=x_{t-1}^{(i)} \oplus u_{t-1}$
$\hat x_t^{( i )}=argmax_x p(x|m_{t-1}^{(i)},z_t,x_{t}^{'(i)})$

p (x k | y 1 : k) = p ( y k | x k ， y 1 : k - 1 ) p ( x k | y 1 : k - 1 ) p ( y k | y k - 1 ) = p ( y k | x k ) p ( x k | y 1 : k - 1 ) p ( y k | y k - 1 )

$\begin{align} p(x_k|y_{1:k}) & = \frac {p(y_k|x_k，y_{1:k-1})p(x_k|y_{1:k-1})}{p(y_k|y_{k-1})} \\ &= \frac {\color{red}{p(y_k|x_k)}p(x_k|y_{1:k-1})}{p(y_k|y_{k-1})} \end{align}$
其中，归一化常数