Simultaneous localization and mapping using the extended Kalman Filter

(1)

Localización y construcción de un mapa en forma simultánea utilizando el Filtro

extendido de Kalman

Carlos Restrepo∗, Alfonso Alzate, Carlos Torres, Jason Molina

Universidad Tecnol´ogica de Pereira, Risaralda, Colombia.

Resumen.-En este art´ıculo se presenta una metodolog´ıa que permite a un robot en un ambiente desconocido ir levantando un mapa en forma incremental a medida que explora su entorno y simultáneamente localizarse en el mapa. Esto es conocido como el problema SLAM (Simultaneous Localization and Mapping) el cual es solucionado utilizando como herramienta de estimación el Filtro Extendido de Kalman (EKF Extended Kalman Filter). La idea central de este trabajo es que el robot, desde un punto de partida espec´ıfico llegue hasta un objetivo deseado. El principal inconveniente en esta tarea es que, durante el recorrido, el robot presentará odométricos que conllevan a una incertidumbre en su posición. Dado que, el mapa que levanta el robot en cada uno de los puntos del recorrido es relativo a su posición, este mapa tendrá también incertidumbres debido a la suma de los efectos de los errores tanto odométricos como de los sensores del robot.

Palabras clave: SLAM, EKF, campos potenciales, programaci´on din´amica

Simultaneous localization and mapping using

the extended Kalman Filter

Abstract.-This article presents a methodology that allows a robot in an unknown environment to build a map in incremental form while it is exploring the environment, and locate on the map simultaneously. The task known as SLAM problem (Simultaneous Localization and Mapping), is solved using the Extended Kalman Filter (EKF) as the estimation tool. The main idea of this work is that the robot commutes from a specific starting point to a required objective. The main inconvenient in this task is that during the route, the robot will present odometric errors that produce position uncertainty. Since the map made by the robot at each of the route points, is relative to its position, this will have also uncertainties, due to the sum of the effects of the odometric errors and the robot’s sensors.

Keywords: SLAM, EKF, potential fields, dynamic programming

1. Introducci´on

La construcción de un mapa del entorno mediante un robot móvil es uno de los temas de investigación más importantes dentro de la rob ótica móvil y la inteligencia artificial desde hace dos décadas. La capacidad de un robot móvil para cons-truir un mapa de su entorno se considera que es fundamental para conseguir una total autonom´ıa. Es por eso que existen numerosos trabajos dentro de esta área con este objetivo.

El principal problema de la construcción de un mapa es el hecho de que las observaciones hechas por el robot son relativas al propio robot, y como el sistema odométri-co es inherentemente inexacto, su posición está caracteriza-da por una incertidumbre intr´ınseca. Por tanto, el mapa no es globalmente observable. Cuando el robot realiza una me-dida, también con incertidumbre, esta medida está referida a su posición, ya incierta. Como consecuencia de este pro-blema, la localización del robot y la construcción del mapa

∗_{Autor para correspondencia}

Correos-e: [email protected] (Carlos Restrepo),

[email protected] (Alfonso Alzate), [email protected]

(Carlos Torres), [email protected] (Jason Molina)

están acopladas y no se pueden resolver de manera rigurosa por ning ún método que desacople ambas estimaciones. En el caso de que el robot no tuviera ruido odométrico, la construc-ción del mapa ser´ıa muy sencilla ya que éste ser´ıa global-mente observable, y bastar´ıa realizar constanteglobal-mente nuevas medidas para disminuir la incertidumbre del mapa y obtener cada vez un mapa más preciso.

La idea subyacente en cualquier algoritmo que aborde el problema SLAM es la minimización conjunta de las incer-tidumbres del robot y el mapa. El EKF es una herramienta de fusión sensorial y de estimación estocástica muy amplia-mente usada en el mundo de la rob ótica.

2. Filtro de Bayes

Los algoritmos más importantes en el levantamiento de mapas modelan la posición del robot y su entorno de una ma-nera probabil´ıstica y utilizan la inferencia para realizar una transformación de las medidas, obtenidas por el robot, a un mapa probabil´ıstico. Esta formulación es la más extendida debido a la naturaleza misma del problema, ya que la incer-tidumbre presente en los sensores como en el movimiento

(2)

del robot hace que este planteamiento sea el más adecua-do. Un robot móvil explorando un ambiente desconocido de-berá moverse a través del entorno de acuerdo con la lectura que tomen los sensores del ambiente en cada punto. Es por esto que es fácil reconocer dos tipos de datos, las medidas de la odometr´ıa del robot y de los sensores de percepción. Se puede asumir que estas medidas llegan de forma secuencial y alternativamente en el tiempo [1, 2], como:

u1, z1, u2, z2, . . . , ut, zt (1) Siendo ut el desplazamiento relativo a la posición ante-rior del robot y ztuna medida sensorial tomada respecto al robot de un objeto del entorno y los sub´ındices representan el instante del tiempo en que fueron tomadas. El estado del sistema en el instante está dado por la posición del robot y la posición de cada uno de los objetos del entorno. Esto es [1, 3]: xt= " st mt # (2) La densidad de probabilidad del estado xt condiciona-do al conjunto de tocondiciona-dos los datos almacenacondiciona-dos hasta el instante t, se representa como P(xt|zt, ut). Donde el su-per´ındice t representa la agrupación de datos desde el ini-cio hasta el instante t, por lo que zt _{= z}

1, z2, . . . , zt y ut =

u1, u2, . . . , ut. El teorema de Bayes permite escribir la proba-bilidad P(xt|zt, ut) como [4,5]:

P(xt|zt, ut) = p(xt, mt| zt, ut)

=η p(zt| st, mt, zt−1, ut−1)

· p(st, mt| zt−1, ut) (3) En donde η es un factor de normalización de la función de probabilidad. Esta función de probabilidad, representa cualquier solución probabil´ıstica al problema SLAM.

Realizando la hip ótesis de Markov [6,7], es decir, que el único estado que existe es el representado por st y mt, la ecuación (3) se puede escribir como [1,3]:

p(st, ut|zt, ut) = η p(zt| st, mt)

·p(st, mt| zt−1, ut) (4) Utilizando la ley de probabilidad total se puede expresar el segundo factor de la derecha de la siguiente manera [4,5,7]:

p(st, ut| zt−1, ut) = ∫ ∫ p(st, ut| zt−1, ut) ·p(st−1, ut−1| zt−1, ut)

dst−1dmt−1 (5)

Haciendo uso nuevamente de la hip ´otesis de Markov y asu-miendo que el movimiento del robot es independiente del mapa que lleva construido, se tiene:

p(st, ut| zt−1, ut) = ηp(zt| st, mt)∫ p(st, | ut, st−1) · p(st−1, m | zt−1, ut−1)

dst−1 (6)

Por lo que la solución al problema SLAM se puede hacer de forma recursiva, necesitando en cada instante de tiempo los datos sensoriales obtenidos para ese instante de tiempo con su probabilidad y la función de probabilidad del estado en el instante anterior. La ecuación (5) se conoce como filtro de Bayes para solucionar el problema SLAM. Para evaluar la expresión (5) se requieren dos funciones de probabilidad denominadas generativas. Estas funciones corresponden al sistema odométrico y al de percepción externo. Por lo gene-ral estas funciones son invariantes en el tiempo, con lo que el modelo de medida se puede expresar como p(zt, | st, mt) y el modelo del movimiento por p(st, | ut, st−1). El proble-ma principal es que la ecuación (6) no puede implementarse, ni resolverse en su forma general, por lo que es necesaria la realización de simplificaciones, suposiciones o hip ótesis adicionales para su solución.

3. Filtro extendido de Kalman

El filtro de Kalman es un estimador recursivo lineal de m´ınimos cuadrados [8,9]. La aplicación para sistemas no lineales se hace mediante la extensión del filtro de Kalman al linealizar las ecuaciones, el cual recibe el nombre de EKF. El EKF se puede derivar de la suposición de que la probabilidad del estado p(st, mt| zt, ut) se puede modelar por una dis-tribución de probabilidad unimodal Gaussiana multidimen-sional [1,17], caracterizada por su valor esperado y su matriz de covarianzas [4,5,7]:

xt = stmt⇐⇒ x(k)

x(k) ∼ N( ˆx(k | k), P(k | k)) (7)

Por la propia naturaleza del EKF, esta solución requiere el uso de un mapa geométrico [3,11], basado en elementos discretos con determinadas caracter´ısticas geométricas para que pueda ser parametrizados en el vector de estado a esti-mar por el algoritmo [12,13]. La solución al problema SLAM con EKF tiene como principal ventaja que es capaz de man-tener la estimación completa a posteriori expl´ıcitamente de forma incremental, a costa de suponer que tanto el ruido del sistema odométrico como del sistema sensorial pueden ser aproximados por una distribución normal [3,12].

El EKF requiere para el cálculo de su matriz de covarian-za, que tanto el modelo de movimiento como el de medida, los cuales por lo general son no lineales, sean linealizados. Generalmente, se supone que esta linealización es aceptable y que los errores cometidos son pequeños, lo que es asumido por la gran mayor´ıa de las implementaciones existentes.

3.1. Formulaci´on del algortimo

Para el desarrollo del EKF se parte de la simplificaci´on de que el entorno en el cual se desplaza el robot es bidimen-sional y plano, y de que la posici´on del robot se puede repre-sentar por un sistema de coordenadas cartesianas [11,13], co-mo se ilustra en la Figura 1, con media y covarianza definidas

(3)

como: ˆxr = ˆxr ˆyr φˆr T Pr =           σ2 xrxr σ 2 xryr σ 2 xrφr σ2 xryr σ 2 xryr σ 2 yrφr σ2xrφr σ 2 xrφr σ 2 φrφr           (8)

Figura 1: Posici´on del robot con respecto al sistema global de referencia.

La observación de un objeto en el plano por parte del robot permite la creación iterativa de un mapa en el mismo sistema coordenado [14,15] como se muestra en la Figura 2. La ma-triz de covarianza Pmde este mapa incluye una correlación cruzada entre los diferentes objetos del mapa. Dado que la posición de los objetos son invariantes con el tiempo, estas correlaciones podr´ıan incrementarse con cada observación del mismo objeto a medida que el mapa es más r´ıgido. El vector de estado de la ubicación de los objetos en el mapa es también modelado por una distribución Gaussiana [11,16,17] con media y covarianza definidas por:

ˆxm=

ˆx1 ˆy1 · · · ˆxi ˆyi· · · ˆxn ˆyn T (9) Pr=                                   σ2 x1x1 σ 2 x1y1 · · · σ 2 x1xn σ 2 x1yn σ2 y1x1 σ 2 y1y1 · · · σ 2 y1xn σ 2 y1yn .. . ... ... ... σ2 xnx1 σ 2 xny1 · · · σ 2 xnxn σ 2 xnyn σ2 ynx1 σ 2 yny1 · · · σ 2 ynxn σ 2 ynyn                                  

En un instante cualquiera de tiempo el vector de estado que representa el modelo del entorno está compuesto por la unión de los vectores de estado de todos y cada uno de los objetos del entorno y el estado del robot [3,12]. Esto se plantea en la ecuación (10): ˆ Xa = "_ˆ Xr ˆ Xm # ˆ Pa = " Pr Prm XT rm Pm # (10)

Figura 2: Construcci´on incremental del mapa.

3.2. Movimiento del robot

Entre el instante k y el instante k + 1 el robot realiza un movimiento expresado por el vector ˆxδ, el cual define el

in-cremento en la posici´on y orientaci´on del robot expresado en coordenadas relativas al robot [12,18], como se puede obser-var en la Figura 3.

Figura 3: Movimiento del robot.

En la práctica, este vector es estimado por la odometr´ıa del robot, de forma no perfecta debido a los errores pro-pios del sistema odométrico de un robot móvil. Por tanto se debe modelar por una distribución Gaussiana, en la que la media es el valor de la medida odometr´ıa y su covarian-za suele ser obtenida mediante experimentación previa del sistema odométrico [3,11,17].

Realizando la hip ótesis de que el único objeto que se mueve en el mapa es el robot, la función de predicción

(4)

est´a dada por (11): ˆ X−_a = f( ˆXa, ˆXδ) = "g( ˆXr_ˆ, ˆXδ) Xm # =               

ˆxr + ˆxδcos( ˆφr) − ˆyδsen( ˆφr) ˆyr + ˆxδsen( ˆφr) + ˆyδcos( ˆφr)

ˆ φr + ˆφδ ˆ Xm                (11) y su covarianza como: ˆ P−_a =∇fxaPa∇fxaT +∇fxδPδ∇fxTδ (12)

donde las matrices Jacobianas ∇fxay ∇fxδnecesarias para el

c´alculo de la matriz de covarianzas tienen la forma:

∇fxa = ∂f ∂xa _{( ˆx} a, ˆxδ) = "_∇g xr 0rm 0rmT Im # _{( ˆx} a, ˆxδ) (13) ∇fxδ = ∂f ∂xδ _{( ˆx} a, ˆxδ) = "_∇g xδ 0rmT # _{( ˆx} a, ˆxδ) (14)

y las matrices Jacobianas ∇g_xry ∇g_xδson expresadas co-mo: ∇g_xr = ∂g ∂xr _{( ˆx} a, ˆxδ) =          

1 0 − ˆxδsen( ˆφr) − ˆyδcos( ˆφr) 0 1 ˆxδcos( ˆφr) − ˆyδsen( ˆφr)

0 0 1           (15) ∇g_xδ = ∂g ∂xδ _{( ˆx} a, ˆxδ) =           cos( ˆφr) − sen( ˆφr) 0 sen( ˆφr) cos( ˆφr) 0 0 0 1           (16)

La dispersidad de las matrices Jacobianas provoca que la ma-triz de covarianzas Pa− modifique sus valores s´olo para la

varianza en la posición del robot y el resto de objetos del en-torno, permaneciendo constantes las varianzas y covarianzas entre los objetos Pm[3,17]. Es por esto que la ecuación (12) puede escribirse de manera más eficiente como:

Pa−= " ∇gxrPr∇gxTr +∇gxδPδ∇gxT_δ ∇gxrPrm (∇g_xrPrm)T Pm # (17)

Figura 4: Movimiento del robot

3.3. Asociaci´on de datos

En el instante k + 1 el robot observa un objeto 0idel en-torno como se ilustra en la figura 4. Se considera por sim-plicidad que una observación está compuesta por la medi-da de un único objeto del entorno, siendo la formulación extendible fácilmente al caso de que el robot realice varias medidas de distintos objetos al mismo tiempo. La medida que realiza el robot se expresa como el vector relativo al mismo zk+1y su varianza Rk+1, las cuales son dadas como

[12,18,19]: zk+1 = "r θ # Rk+1 = " σ 2 r σ2rθ σ2 rθ σ 2 θ # (18) La varianza, o los par´ametros necesarios para calcular-la, se suelen obtener experimentalmente mediante ensayos a priori del sistema sensorial del robot.

El primer paso es realizar la comparación de esta obser-vación con todos y cada uno de los objetos previamente al-macenados en el mapa, para decidir si corresponde o no a alguno de dichos objetos, y a cuál en caso positivo. Si la observación se asocia correctamente a un objeto del mapa, entonces se proceda a la etapa de corrección del EKF con la información de este emparejamiento. Cuando la observación no es asociada a ning ún objeto del mapa previo, eso quiere decir que dicha observación corresponde a un objeto que ha sido visto por primera vez y deberá ser añadido al mapa.

3.4. Correcci´on del EKF

Si una observación ha sido emparejada con alguno de los objetos ya existentes en el mapa, entonces se puede hacer con este emparejamiento una mejora en la estimación del ma-pa. Dado que ya se tiene una estimación de la observación (xi, yi) previamente almacenada en el mapa, se calcula una estimación de la lectura relativa al robot para esa observación como [11,17,19]: ˆzr= hi(ˆxa) =       p ( ˆxi− ˆxr)2 + (ˆyi− ˆyr)2 arctan(ˆyi−ˆyr

ˆxi− ˆxr) − ˆφr       (19)

(5)

La observación sirve para mejorar la estimación del estado del sistema. Esta observación se puede representar en fun-ción del estado del sistema mediante el modelo no lineal ex-pl´ıcito de observación.

zk+1= hj(xm) + rk+1 (20)

En donde j representa la posici´on (xi, yi) del objeto con res-pecto al sistema global de coordenadas presentado en la figu-ra 4 y rk+1es ruido blanco Gaussiano que contamina la

me-dida del sensor real y esta dado por:

rk+1∼ N(0, Rk+1) (21)

La innovación está dada por la diferencia entre la lectura tomada por el robot z en el tiempo k + 1 y la lectura ˆzi esti-mada con la información almacenada hasta ese instante, esto es [3,12]:

vi= zk+1− ˆzi (22)

La covarianza Side la innovaci´on es calculada como:

Si=∇hxaP−a∇hxTa + R (23)

La ganancia del EKF se obtiene seg ´un la ecuaci´on (24),

wi= P−a∇hxTa S

−

i (24)

donde la matriz jacobiana ∇hxa se expresa como en la

ecuaci´on (25) ∇hx a = ∂hi ∂xa _ˆx− a = "−∆x d −∆y d 0 0 · · · 0 ∆x d ∆y d 0 · · · 0 −∆y d2 −∆x d2 − 1 0 · · · 0 −∆y d2 ∆x d2 0 · · · 0 # (25) Con ∆x = ˆxi− ˆxr ∆y = ˆyi− ˆyr

d =

q

( ˆxi− ˆxr)2 + (ˆyi− ˆyr)2

La estimación del mapa en el instante k + 1, con toda la in-formación recibida hasta ese instante se calcula como en la ecuación (26):

ˆx+a = ˆx−a + Wivi

ˆ

P+_a = Pˆ−_a − WiSiWT_i (26)

Esta corrección requiere la actualización completa de toda la matriz de covarianzas ˆPa, es esta etapa la fase más cr´ıtica

en cuesti´on de coste computacional en todo el proceso de construcci´on del mapa.

3.5. Adici´on de nuevos objetos

A medida que el robot explora el entorno va encontrando nuevos objetos los cuales deben ser almacenados en el mapa que tiene guardado hasta ese instante. Primero, el vector del estado del mapa y la matriz de covarianzas son aumentados con la nueva observaci´on z, y la covarianza R, con medidas relativas al robot [11,18,19]. ˆxaum = " ˆxa z # ˆ Paum =           Pr Prm 0 PT rm Pm 0 0 0 R           (27)

La funci´on giconvierte la lectura del objeto z dada en

coor-denadas polares al sistema global dado en coorcoor-denadas carte-sianas [20]. gi(xr, z) =" xi yi # =" xr+ rcos(θ + φr) yr+ rsen(θ + φr) # (28)

Con lo que la estimaci´on del nuevo estado, ya aumentado, est´a dado por:

ˆx+a = fi(ˆxaum) = " ˆxa gi(xr, z) # (29)

El estado de los objetos del entorno que ya exist´ıan en el ma-pa permanecen inalterados mientras que el estado del nuevo objeto se calcula como una función del estado del robot y la medida teórica. El resto de objetos del entorno distintos al robot no intervienen en esta ecuación.

La matriz de covarianza, se calcula como en la ecuaci´on (30):

P+a =∇fxaumPaumfxtaum (30)

La matriz Jacobiana es dada por:

∇fxaum= ∂fi ∂xaum _x aum =           Ir 0 0 0 Im 0 ∇g_xr 0 ∇gz           (31)

Donde las matrices Jacobianas ∇g_xry ∇gzse presentan en la

ecuaci´on (32) ∇g_xr = ∂gi ∂xr _{( ˆx}_r_,z) ="1 0 −r sen(θ + ˆφr) 0 1 r cos(θ + ˆφr) # (32) ∇gz = ∂gi ∂z _{( ˆx} r,z) ="cos(θ + ˆφr) −r sen(θ + ˆφr) sen(θ + ˆφr) r cos(θ + ˆφr) #

Gracias a la dispersión de las anteriores matrices Jacobianas, el costo computacional de la adición de nuevos objetos crece sólo linealmente con el n úmero de objetos del mapa, ya que no es necesario realizar la actualización completa de la ma-triz Pa.

(6)

Figura 5: Construcci´on de un mapa con 80 marcas en forma incremental con un control constante.

3.6. Clasificaci´on de datos

Cuando el robot toma una lectura de un objeto en su am-biente, es necesario determinar si esa lectura corresponde a un elemento ya almacenado en su mapa o si por el contrario es un nuevo objeto que se deber´a adicionar al mapa. Para realizar una asociaci´on correcta se hace necesario emplear una distancia estad´ıstica para determinar la similitud entre las variables aleatorias multidimensionales.

Para realizar la discriminaci´on con un nivel de confianza

α se realiza un test con la distancia de Mahalanobis entre el

objeto nuevo y cada uno de los que se tiene almacenado en el mapa. Esta distancia esta dada por [3,4,12,21,22]:

vT_i Sivi < χ2(dim(vi),α) (33)

Este test emplea la métrica o distancia de Mahalanobis de la innovación para aceptar la medida o rechazarla en función de la confianza deseada.

4. Discusi´on de resultados

En la figura 5 se muestra una secuencia para la construc-ci´on de un mapa en forma incremental. El mapa es generado de forma aleatoria con 80 marcas. El control para el despla-zamiento del robot es constante para todo el recorrido y es dado por la ecuaci´on (34):

ˆxδ= 0 0,15 −0,2 π 180 (34) Para el recorrido que se presenta en la Figura 5, el robot pre-senta errores en su posici´on como se muestra en las Figu-ras 6, 7 y 8. En la Figura 6 se presenta el error en la posici´on

x con ±2σ. En la Figura 7 se tiene el error en y con ±2σ y

de igual forma para el ´angulo del robot como se ilustra en la Figura 8.

Debido a que el EKF aproxima la soluci´on del fil-tro de Bayes suponiendo que la probabilidad del estado

(7)

Figura 6: Error en xrcon 2 σ [cm].

Figura 7: Error en yrcon 2 σ [cm].

Figura 8: Error en φrcon 2 σ [cm].

probabilidad unimodal Gaussiana multidimensional, es im-portante analizar si los datos obtenidos cumplen con esta dis-tribuci´on.

La inecuaci´on de Chebyshev [4,5] para una variable aleatoria X, con una media finita µ y varianza σ2, resulta

para todo n ´umero real k > 0:

P(|X − µ| < kσ) ≥ 1

k2 (35)

con lo que para un k = 2 se tiene que

P(|X − µ| < 2σ) ≥ 0,75

Es decir, la probabilidad de que esté entre dos desviaciones estándar de su media es mayor o igual que 75 %. Esta de-sigualdad se cumple independientemente de la distribución de probabilidad . Por lo tanto, con la desigualdad Chebyshev se puede analizar si los resultados obtenidos cumplen con las suposiciones del EKF.

5. Conclusiones

El EKF permitió la construcción de mapas de entornos exteriores en presencia de ruido odométrico en el robot y de incertidumbres en los sensores, construyendo mapas consis-tentes. Se concluye de las simulaciones realizadas con difer-entes plataformas y sensores, que esta metodolog´ıa requiere que ellos tengan una buena precisión para que el algoritmo converja. La suposición de que el modelo del robot y los sen-sores tiene una distribución normal implica que tengan una buena precision, de lo contrario las linealizaciones de las que parte el modelo no son adecuadas y tampoco los modelos obtenidos. La principal consecuencia de esto es que el algo-ritmo realiza malas estimaciones del robot y la posición de los objetos. Debido a que los errores odométricos son acu-mulativos, el algoritmo empezará a realizar estimaciones con errores mayores a medida que realiza más desplazamientos.

Referencias

[1] Hugh Durrant-Whyte and Tim Bailey. (2006): ((Simultaneous localiza-tion and mapping: part I)). Robotics and Automalocaliza-tion Magazine, IEEE. Vol. 13, pp. 99-110.

[2] Mark Cummins and Paul Newman. (2007): ((Probabilistic Appearance Based Navigation and Loop Closing)). International Conference on Robotics and Automation, Rome.

[3] Diego Rodr´ıguez Losada González. (2004): ((SLAM Geométrico en Tiempo Real para Robots Móviles en Interiores Basado en EKF)). Ph.D. dissertation, Escuela Técninca Superior de Ingenieros Industri-ales.

[4] Michael Lavine. (2006): ((Introduction to Statistical Thought)). Amer-ican Statistical Association. Vol. 101.

[5] E. Jaynes and G. Larry Bretthorst. (2003): ((Probability Theory: The Logic of Science)). Cambridge University Press.

[6] L. R. Rabiner. (1989): ((A Tutorial on Hidden Markov Models and Selected Applications in Speech Recognition)). Proceedings of The IEEE. Vol. 77. No. 2.

[7] Richard O. Duda and Peter E. Hart and David G. Stork. (2001):

((Pattern Classification)). John Wiley & Sons, INC. Segunda Edici ´on.

[8] R. E. Kalman. (1960): ((A New Approach to Linear Filtering and Pre-diction Problems)). Transactions of the ASME-Journal of Basic Engi-neering. No. 82, pp. 34-45.

[9] E. Kamen and J. Su. (1999): ((Introduction to Optimal Estimation)). Springer.

[10] Hugh Durrant-Whyte and Tim Bailey. (2006): ((Simultaneous local-ization and mapping: part II)). Robotics and Automation Magazine, IEEE. Vol. 13, pp. 108-117.

(8)

[11] Stefan Bernard Williams. (2001): ((Efficient Solutions to Autonomous Mapping and Navigation Problems)). Ph.D. dissertation, The Univer-sity of Sydney.

[12] Tim Bailey. (2002): ((Mobile Robot Localisation and Mapping in Ex-tensive Outdoor Environments)). Ph.D. dissertation, The University of Sydney.

[13] Juan Nieto, Tim Bailey and Eduardo Nebot. (2007): ((Recursive scan-matching SLAM)). Robotics and Autonomous Systems. No. 55, pp. 39-49.

[14] G. Dissanayake, P. Newman, H. Durrant-Whyte, S. Clark, and M. Csorba. (2001): ((A Solution to the Simultaneous Localization and Map Building (SLAM) Problem)). IEEE Transactions on Robotic and Automation. Vol. 17. No. 3, pp. 229-241.

[15] P. Newman, J. Leonard, J. Neira, and J. Tard´os. (2002): ((Explore and Return: Experimental Validation of Real Time Concurrent Mapping and Localization)). IEEE International Conference on Robotics and Automation, Washington. pp. 1802-1809.

[16] J. Castellanos, R. Martinez-Cantin, J. Tard´os and J. Neira. (2007):

((Robocentric map joining: Improving the consistency of

EKF-SLAM)). Robotics and Autonomous Systems. No. 55, pp. 21-29. [17] Jos´e E. Guivant. (2002): ((Efficient Simultaneous Localization and

Mapping in Large Environments)). Ph.D. dissertation, The Universi-ty of Sydney.

[18] Michael Csorba. (1997): ((Simultaneous Localisation and Map Build-ing)). Ph.D. dissertation, University of Oxford.

[19] Paul Michael Newman. (1999): ((On the Structure and Solution of the Simultaneous Localisation and Map Building Problem)). Ph.D. disser-tation, The University of Sydney.

[20] Tim Bailey. (2003): ((Constrained Initialisation for Bearing-Only SLAM)). IEEE International Conference on Robotics and Automation. [21] J. Castellanos, J. Montiel, J. Neira and J. Tardos. (1999): ((The SPmap: A Probabilistic Framework for Simultaneous Localization and Map Building)). IEEE Transactions on Robotics and Automation. Vol. 15. No. 5, pp. 948-953.

[22] Tim Bailey, Ben Upcroft and Hugh Durrant-Whyte. (2006):

((Validation Gating for Non-Linear Non-Gaussian Target Tracking)).