FastSLAM

EKF SLAM의 문제점

- 2차원 평면상에서 $K$개의 landmark 존재.  LM 파라미터: $(x_i, y_i)$

- robot pose: $(x, y,  \phi)$

- 합하면 $2K+3$개의 인자

- Kalman filter를 통해 이것들을 추정하면 mean값을 $2K+3$개를 유지

  Covariance matrix는 $(2K+3)^2$크기가 요구
- LM의 수가 늘어 감에 따라 유지해야 하는 파라메터의 수가 급격히 증가(계산량 증가)

FastSLAM에서의 개선

- Landmark는 로봇의 pose가 주어진다면 상호 독립이라 가정

- 각 landmark에 대해 $2\times2$ covariance matrix

  전체 인자의 수: $K(2+2\times2)+3$

Robot poses: $s^t=s_1, s_2, ...,s_t$
Robot actions: $u^t=u_1,u_2,...,u_t$
Observations: $z^t=z_1,z_2,...,z_t$
Landmarks: $\boldsymbol{\theta}=\theta_1,...,\theta_k$
Correspondence: $n^t=n_1,n_2,...,n_t$
- $n_t=k$는 $z_t$가 $\theta_k$의 관찰 값임을 의미
- landmark는 한번에 하나씩 관찰된다고 가정

그림에서 shade영역은 $t_1\sim$현재까지의 로봇의 경로이다.
만일 로봇의 실제 경로를 안다면(즉 shade영역) LM $\theta_1$과 $\theta_2$는 서로 독립이다.


SLAM 문제
- action model: $p(s_t|u^t,s^{t-1})$
- sensor model: $p(z_t|s_t,\boldsymbol{\theta},n_t)$

SLAM 문제는 $z^t, u^t, n^t$가 주어질 때 $\boldsymbol{\theta},s^t$를 추정하는 문제이다.  Bayes theory를 이용하면

$p(s^t,\boldsymbol{\theta}|z^t,u^t,n^t)=
{{p(\boldsymbol{\theta},s^t,z^t,u^t,n^t)} \over {p(z^t,u^t,n^t)}}={{p(\boldsymbol{\theta}|s^t,z^t,u^t,n^t)p(s^t,z^t,u^t,n^t)} \over {p(z^t,u^t,n^t)}}$
$=p(\boldsymbol{\theta}|s^t,z^t,u^t,n^t)p(s^t|z^t,u^t,n^t)$

landmark의 독립가정$^{주1}$에 따라
$p(s^t,\boldsymbol{\theta}|z^t,u^t,n^t)
 =p(s^t|z^t,u^t,n^t)\prod_k^{}p(\theta_k|s^t,z^t,u^t,n^t)$

- trajectory 추정 $p(s^t|z^t,u^t,n^t)$
- robot의 자세 추정은 particle filter를 이용하고, 각 particle에 대한 landmark위치와 분산의 갱신의 EKF를 사용
- particle당 $K$개의 Kalman filter가 있으므로 particle의 수가 $M$개라면 갱신과정의 시간 복잡도는 $O(MK)$이다.



(주1) 독립가정에 따라
$p(\boldsymbol{\theta}|s^t,z^t,u^t,n^t)=...$



References
[1] M. Montemerlo, FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem With Unknown Data Association, CMU-CS-03, Ph.D. thesis, 2003.
[2] Benjamin Kuipers, FastSlam, lecture notes, CS 344R/393R: Robotics.