11.3: EKF SLAM

Última actualización
Guardar como PDF

Page ID: 85046

\( \newcommand{\vecs}[1]{\overset { \scriptstyle \rightharpoonup} {\mathbf{#1}} } \) \( \newcommand{\vecd}[1]{\overset{-\!-\!\rightharpoonup}{\vphantom{a}\smash {#1}}} \)\(\newcommand{\id}{\mathrm{id}}\) \( \newcommand{\Span}{\mathrm{span}}\) \( \newcommand{\kernel}{\mathrm{null}\,}\) \( \newcommand{\range}{\mathrm{range}\,}\) \( \newcommand{\RealPart}{\mathrm{Re}}\) \( \newcommand{\ImaginaryPart}{\mathrm{Im}}\) \( \newcommand{\Argument}{\mathrm{Arg}}\) \( \newcommand{\norm}[1]{\| #1 \|}\) \( \newcommand{\inner}[2]{\langle #1, #2 \rangle}\) \( \newcommand{\Span}{\mathrm{span}}\) \(\newcommand{\id}{\mathrm{id}}\) \( \newcommand{\Span}{\mathrm{span}}\) \( \newcommand{\kernel}{\mathrm{null}\,}\) \( \newcommand{\range}{\mathrm{range}\,}\) \( \newcommand{\RealPart}{\mathrm{Re}}\) \( \newcommand{\ImaginaryPart}{\mathrm{Im}}\) \( \newcommand{\Argument}{\mathrm{Arg}}\) \( \newcommand{\norm}[1]{\| #1 \|}\) \( \newcommand{\inner}[2]{\langle #1, #2 \rangle}\) \( \newcommand{\Span}{\mathrm{span}}\)\(\newcommand{\AA}{\unicode[.8,0]{x212B}}\)

La idea clave en EKF SLAM es extender el vector de estado desde la posición del robot para contener la posición de todas las características. Así, el estado

\[\boldsymbol{x̂}_{k'|k-1}=(x,y,\theta )^{T}\]

se convierte

\[\boldsymbol{x̂}_{k}=(x,y,\theta ,\alpha _{1},...,\alpha _{N},r_{N})^{T}\]

asumiendo N características, que es un vector (3 + 2N) x1. La actualización de acción (o “actualización de predicción”) es idéntica a aquella si ya se conocen las características; el robot simplemente actualiza su posición usando odometría y actualiza la varianza de su posición usando propagación de errores. La matriz de covarianza es ahora una matriz (3+2N) x (3+2N) que inicialmente mantiene las varianzas en posición y las de cada entidad en su diagonal.

Las cosas interesantes suceden durante la actualización de la percepción. Aquí es importante que solo se observe una característica a la vez. Por lo tanto, si el robot observa múltiples características a la vez, uno necesita hacer múltiples actualizaciones de percepción consecutivas. Hay que tener cuidado de que las multiplicaciones matriciales funcionen. En la práctica necesitarás establecer únicamente aquellos valores del vector de observación (un vector (3+2N) x1) que corresponden a la característica que observas. Consideraciones similares se aplican a la función de observación y su jacobiano.