Least squares

SLAM을 최적화하기위한 수학적 기법 및 방법론 중 하나 (L2를 최소로만드는것)

- over-determined system:미지수보다 data가 많은 경우 -> 최적화 가능

x의 object를 토대로 f(x') = Z' 일때 실제 Z의 오차를 error

 

MAP estimation (지도아님)

Motion model

state = $x_{k}^{r} = f(x_{k-1}^{r} , u_{k}, e_{u})$

$u_{k}$: control or odometry

$e_{u}$: motion noise

$x_{k}^{r} = \begin{bmatrix} x^{r}(k) \\ y^{r}(k) \\ θ^{r}(k) \end{bmatrix}$

$x_{k-1}^{r}=[0,0,0], u_{k}=1, 45도, e_{u}=0.1$이면 [1.1, 1.1, 50] 가 되는 셈

 

Observation model

state = $z_{k} = h(x_{k}^{r} , l_{i}, e_{z})$

$l_{i}$ 차량이 object와 얼마나 거리가 있는지를 표현

 

2D LiDAR 라면

distance: $z_{k} = \sqrt{(x^{r}(k)-l_{i}^{x})^{2} + (y^{r}(k)-l_{i}^{y})^{2}}$bearing:  $z_{k} = atan2(y_{i}^{y} - y^{r}(k), l_{i}^{x} - x^{r}(k)) - θ^{r}(k) + e_{z}$

 

SLAM State and UncertaintySLAM State = [$x_{k}^{r}, l_{1}, l_{2}, ... l_{m}$] + noise센서별 noise가 있으며 이동, 관측 할때마다 noise 가 누적될수록 오차도 커진다

 

SLAM Scenario

Maximum-a-posteriori eeimation: 최대 사후 확률 추정motion model, observation의 확률 분포를 찾고 이를 표현하는 게 목표motion model$x_{k} = f(x_{k-1}, u_{k}, e_{u}) -> x_{k} = f(x_{k-1}, u_k}) 가정 -> x_k - f(x_{k-1}, u_{k}) = 0 =e_{u}$observation model$x_{k,j} = h(x_{k}, l_{j}, e_{z}) -> x_{k,j} = h(x_{k}, l_{j}) 가정 -> x_{k,j} - h(x_{k}, l_{j}) = 0 =e_{z}$

 

state에 대한 least squares의 최소값을 수식화하면 -> Joint optimization

argmin_{x_{0},...,x_{k}} \sum_{i=1}^{k} ||x_{i} - f(x_{i-1}, u_{i})||^2 + \sum_{i,j∈\varepsilon }z_{i,j} - h(x_{i},l_{j})||^2$

 

Graph-based SLAM

Incremental SLAM(i.e. Online SLAM)

- Particle filter, extended kalman filter...- chain rule과 비슷한 방식으로 가장 최종 state만 추정 (최종 state는 이전state가 반영되어있음)- 장:빠르다. 실시간성- 단:drift error에 취약하며 상대적 부정확

Batch SLAM(i.e. Offline SLAM)

- Graph-based SLAM- 한번에 여러 state를 추정- 장: drifit error에 강- 현대에는 여러 기법을 사용해 실시간으로도 가능- 동일 정확도를 위한 계산량이 더 적다-> 현대 SLAM에서 채택됨

 

Factor graphNode - states - pose                      - landmarkedge - measurements - motion                                    - observationedge를 factor라고 부름

-> loop closure에 효과적

 

pipeline

Frontend - add graph(nodes, edges)

backend - loop closure, 최적