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

  1. MT3006 - Localización y mapeo en robótica móvil.pdf
  2. P. Corke, Robotics Vision and Control Fundamentals 2nd Ed., capítulo 6.