SLAM II: EKF-SLAM
MT3006 - Robótica 2
La primera solución "viable" al problema de SLAM
mismo setup que para mapeo, pero se añade la pose del robot a la estimación
\hat{\mathbf{x}}_{0|0}=\begin{bmatrix} {^N}\hat{\boldsymbol{\xi}}_{0|0} \end{bmatrix}
\mathbf{P}_{0|0}=\begin{bmatrix} \mathbf{P}_{\xi\xi,0|0} \end{bmatrix}=\begin{bmatrix} \sigma_e^2 \mathbf{I} \end{bmatrix}
M=0
número de landmarks
n=3
dimensión (inicial) de \(\hat{\mathbf{x}}\)
Combinando ambos casos
y_N
x_N
no se ha encontrado ningún landmark
\(\Rightarrow\) el robot sigue explorando pero su pose empieza a divergir por el dead reckoning
t=t_1
y_N
x_N
t=t_1
{^N}\hat{\boldsymbol{\xi}}_{1|0}=\text{odometría}\left({^N}\hat{\boldsymbol{\xi}}_{0|0}, \mathbf{u}_1\right) \\
\mathbf{P}_{\xi\xi,1|0}=\text{odometría}\left(\mathbf{P}_{\xi\xi,0|0}\right)
NO hay corrección
no se ha encontrado ningún landmark
y_N
x_N
t=t_2
?
y_N
x_N
t=t_2
?
{^N}\hat{\boldsymbol{\xi}}_{2|1}=\text{odometría}\left({^N}\hat{\boldsymbol{\xi}}_{1|1}, \mathbf{u}_2\right) \\
\mathbf{P}_{\xi\xi,2|1}=\text{odometría}\left(\mathbf{P}_{\xi\xi,1|1}\right)
como no hubo corrección, deben emplearse los resultados de la predicción previa
y_N
x_N
se detecta un landmark
t=t_2
?
(\kappa_?,\beta_?)
y_N
x_N
se detecta un landmark
t=t_2
?
(\kappa_?,\beta_?)
\mathbf{p}_{?}(t_2)= \underbrace{\begin{bmatrix} \xi_1(t_2) + \kappa_?\cos\left(\xi_3(t_2)+\beta_?\right) \\ \xi_2(t_2) + \kappa_?\sin\left(\xi_3(t_2)+\beta_?\right)\end{bmatrix}}_{{^N}\mathbf{g}\left({^N}\boldsymbol{\xi}(t_2),\mathbf{y}_?(t_2)\right)}
\(\Rightarrow\) se emplean los sensores para estimar la posición del landmark
y_N
x_N
¿Es \(\mathbf{p}_?\) un landmark conocido? NO, es un landmark nuevo
\mathbf{p}_?(t_2) \to \mathbf{p}_1(t_2)
(\kappa_?,\beta_?)\to (\kappa_1(t_2),\beta_1(t_2))
¿Es \(\mathbf{p}_?\) un landmark conocido? NO, es un landmark nuevo
\mathbf{p}_?(t_2) \to \mathbf{p}_1(t_2)
(\kappa_?,\beta_?)\to (\kappa_1(t_2),\beta_1(t_2))
\hat{\mathbf{x}}_{2|2}^\mathrm{new}=\begin{bmatrix} {^N}\hat{\boldsymbol{\xi}}_{2|2} \\ \hat{\mathbf{p}}_{1,2|2} \end{bmatrix}
=\begin{bmatrix} {^N}\hat{\boldsymbol{\xi}}_{2|2} \\ \mathbf{p}_1(t_2) \end{bmatrix}
\mathbf{P}_{2|2}^\mathrm{new}=\mathbf{Y}_\mathbf{y}\begin{bmatrix} \mathbf{P}_{\xi\xi,2|2} & \mathbf{0}_{n \times 2} \\ \mathbf{0}_{2\times n} & \mathbf{Q}_{vg} \end{bmatrix}\mathbf{Y}_\mathbf{y}^\top=
\begin{bmatrix} \mathbf{P}_{\xi\xi,2|2} & \mathbf{P}_{\xi p,2|2} \\ \mathbf{P}^\top_{\xi p,2|2} & \mathbf{P}_{pp,2|2} \end{bmatrix}
\mathbf{P}_{\xi\xi,2|2}=\mathbf{P}_{\xi\xi,2|1}
¿Es \(\mathbf{p}_?\) un landmark conocido? NO, es un landmark nuevo
\mathbf{p}_?(t_2) \to \mathbf{p}_1(t_2)
(\kappa_?,\beta_?)\to (\kappa_1(t_2),\beta_1(t_2))
\hat{\mathbf{x}}_{2|2}^\mathrm{new}=\begin{bmatrix} {^N}\hat{\boldsymbol{\xi}}_{2|2} \\ \hat{\mathbf{p}}_{1,2|2} \end{bmatrix}
=\begin{bmatrix} {^N}\hat{\boldsymbol{\xi}}_{2|2} \\ \mathbf{p}_1(t_2) \end{bmatrix}
\mathbf{P}_{2|2}^\mathrm{new}=\mathbf{Y}_\mathbf{y}\begin{bmatrix} \mathbf{P}_{\xi\xi,2|2} & \mathbf{0}_{n \times 2} \\ \mathbf{0}_{2\times n} & \mathbf{Q}_{vg} \end{bmatrix}\mathbf{Y}_\mathbf{y}^\top=
\begin{bmatrix} \mathbf{P}_{\xi\xi,2|2} & \mathbf{P}_{\xi p,2|2} \\ \mathbf{P}^\top_{\xi p,2|2} & \mathbf{P}_{pp,2|2} \end{bmatrix}
\mathbf{P}_{\xi\xi,2|2}=\mathbf{P}_{\xi\xi,2|1}
¿Es \(\mathbf{p}_?\) un landmark conocido? NO, es un landmark nuevo
\mathbf{Y}_\mathbf{y}=\begin{bmatrix} \mathbf{I}_{n \times n} & & \mathbf{0}_{n \times 2} \\ \mathbf{G}_{\boldsymbol{\xi}} & \mathbf{0}_{2\times n-3} & \mathbf{G}_\mathbf{y} \end{bmatrix}
\mathbf{G}_{\boldsymbol{\xi}}=\begin{bmatrix} 1 & 0 & -\kappa_1(t_2)\sin\left(\xi_3(t_2)+\beta_1(t_2)\right) \\
0 & 1 & \kappa_1(t_2)\cos\left(\xi_3(t_2)+\beta_1(t_2)\right) \end{bmatrix}
\mathbf{G}_\mathbf{y}=\begin{bmatrix} \cos(\xi_3(t_2) + \beta_1(t_2)) & -\kappa_1(t_2)\sin(\xi_3(t_2)+\beta_1(t_2)) \\
\sin(\xi_3(t_2) + \beta_1(t_2)) & \kappa_1(t_2)\cos(\xi_3(t_2)+\beta_1(t_2))\end{bmatrix}
¿Es \(\mathbf{p}_?\) un landmark conocido? NO, es un landmark nuevo
\mathbf{Y}_\mathbf{y}=\begin{bmatrix} \mathbf{I}_{n \times n} & & \mathbf{0}_{n \times 2} \\ \mathbf{G}_{\boldsymbol{\xi}} & \mathbf{0}_{2\times n-3} & \mathbf{G}_\mathbf{y} \end{bmatrix}
\mathbf{G}_{\boldsymbol{\xi}}=\begin{bmatrix} 1 & 0 & -\kappa_1(t_2)\sin\left(\xi_3(t_2)+\beta_1(t_2)\right) \\
0 & 1 & \kappa_1(t_2)\cos\left(\xi_3(t_2)+\beta_1(t_2)\right) \end{bmatrix}
\mathbf{G}_\mathbf{y}=\begin{bmatrix} \cos(\xi_3(t_2) + \beta_1(t_2)) & -\kappa_1(t_2)\sin(\xi_3(t_2)+\beta_1(t_2)) \\
\sin(\xi_3(t_2) + \beta_1(t_2)) & \kappa_1(t_2)\cos(\xi_3(t_2)+\beta_1(t_2))\end{bmatrix}
M=1 \qquad n=5
?
\square
t=t_3
y_N
x_N
t=t_3
?
\square
\mathbf{P}_{\xi\xi,3|2}=\text{odometría}\left(\mathbf{P}_{\xi\xi,2|2}\right) \\
\mathbf{P}_{\xi p,3|2}=\mathbf{A}\mathbf{P}_{\xi p,2|2}, \quad \mathbf{P}_{pp,3|2}=\mathbf{P}_{pp,2|2}
\hat{\mathbf{x}}_{3|2}=\begin{bmatrix} {^N}\hat{\boldsymbol{\xi}}_{3|2} \\ \hat{\mathbf{p}}_{1,3|2} \end{bmatrix}
\hat{\mathbf{x}}_{3|2}=\begin{bmatrix} \text{odometría}\left({^N}\hat{\boldsymbol{\xi}}_{2|2}, \mathbf{u}_3\right) \\ \hat{\mathbf{p}}_{1,2|2} \end{bmatrix}
y_N
x_N
?
\square
(\kappa_?,\beta_?)
(\kappa_\square,\beta_\square)
se detecta un landmark conocido y uno nuevo
t=t_3
y_N
x_N
?
\square
\mathbf{p}_{?}(t_3)= {^N}\mathbf{g}\left({^N}\boldsymbol{\xi}(t_3),\mathbf{y}_?(t_3)\right)
\mathbf{p}_\square(t_3)= {^N}\mathbf{g}\left({^N}\boldsymbol{\xi}(t_3),\mathbf{y}_\square(t_3)\right)
(\kappa_?,\beta_?)
(\kappa_\square,\beta_\square)
se detecta un landmark conocido y uno nuevo
t=t_3
y_N
x_N
para el landmark conocido
\mathbf{p}_?(t_3) \to \mathbf{p}_1(t_3)
(\kappa_?,\beta_?)\to (\kappa_1(t_3),\beta_1(t_3))
asignación
\mathbf{z}(t_3) = \begin{bmatrix} \mathbf{y}_1(t_3)-\mathcal{H}_1 \left(\hat{\mathbf{x}}_{3|2} \right)
\end{bmatrix}
\mathbf{C}(t_3)=\begin{bmatrix} \mathbf{C}_{\boldsymbol{\xi}_1}(t_3) & \mathbf{C}_1(t_3) \end{bmatrix}
\mathbf{L}_3=\mathbf{P}_{3|2}\mathbf{C}^\top(t_3)\left(\mathbf{C}(t_3)\mathbf{P}_{3|2}\mathbf{C}^\top(t_3)+\mathbf{I}_{M \times M} \otimes \mathbf{Q}_{v\mathcal{G}}\right)^{-1}
\hat{\mathbf{x}}_{3|3}=\hat{\mathbf{x}}_{3|2}+\mathbf{L}_3\mathbf{z}(t_3)
\mathbf{P}_{3|3}=\mathbf{P}_{3|2}-\mathbf{L}_3\mathbf{C}(t_3)\mathbf{P}_{3|2}
para el landmark nuevo
\mathbf{p}_\square(t_3) \to \mathbf{p}_2(t_3)
(\kappa_\square,\beta_\square)\to (\kappa_2(t_3),\beta_2(t_3))
\hat{\mathbf{x}}_{3|3}^\mathrm{new}=\begin{bmatrix} \hat{\mathbf{x}}_{3|3}^\mathrm{old} \\ \hat{\mathbf{p}}_{2,3|3} \end{bmatrix}
=\begin{bmatrix} \hat{\mathbf{x}}_{3|3}^\mathrm{old} \\ \mathbf{p}_2(t_3) \end{bmatrix}
\mathbf{P}_{3|3}^\mathrm{new}=\mathbf{Y}_\mathbf{y}\begin{bmatrix} \mathbf{P}_{3|3}^\mathrm{old} & \mathbf{0}_{n \times 2} \\ \mathbf{0}_{2\times n} & \mathbf{Q}_{vg} \end{bmatrix}\mathbf{Y}_\mathbf{y}^\top=
\begin{bmatrix} \mathbf{P}_{\xi\xi,3|3} & \mathbf{P}_{\xi p,3|3} \\ \mathbf{P}^\top_{\xi p,3|3} & \mathbf{P}_{pp,3|3} \end{bmatrix}
para el landmark nuevo
\mathbf{p}_\square(t_3) \to \mathbf{p}_2(t_3)
(\kappa_\square,\beta_\square)\to (\kappa_2(t_3),\beta_2(t_3))
\hat{\mathbf{x}}_{3|3}^\mathrm{new}=\begin{bmatrix} \hat{\mathbf{x}}_{3|3}^\mathrm{old} \\ \hat{\mathbf{p}}_{2,3|3} \end{bmatrix}
=\begin{bmatrix} \hat{\mathbf{x}}_{3|3}^\mathrm{old} \\ \mathbf{p}_2(t_3) \end{bmatrix}
\mathbf{P}_{3|3}^\mathrm{new}=\mathbf{Y}_\mathbf{y}\begin{bmatrix} \mathbf{P}_{3|3}^\mathrm{old} & \mathbf{0}_{n \times 2} \\ \mathbf{0}_{2\times n} & \mathbf{Q}_{vg} \end{bmatrix}\mathbf{Y}_\mathbf{y}^\top=
\begin{bmatrix} \mathbf{P}_{\xi\xi,3|3} & \mathbf{P}_{\xi p,3|3} \\ \mathbf{P}^\top_{\xi p,3|3} & \mathbf{P}_{pp,3|3} \end{bmatrix}
M=2 \qquad n=7
?
\square
t=t_4
y_N
x_N
?
\square
t=t_4
\hat{\mathbf{x}}_{4|3}=\begin{bmatrix} {^N}\hat{\boldsymbol{\xi}}_{4|3} \\ \hat{\mathbf{p}}_{1,4|3} \\ \hat{\mathbf{p}}_{2,4|3} \end{bmatrix}
\hat{\mathbf{x}}_{4|3}=
\begin{bmatrix} \text{odometría}\left({^N}\hat{\boldsymbol{\xi}}_{3|3}, \mathbf{u}_4\right) \\ \hat{\mathbf{p}}_{1,3|3} \\ \hat{\mathbf{p}}_{2,3|3} \end{bmatrix}
y_N
x_N
?
\square
t=t_4
\mathbf{P}_{\xi\xi,4|3}=\text{odometría}\left(\mathbf{P}_{\xi\xi,3|3}\right) \\
\mathbf{P}_{\xi p,4|3}=\mathbf{A}\mathbf{P}_{\xi p,3|3}, \quad \mathbf{P}_{pp,4|3}=\mathbf{P}_{pp,3|3}
\hat{\mathbf{x}}_{4|3}=\begin{bmatrix} {^N}\hat{\boldsymbol{\xi}}_{4|3} \\ \hat{\mathbf{p}}_{1,4|3} \\ \hat{\mathbf{p}}_{2,4|3} \end{bmatrix}
y_N
x_N
?
\square
t=t_4
(\kappa_?,\beta_?)
(\kappa_\square,\beta_\square)
se detectan dos landmarks conocidos
y_N
x_N
?
\square
t=t_4
(\kappa_?,\beta_?)
(\kappa_\square,\beta_\square)
se detectan dos landmarks conocidos
\mathbf{p}_{?}(t_4)= {^N}\mathbf{g}\left({^N}\boldsymbol{\xi}(t_4),\mathbf{y}_?(t_4)\right)
\mathbf{p}_\square(t_4)= {^N}\mathbf{g}\left({^N}\boldsymbol{\xi}(t_4),\mathbf{y}_\square(t_4)\right)
y_N
x_N
\mathbf{p}_?(t_4) \to \mathbf{p}_1(t_4)
(\kappa_?,\beta_?)\to (\kappa_1(t_4),\beta_1(t_4))
asignación
\mathbf{z}(t_4) = \begin{bmatrix} \mathbf{y}_1(t_4)-\mathcal{H}_1 \left( \hat{\mathbf{x}}_{4|3} \right) \\
\mathbf{y}_2(t_4)-\mathcal{H}_2 \left( \hat{\mathbf{x}}_{4|3} \right) \end{bmatrix}
\mathbf{C}(t_4)=\begin{bmatrix} \mathbf{C}_{\boldsymbol{\xi}_1}(t_4) & \mathbf{C}_1(t_4) & \mathbf{0} \\
\mathbf{C}_{\boldsymbol{\xi}_2}(t_4) & \mathbf{0} & \mathbf{C}_2(t_4) \end{bmatrix}
\mathbf{p}_\square(t_4) \to \mathbf{p}_2(t_4)
(\kappa_\square,\beta_\square)\to (\kappa_2(t_4),\beta_2(t_4))
\mathbf{p}_?(t_4) \to \mathbf{p}_1(t_4)
(\kappa_?,\beta_?)\to (\kappa_1(t_4),\beta_1(t_4))
asignación
\mathbf{p}_\square(t_4) \to \mathbf{p}_2(t_4)
(\kappa_\square,\beta_\square)\to (\kappa_2(t_4),\beta_2(t_4))
\mathbf{L}_4=\mathbf{P}_{4|3}\mathbf{C}^\top(t_4)\left(\mathbf{C}(t_4)\mathbf{P}_{4|3}\mathbf{C}^\top(t_4)+\mathbf{I}_{M \times M} \otimes \mathbf{Q}_{v\mathcal{G}}\right)^{-1}
\hat{\mathbf{x}}_{4|4}=\hat{\mathbf{x}}_{4|3}+\mathbf{L}_4\mathbf{z}(t_4)
\mathbf{P}_{4|4}=\mathbf{P}_{4|3}-\mathbf{L}_4\mathbf{C}(t_4)\mathbf{P}_{4|3}
?
\square
t=t_5
y_N
x_N
?
\square
t=t_5
\hat{\mathbf{x}}_{5|4}=\begin{bmatrix} {^N}\hat{\boldsymbol{\xi}}_{5|4} \\ \hat{\mathbf{p}}_{1,5|4} \\ \hat{\mathbf{p}}_{2,5|4} \end{bmatrix}
\hat{\mathbf{x}}_{5|4}=
\begin{bmatrix} \text{odometría}\left({^N}\hat{\boldsymbol{\xi}}_{4|4}, \mathbf{u}_5\right) \\ \hat{\mathbf{p}}_{1,4|4} \\ \hat{\mathbf{p}}_{2,4|4} \end{bmatrix}
y_N
x_N
?
\square
t=t_5
\hat{\mathbf{x}}_{5|4}=\begin{bmatrix} {^N}\hat{\boldsymbol{\xi}}_{5|4} \\ \hat{\mathbf{p}}_{1,5|4} \\ \hat{\mathbf{p}}_{2,5|4} \end{bmatrix}
\mathbf{P}_{\xi\xi,5|4}=\text{odometría}\left(\mathbf{P}_{\xi\xi,4|4}\right) \\
\mathbf{P}_{\xi p,5|4}=\mathbf{A}\mathbf{P}_{\xi p,4|4}, \quad \mathbf{P}_{pp,5|4}=\mathbf{P}_{pp,4|4}
y_N
x_N
?
\square
t=t_5
se detecta un landmark conocido pero ya no puede verse al otro
(\kappa_?,\beta_?)
y_N
x_N
?
\square
t=t_5
se detecta un landmark conocido pero ya no puede verse al otro
(\kappa_?,\beta_?)
\mathbf{p}_{?}(t_5)= {^N}\mathbf{g}\left({^N}\boldsymbol{\xi}(t_5),\mathbf{y}_?(t_5)\right)
y_N
x_N
landmark conocido
\mathbf{p}_?(t_5) \to \mathbf{p}_1(t_5)
(\kappa_?,\beta_?)\to (\kappa_1(t_5),\beta_1(t_5))
\mathbf{p}_2(t_5)=\begin{bmatrix} \quad \end{bmatrix}
(\kappa_2(t_5),\beta_2(t_5))=(\quad , \quad )
\mathbf{z}(t_5) = \begin{bmatrix} \mathbf{y}_1(t_5)-\mathcal{H}_1 \left( \hat{\mathbf{x}}_{5|4} \right) \\ \mathbf{0}_{2 \times 1} \end{bmatrix}
landmark fuera de rango
\mathbf{C}_2(t_5)
\mathbf{C}(t_5)=\begin{bmatrix} \mathbf{C}_{\boldsymbol{\xi}_1}(t_5) & \mathbf{C}_1(t_5) & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} \end{bmatrix}
\mathbf{C}_{\boldsymbol{\xi}_2}(t_5)
y el proceso continúa...
(velocidad 3x)
>> mt3006_clase15_ekfslam.m
y el proceso continúa...
(velocidad 3x)
>> mt3006_clase15_ekfslam.m
sin embargo, ahora la corrección ya no puede separarse por landmark, dado que la matriz de covarianza pierde su estructura diagonal por bloques
ahora la estimación de la pose del robot y de los landmarks dependen todos entre sí
\mathbf{P}=\begin{bmatrix}
\boldsymbol\square & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\
\mathbf{0} & \boldsymbol\square & \mathbf{0} & \cdots & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \boldsymbol\square & \cdots & \mathbf{0} \\
\vdots & \vdots & \vdots & \ddots & \vdots \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \cdots & \boldsymbol\square
\end{bmatrix}
\approx
Referencias
- MT3006 - Localización y mapeo en robótica móvil.pdf
- P. Corke, Robotics Vision and Control Fundamentals 2nd Ed., capítulo 6.