SLAM for Dummies

Published: 2020-01-07 | Lastmod: 2020-01-27

TO BE CONTINUED

https://zhuanlan.zhihu.com/p/32937247

观测方程 #

$\underbrace{\begin{pmatrix} r_1 \\ \theta_1 \\ r_2 \\ \theta_2 \\ \vdots \\ r_n \\ \theta_n \end{pmatrix}}_Z = \underbrace{\begin{bmatrix} A_1 &B_1 &C_1 &-A_1 &-B_1 \\ D_1 &E_1 &F_1 &-D_1 &-E_1 \\ A_2 &B_2 &C_2 & & &-A_2 &-B_2 \\ D_2 &E_2 &F_2 & & &-D_2 &-E_2 \\ &\vdots & & & & & & \ddots \\ A_n &B_n &C_n & & & & & &-A_n &-B_n \\ D_n &E_n &F_n & & & & & &-D_n &-E_n \\ \end{bmatrix}}_H \underbrace{\begin{pmatrix} x_r \\ y_r \\ \theta_r \\ x_1 \\ y_1 \\ x_2 \\ y_2 \\ \vdots \\ x_n \\ y_n \end{pmatrix}}_X \\[2mm] R = \begin{pmatrix} r_c & \\ &r_d \end{pmatrix}$

动态方程 #

$\underbrace{\begin{pmatrix} x_r \\ y_r \\ \theta_r \end{pmatrix}}_{X_{k+1}} = \underbrace{\begin{bmatrix} 1 & &-\Delta y \\ &1 &\Delta x \\ & &1\end{bmatrix}}_A \underbrace{\begin{pmatrix} x_r \\ y_r \\ \theta_r \end{pmatrix}}_{X_k} \\[2mm] Q = \begin{pmatrix} c\Delta x^2 \\ & c\Delta y^2 \\ & &c\Delta t^2\end{pmatrix}$

新增路标点的协方差矩阵增广 #

landmark wrt. robot state

$J_{xr} = \begin{bmatrix} 1 & &-\Delta y \\ &1 &\Delta x\end{bmatrix}$

landmark wrt. [range, bearing]

$J_z = \begin{bmatrix} \cos(\theta + \Delta \theta) &-\Delta t \cdot \sin(\theta + \Delta \theta) \\ \sin(\theta + \Delta \theta) &\Delta t \cdot \cos(\theta + \Delta \theta) \end{bmatrix}$

covariance

$P^{r \ {N+1}} = P^{rr}J_{xr}^T \\[2mm] P^{i \ {N+1}} = P^{ri}J_{xr}^T \\[2mm] P^{{N+1} \ {N+1}} = J_{xr}PJ_{xr}^T + J_zRJ_z^T$

Next: GMapping
Previous: 非线性优化