EL7009 Robótica Móvil

Tarea 4:
Filtro de Kalman Extendido con el robot Khepera

Matı́as Mattamala Aravena

Prof: Javier Ruiz del Solar

Auxiliar: Mauricio Correa
Ayudante: Fernando Bernuy

26 de Noviembre de 2013

1
Índice

1. Introducción 3

2. Marco teórico 3
2.1. Filtro de Kalman Extendido . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.1.1. Etapa predictiva . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.1.2. Etapa correctiva . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

3. Planteamiento del problema 4

4. Desarrollo del trabajo 4

4.1. Etapa predictiva . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
4.1.1. Modelo cinemático: Función f . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
4.1.2. Matriz A . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
4.1.3. Matriz Q . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
4.2. Etapa correctiva . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
4.2.1. Modelo observacional: Función h . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
4.2.2. Modelo analı́tico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
4.2.3. Matriz H . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
4.3. Matriz R . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
4.4. Resultados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
4.4.1. Configuración de la prueba . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
4.4.2. Modelo analı́tico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
4.4.3. Modelo con h interpolada . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

5. Discusión y análisis de resultados 11

5.1. Respecto a los resultados obtenidos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
5.2. Observaciones del simulador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

6. Conclusiones 12

7. Referencias 12
Facultad de Ciencias Fı́sicas y Matemáticas EL7009 Robótica Móvil
Universidad de Chile Primavera 2013

1. Introducción
En el presente trabajo se muestran los resultados obtenidos en la cuarta tarea del curso de Robótica
Móvil. En esta oportunidad se trabajará nuevamente con el robot diferencial Khepera, a través del simu-
lador Kiks disponible para MATLAB. En esta tarea se pide resolver el problema de auto-localización de
robots móviles a través de la solución clásica: el Filtro de Kalman. Sin embargo, debido a la no-linealidad
del problema, se trabajó con una versión no-lineal del filtro conocida como EKF (Extended Kalman
Filter) Este informe pretende mostrar las etapas de desarrollo realizadas para su implementación, como
ası́ también mostrar los principales resultados obtenidos con ella.

2. Marco teórico
Debido a que ya se trabajó con Kiks anteriormente, no se detallarán los aspectos técnicos relacionados a
éste, ni las especificaciones técnicas del robot Khepera. Sı́ se expondrán los conceptos nuevos introducidos
este trabajo, como el Filtro de Kalman extendido.

2.1. Filtro de Kalman Extendido

El Filtro de Kalman Extendido corresponde a la versión no-lineal del conocido Filtro de Kalman.
Este considera que la predicción del estado es una función del estado anterior y de la acción ejecutada
f (xk−1 , uk ), como ası́ también el modelo observacional es una función del estado actual, h(x−
k ).
Las etapas ejecutadas con el filtro se muestran a continuación, con las respectivas ecuaciones asociadas.
Es importante señalar que estas ecuaciones son puestas en esta sección de forma referencial, dado que son
parte del marco teórico del problema. Sin embargo, en la siguiente sección serán explicadas con mayor
detalle, indicando como se adaptaron para el caso en cuestión.

2.1.1. Etapa predictiva

Proyección del estado

x̂−
k = f (x̂k−1 , uk )

Proyección de la covarianza del error

Pk− = Ak Pk−1 ATk + Wk Qk−1 WkT

2.1.2. Etapa correctiva

Cálculo de la innovación
vk = zk − h(x̂−
k)

Covarianza de la innovación
Mk = Hk Pk− Hk−
Matching de observaciones
vk Mk−1 vk ≤ T h
Ganancia de Kalman
Kk = Pk− HkT Mk

3
Facultad de Ciencias Fı́sicas y Matemáticas EL7009 Robótica Móvil
Universidad de Chile Primavera 2013

Corrección del estado

x̂k = x̂−
k + Kk vk

Actualización de la covarianza del error

Pk = (I − Kk Hk )Pk−

3. Planteamiento del problema

El problema a resolver consiste en la implementación de un filtro de Kalman Extendido para la
localización de un robot Khepera, el cual se utiliza a través del simulador Kiks, disponible para MATLAB.

4. Desarrollo del trabajo

4.1. Etapa predictiva
4.1.1. Modelo cinemático: Función f
Para el modelo cinemático del robot Khepera se utilizó una expresión analı́tica, dada por la cinemática
directa de un robot diferencial. La expresión de la función f es las siguiente:
vL −vR
xr + ( vR +v
 
2
L
) cos(θr + 2L )
 
 
−vR
f (x, y, θ, vR , vL ) =  yr + ( vR +vL
) sin(θr + vL2L )
 
 2 

 
vL −vR
θr + L

Donde se ha utilizado L = 53[mm], correspondiente a la distancia entre las ruedas del robot Khep-
era. Es importante identificar que en la expresión anterior las variables (x, y, θ) corresponden al estado
xk−1 , mientras que las entradas (vL , vR ) corresponden a la acción uk que se observaron en las ecuaciones
anteriores.

4.1.2. Matriz A
La matriz A permite caracterizar como afecta el movimiento efectuado en la covarianza Pk del estado
x−
k. Esta se define como
A(x, y, θ, vR , vL ) = J(f ) (1)
donde J(f ) denota el jacobiano de la función f respecto a (x, y, θ). Al desarrollar el jacobiano analı́tica-
mente se obtiene:
vL −vR
1 + ( vR +v
 
2
L
) cos(θr + 2L ) 0 0
 
 
vL −vR
A(vR , vL ) =  0 yr + ( vR +v L
) sin(θr + 2L ) 0
 
 2 

 
vL −vR
0 0 1+ L

4
Facultad de Ciencias Fı́sicas y Matemáticas EL7009 Robótica Móvil
Universidad de Chile Primavera 2013

4.1.3. Matriz Q
La matriz Q permite caracterizar el error de odometrı́a del robot. En clases se vieron dos maneras de
caracterizar esta covarianza: Una empı́rica y otra analı́tica. La empı́rica considera la realización de pruebas
con un robot real, midiendo la posición esperada y la obtenida mediante la medición de la odometrı́a,
de modo de realizar estadı́stica sobre los datos y caracterizar este error. La segunda forma considera la
existencia de un modelo analı́tico del error de odometrı́a, y se pre y post multiplica por una matriz W
(W QW T ), correspondiente al Jacobiano de la funcion f pero respecto a las ordenes dadas al sistema. Esta
segunda caracterización permite obtener un modelo dinámico del error que permite responder de forma
distinta frente a distintas órdenes.
Para motivos de este trabajo, se realizó una mezcla de las dos caracterizaciones: se utilizó una matriz
Q constante para estimar el error de odometrı́a pero se realizó la multiplicación anterior, de modo de
considerar el efecto de las ordenes efectuadas.

4.2. Etapa correctiva

4.2.1. Modelo observacional: Función h
La función h corresponde al modelo observacional del robot, el cual permite estimar las observaciones
que deberı́a tener el sensor dado que se conoce la pose del robot estimada en la etapa predictiva x− k . Esta
correspondió a la etapa más difı́cil de determinar en el trabajo, debido a una serie de problemas que serán
explicados en las próximas secciones. Para determinar la función h se utilizaron tres métodos distintos,
que se presentan a continuación:

4.2.2. Modelo analı́tico

El primer enfoque utilizado correspondió al diseño de un modelo analı́tico de las observaciones. Para
ello, se utilizó la información disponible en el Manual de Usuario del robot Khepera [1] y en la tesis de
magı́ster de Theodor Nilsson [2], quien desarrolló Kiks.
El procedimiento para calcular h se resume en el siguiente algoritmo:

Algorithm 1 Algoritmo para calcular h

Require: Pose, Beacons
1: Calcular la posición del sensor x~s respecto al sistema de referencia global.
2: for cada Beaconi do
3: Calcular distancia del sensor al beacon
4: Calcular ángulo del sensor al beacon θbs
5: Transformar distancia en medición de sensor, R (ver 1).
6: Ajustar la medición de acuerdo al ángulo usando la relación Ro + (1 − cos(θbs ))|500 − R|
7: end for
8: return Medición del beacon que se encuentra más cerca

Este enfoque analı́tico, a pesar de ser correcto teóricamente, no permitió obtener buenos resultados,
ya que los sensores no quedaban bien caracterizados.

5
Facultad de Ciencias Fı́sicas y Matemáticas EL7009 Robótica Móvil
Universidad de Chile Primavera 2013

Figura 1: Curva del sensor del Khepera

Modelo interpolado
Dado que el modelo analı́tico no realizaba una buena caracterización del sensor, se analizaron otras
alternativas. La primera de ellas consistió en realizar un muestro de las mediciones realizadas por el sensor
para observar el comportamiento. Para ello se implementó un script en MATLAB, sensor sampling.m,
el cual permitió obtener una serie de datos del sensor 8 en una grilla. La configuración utilizada se observa
en la figura 2.
El objetivo de esta prueba era medir la pose (x, y) del robot y la medición del sensor ss , de modo de
obtener una función que permitiera obtener la medición esperada del sensor dada la distancia (dx , dy ) que
existe entre el sensor y el beacon:
ss = sensormodel(dx , dy ) (2)
Para ello, los datos fueron interpolados mediante la herramienta cftool del toolbox de ajuste de curvas
de MATLAB, con el que se obtuvo el siguiente modelo (figura 3):
Este modelo interpolado funciona de manera equivalente a una función, por lo que no existieron
problemas para el cálculo de la función H, que se explica más adelante. Este modelo permitió obtener
una aproximación mejor que el modelo analı́tico, ya que fue obtenida de una forma más pragmática.

Modelo con Redes Neuronales

La ultima alternativa evaluada de utilizar un modelo de Redes Neuronales para realizar el modelo
del sensor, de modo análogo a lo realizado con interpolación. La ventaja de este método, es que la
red neuronal permitirı́a obtener un modelo continuo y suave del sensor. Sin embargo, a pesar de que
se realizó un script que permitı́a entrenar la red de forma rápida (nn sensor model sampling.m) y se
obtenı́a una caracterización suave del sensor, su desempeño al ser evaulada de forma iterativa era bastante
poco eficiente, por lo que finalmente fue descartada.

6
Facultad de Ciencias Fı́sicas y Matemáticas EL7009 Robótica Móvil
Universidad de Chile Primavera 2013

Figura 2: Prueba realizada para la obtención de datos del sensor del Khepera. El beacon se encuentra al centro.
Los puntos amarillos denotan las posiciones donde se ubicaba el robot para realizar el muestreo.

Figura 3: Gráficos ilustrativos del modelo del sensor obtenido mediante interpolación. A la izquierda se muestra el
grafico real y a la derecha un modelo que ilustra el esperado. En el plano x-y se muestra la distancia del sensor al
beacon, y en z la medición del sensor.

4.2.3. Matriz H
Para el cálculo de la matriz H, definida como el jacobiano de h respecto a las órdenes uk , existı́an dos
caminos: por un lado, se puede realizar la derivación analı́tica, mientras que la otra alternativa corresponde
a una aproximación discreta numérica de la derivada. Por motivos prácticos de la forma de implementación
de la función h, que ya fue vista en la sección anterior, se escogió la alternativa numérica, que corresponde

7
Facultad de Ciencias Fı́sicas y Matemáticas EL7009 Robótica Móvil
Universidad de Chile Primavera 2013

a la siguiente ecuación:
∂hi hi (x− −
j,k + ∆, 0) − hi (xj,k − ∆, 0)
H[ij],k = = (3)
∂x−
j,k 2∆
Esta caracterización posee más errores, pero permitı́a obtener una aproximación de la derivada, ya
que modelos como el de interpolación no son directamente analı́ticos.

4.3. Matriz R
La matriz R representa los errores observacionales que produce el sensor. Para estimarla, se desarrolló el
script estimate R.m, el cual realiza un procedimiento análogo al utilizado para caracterizar el sensor con
interpolación de puntos. En este caso, también se realizó un desplazamiento del robot a través de una
grilla, pero se realizaron mediciones con los 8 sensores, y se calcularon las estimaciones de la función h
interpolada determinada anteriormente. Una vez realizadas las iteraciones y obtenidas las muestras, se
calculó
zerror = zo bs − ẑ (4)
Obteniéndose una matriz de N × 8, con N el número de muestras. Sobre esta matriz se aplicó la
covarianza, obteniéndose la matriz R de 8 × 8 esperada.

4.4. Resultados
4.4.1. Configuración de la prueba
Para la realización de pruebas del modelo, se realizó una adaptación de la demo avoid.m disponible
en Kiks. Esta demo ejecuta una rutina de movimiento aleatorio del robot Khepera, de borde a borde de la
arena. Esta prueba permitirı́a probar una situación aleatoria que podrı́a resultar bastante representativa
del robot funcionando en un entorno real, lo que a fin de cuentas terminó produciendo bastantes problemas
en la estimación debido los choques.

4.4.2. Modelo analı́tico

En general no se obtuvieron buenos resultados con el modelo analı́tico: La predicción realizada por
la odometrı́a era como se esperaba, con errores que aumentaban en el tiempo, pero las observaciones no
tuvieron el impacto esperado. El comportamiento del error con el modelo analı́tico se observa en la figura
4, mientras que una secuencia de movimiento obtenida con el simulador se muestra en la figura 5.

4.4.3. Modelo con h interpolada

Para el modelo con el Filtro de Kalman con función h interpolada tampoco se obtuvieron buenos
resultados. Al momento de cumplir el criterio de la innovación con el umbral T h y considerar las obser-
vaciones, se producı́a un salto en el estado que no se recuperaba, debido a que la corrección inducida
por las observaciones era del orden de 1000, afectando enormemente la pose estimada (ver figura 6 y 7).
Se realizaron pruebas para analizar el algoritmo, y la función h no realizaba buenas predicciones de las
observaciones a pesar del enfoque con que fue diseñada sugerı́a lo contrario.

8
Facultad de Ciencias Fı́sicas y Matemáticas EL7009 Robótica Móvil
Universidad de Chile Primavera 2013

Figura 4: Error en las 3 componentes de la pose para el Filtro de Kalman con función h analı́tica. Se observa que
existen peaks donde el error es mayor, probablemente asociados a momentos en que el robot colisionaba con las
paredes.

Figura 5: Resultados observados en la simulación con el modelo analı́tico. En cyan la localización por ground truth
y en verde la predicción del Filtro de Kalman. Claramente se observa que después de varias iteraciones el error de
localización es enorme, y el robot tiene una predicción completmente errónea.

9
Facultad de Ciencias Fı́sicas y Matemáticas EL7009 Robótica Móvil
Universidad de Chile Primavera 2013

Figura 6: Error en las 3 componentes de la pose para el Filtro de Kalman con función h interpolada.Se observa
un error enorme debido a los problemas con el modelamiento de h. Debido al cambio de magnitud, el resto de las
variaciones se hace despreciable.

Figura 7: Resultados observados en la simulación con el modelo con h interpolada. Se observa que existe un error
en el modelo, ya que cuando se comienzan a percibir observaciones útiles por el criterio de innovación, existe un
salto en la predicción que no vuelve a recuperar.

10
Facultad de Ciencias Fı́sicas y Matemáticas EL7009 Robótica Móvil
Universidad de Chile Primavera 2013

5. Discusión y análisis de resultados

5.1. Respecto a los resultados obtenidos
En lo que respecta a los resultados obtenidos, como se apreció anteriormente no se obtuvieron re-
sultados esperados: El filtro de Kalman implementado nunca logró converger correctamente a la pose
correcta, a pesar de que teóricamente los conceptos e ideas propuestas eran correctas. Los errores exis-
tentes se produjeron principalmente por la formulación de la función h utilizada, la cual no realizaba una
buena estimación de las observaciones en ninguno de los dos casos, por lo que tampoco se efectuaron las
correcciones esperadas.
Respecto a la predicción odométrica, se observó el comportamiento esperado, con una predicción
bastante cercana a la realidad en las primeras iteraciones, pero con un pequeño error que aumentaba
considerablemente al recorrer grandes distancias. Eso, sumado al hecho de que las rotaciones influyen
en la determinación de la pose y que las correcciones no funcionaron como deberı́an, finalmente siempre
acabaron en una predicción completamente errónea.
Otro fenómeno que se pudo apreciar en el trabajo, y que es posible ver en las figuras anteriores respecta
al comportamiento de la odometrı́a frente a choques: debido a la naturaleza del choque, la predición es
completamente errática, y es la principal culpable de un aumento considerable del error en los resultados.

5.2. Observaciones del simulador

Una de las observaciones principales que respectan a Kiks que fueron observadas durante su uso en
esta tarea, respectan al comportamiento del robot y sus sensores. En general se observó que existen
dos fenómenos bastante curiosos asociados a los dos principales sensores del robot: los sensores de las
ruedas, y los sensores de luminosidad. Respecto a los primeros, utilizando la función kGetEncoders, la
cual entrega la posición de la rueda en número de pulsos, se observó en reiteradas ocasiones que sus
mediciones no se relacionan con el estado actual del robot, y actúan como si existiera un reloj interno del
robot independiente del sistema. Esto podı́a apreciarse de forma fácil con el siguiente código (disponible
en el script velocityTest.m):

for i=1:100
odo=kGetEncoders(ref);
kiks_ksend([’D,1,1’ 10],ref)
pause(TIME);
new_odo=kGetEncoders(ref);
other_odo=kGetEncoders(ref);
disp(new_odo-odo);
disp(other_odo);
end

Este código ejecuta una rutina simple de movimiento del robot, produciendo que se mueva en lı́nea
recta. Se espera que el robot realice una pausa de TIME segundos en cada iteración, antes de continuar con
la siguiente. El problema ocurrı́a justamente al variar T IM E, ya que las mediciones de odometrı́a variaban
de manera proporcional al tiempo que se mantuviera pausado. Esto no hace sentido si se considera que
se está utilizando la misma velocidad en todas las pruebas.

11
Facultad de Ciencias Fı́sicas y Matemáticas EL7009 Robótica Móvil
Universidad de Chile Primavera 2013

Respecto a los sensores de luminosidad, se apreció que era bastante difı́cil caracterizarlos, ya que cada
sensor respondı́a de forma distinta. Utilizando el mismo modelo observacional para todos, no se obtenı́an
resultados similares, y esa podrı́a ser una de las causas de la falla del segundo modelo propuesto.

6. Conclusiones
El trabajo desarrollado permitió comprender las complejidades que involucra la implementación de
un algoritmo de localización como lo es el filtro de Kalman.

Se observó que a pesar de que en su concepto es sencillo e intuitivo, su implementación puede resultar
bastante complicada si no se realiza un correcto modelamiento de las funciones que involucra. En
particular desarrollar un correcto modelo observacional es vital para el correcto desempeño del filtro.

A pesar de que no se lograron resultados satisfactorios, el trabajo realizado permitió comprender

una gran cantidad de conceptos relacionados al problema de auto localización de robots móviles.

Se espera que la experiencia lograda en el desarrollo de este trabajo, ya sea la relacionada con los
algoritmos de localización, como también con las soluciones propuestas para solucionar el problema
que existı́a en la caracterización del modelo, pueda ser utilizada a futuro en otros problemas. En
particular, se espera que los errores cometidos en el desarrollo de este trabajo puedan ser consider-
ados y corregidos en la siguiente tarea de auto localización mediante Filtros de Partı́culas, de modo
de ahora si obtener un modelo de estimación de estado como es esperado.

7. Referencias
[1] K-Team, Khepera User Manual, Laussane, 1999.

[2] T. Nilsson, KiKS is a Khepera Simulator - Master thesis.

[3] J. Ruiz-del-Solar, Filtro de Kalman, Apuntes del curso EL7009 Robótica Móvil, 2013.

12