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
¿Una posible solución? el information filter o inverse covariance filter
\hat{\mathbf{y}}=\mathbf{P}^{-1}\hat{\mathbf{x}}
\mathbf{Y}=\mathbf{P}^{-1}
La matriz de información \(\mathbf{Y}\) presenta una estructura dispersa en comparación a la covarianza
\mathbf{I}_k=\mathbf{C}_k^\top \mathbf{Q}_v^{-1}[k] \mathbf{C}_k
\mathbf{i}_k=\mathbf{C}_k^\top \mathbf{Q}_v^{-1}[k] \mathbf{z}_k
\mathbf{Y}_{k|k}=\mathbf{Y}_{k|k-1}+\mathbf{I}_k
\hat{\mathbf{y}}_{k|k}=\hat{\mathbf{y}}_{k|k-1}+\mathbf{i}_k
¿Una posible solución? el information filter o inverse covariance filter
\hat{\mathbf{y}}=\mathbf{P}^{-1}\hat{\mathbf{x}}
\mathbf{Y}=\mathbf{P}^{-1}
La matriz de información \(\mathbf{Y}\) presenta una estructura dispersa en comparación a la covarianza
\mathbf{I}_k=\mathbf{C}_k^\top \mathbf{Q}_v^{-1}[k] \mathbf{C}_k
\mathbf{i}_k=\mathbf{C}_k^\top \mathbf{Q}_v^{-1}[k] \mathbf{z}_k
\mathbf{Y}_{k|k}=\mathbf{Y}_{k|k-1}+\mathbf{I}_k
\hat{\mathbf{y}}_{k|k}=\hat{\mathbf{y}}_{k|k-1}+\mathbf{i}_k
\sum_{j=1}^{m} \mathbf{I}_{k,j}
\sum_{j=1}^{m} \mathbf{i}_{k,j}
puede actualizarse por medición (landmark en este caso)
¿Otros problemas y limitantes de la formulación con el EKF | EIF?
\(\to\) limitante por distribución normal
\(\to\) distribución unimodal
\(\to\) una única hipótesis
dependencia crítica en la asignación
\(\to\) limitante por distribución normal
\(\to\) distribución unimodal
\(\to\) una única hipótesis
dependencia crítica en la asignación
resuelto en propuestas como FastSLAM mediante un filtro de partículas
\(\to\) limitante por distribución normal
\(\to\) distribución unimodal
\(\to\) una única hipótesis
dependencia crítica en la asignación
el particle filter ayuda pero también hay propuestas basadas en Pose Graph Optimization que formulan esto de manera más efectiva
Filtro de partículas
Pose Graph SLAM
¿Cuándo se vuelven mainstream los vehículos autónomos?
Referencias
- MT3006 - Localización y mapeo en robótica móvil.pdf
- P. Corke, Robotics Vision and Control Fundamentals 2nd Ed., capítulo 6.
MT3006 - Lecture 15 (2025)
By Miguel Enrique Zea Arenales
