comparativa algoritmos fusion

7/29/2019 Comparativa Algoritmos Fusion

1/24

Grupo de Automtica, Robtica y Visin Artificial

Departamento de Fsica, Ingeniera de Sistemas y Teora de la Seal

Comparativa

Algoritmos Fusin

MoCap-UWBPublicacin Interna 14

04-04-2008

Francisco A. CandelasHeras

Juan A. Corrales Ramn


2/24

PubInt14

Comparativa Algori tmos Fusin MoCap-UWB

1

1.-Motivacin de la fusin sensorialEn dos informes anteriores [1, 2], presentamos una descripcin pormenorizada de los dos

sistemas de localizacin de los que disponemos: GypsyGyro-18 y Ubisense. El anlisis

desarrollado nos ha permitido valorar con datos empricos cules son las fortalezas y las

debilidades de ambas tecnologas. En la siguiente tabla se comparan entre s:

Ventajas Inconvenientes

GypsyGyro

Frecuencia de muestreo alta (60-120FPS).

Muestreo constante y estable. Localizacin de todas las articulaciones

del cuerpo.

Alta precisin en las medidas derotacin de las articulaciones (errores

inferiores a 1).

Infraestructura necesaria para lainstalacin reducida.

Baja precisin en la medida global detraslacin del nodo raz debido a la

acumulacin de errores (drift) a lo

largo del tiempo.

Las interferencias magnticas causanerrores en los ngulos de guiada

(rotacin Z).

Sistema de coordenadas establecidoen cada inicializacin del sistema.

Ubisense

Errores asumibles en las medidas detraslacin (errores inferiores a 20cm).

Inmunidad ante interferenciaselectromagnticas.

Infraestructura necesaria para lainstalacin reducida.

Sistemas de coordenadas fijo.

Frecuencia de muestreo baja (4-7FPS).

Frecuencia de muestreo variable,dependiente de las condiciones de

recepcin de los pulsos UWB.

Localizacin global de la persona sininformacin de las articulaciones.

Tabla 1. Comparativa entre los sistemas GypsyGyro-18 y Ubisense.

Si comparamos las ventajas e inconvenientes de ambos sistemas, podemos comprobar

como son complementarios. As, mientras que la frecuencia de registro de medidas del

Ubisense es baja y variable, en el GypsyGyro es alta y estable. En entornos industriales se

buscan sistemas con muestreos altos para evitar latencias altas en el funcionamiento del

sistema. Por otra parte, el GypsyGyro presenta errores en los datos de traslacin global del

usuario (localizacin del giroscopio situado en las caderas) mientras que el Ubisense

registra con precisin la localizacin de la etiqueta que lleva el usuario.


3/24

AUROVA F. A. Candelas, J. A. Corrales

2

La motivacin inicial para el uso del Ubisense era la acumulacin progresiva de errores

(drift) en las medidas traslacionales del GypsyGyro. Por ello, la primera estrategia en la

que podramos pensar es sustituir simplemente las medidas traslacionales del GypsyGyro

con las medidas devueltas por el Ubisense. De este modo, aumentaramos la precisin

traslacional del sistema. No obstante, esta estrategia no es idnea debido a las deficiencias

del sistema Ubisense. En particular, no es conveniente utilizar slo las medidas

procedentes del Ubisense porque su frecuencia de muestreo es demasiado baja. Esta baja

frecuencia en el registro de las medidas da lugar a que durante intervalos de tiempo

demasiado largos no se reciba informacin de localizacin del Ubisense.

Por ello, es necesario combinar las medidas traslacionales de ambos sistemas para mitigar

sus deficiencias. Por una parte, la alta frecuencia de muestreo del GypsyGyro permitir

corregir las altas latencias del Ubisense. De este modo, en los intervalos temporales donde

no se reciban medidas del Ubisense, podremos utilizar las medidas del GypsyGyro. Por

otra parte, las medidas del Ubisense permitirn eliminar el error acumulado por las

medidas traslacionales del GypsyGyro.

En un informe anterior [3] se presenta una primera aproximacin a un algoritmo de fusin

de las medidas de ambos sistemas. Este algoritmo tiene como principal inconveniente su

dependencia directa de las medidas obtenidas del sistema Ubisense. Cada vez que se

registra una nueva medida del Ubisense, sta se establece como nuevo punto en la

trayectoria para corregir el error que ha ido acumulado el GypsyGyro. Por ello, cuando el

Ubisense suministra medidas muy espaciadas en el tiempo, se producen saltos en la

trayectoria al pasar de la ltima medida del GypsyGyro a la medida actual del Ubisense.

Estos saltos son importantes (de ms de 50cm) cuando el humano se desplaza ya que el

Ubisense presenta una frecuencia de muestreo insuficiente y devuelve muy pocas medidas

diferentes. Adems, este algoritmo slo corrige la acumulacin de error del sistema

GypsyGyro pero no tiene en cuenta el posible error de las medidas del sistema

Ubisense. Por ello, se vuelve necesario desarrollar un nuevo algoritmo que tenga en cuenta

los errores de ambos sistemas y que evite la aparicin de saltos excesivos en la trayectoria

cuando se reciba una medida del Ubisense muy espaciada en el tiempo con respecto a la

anterior.


4/24

PubInt14


3

En primer lugar presentaremos las distintas tcnicas utilizadas en investigaciones previas

para combinar medidas procedentes de sensores inerciales (como los sensores del

GypsyGyro) con otros tipos de sensores. A continuacin, teniendo en cuenta dichas

tcnicas, expondremos los tres algoritmos que hemos desarrollado. Finalmente,

presentaremos varios experimentos que nos han permitido comparar estos algoritmos.

2.-Trabajo previo en fusin sensorialLa estimacin de la localizacin (posicin y orientacin) mediante sensores inerciales es

un campo ampliamente estudiado y aplicado en distintos mbitos: navegacin de

vehculos, realidad virtual (VR), realidad aumentada (AR), robtica y sistemas de captura

de movimiento. En estos sistemas, las posiciones y orientaciones del elemento a localizar

se calculan mediante la integracin de las aceleraciones y velocidades angulares devueltas

por acelermetros y giroscopios, respectivamente. Sin embargo, este proceso de

integracin va acumulando un pequeo error (drift) a lo largo del tiempo que se vuelve

considerable en unos pocos segundos. Por ello, los sistemas de localizacin basados en

sensores inerciales suelen utilizar sensores adicionales (P. Ej.: GPS, ultrasonidos, sensores

magnticos, cmaras, transceptores UWB y WiFi) que devuelven medidas de localizacin

absolutas que permiten corregir el error acumulado. En estos sistemas de localizacin

hbridos se utilizan tcnicas bayesianas para estimar de manera estadstica la localizacin

(estado del sistema) a partir de las medidas (observaciones) con ruido de los sensores.

Existen dos tcnicas principales a la hora de implementar la definicin abstracta del filtro

de Bayes [4]: el filtro de Kalman y el filtro de partculas. A continuacin, vamos a

presentar las diferentes adaptaciones de estas dos tcnicas utilizadas en investigaciones

anteriores para combinar medidas de diferentes sensores.

Una primera aproximacin para integrar las medidas de varios sensores en un filtro de

Kalman es mediante la utilizacin de un filtro de Kalman complementario. Los filtros de

Kalman complementarios estiman los errores de las medidas de los sensores sin cambiar la

estructura interna genrica del filtro. Estos filtros reciben como entrada la diferencia (error)

entre las medidas de los sensores en lugar de las medidas directas de los sensores. Foxlin

ha desarrollado una IMU (Inertial Measurement Unit) basada en un filtro de Kalman

complementario para el seguimiento de la cabeza de una persona en entornos virtuales [5]


5/24


4

y un sistema de navegacin [6] compuesto por dos IMUs y un receptor GPS. Roetenberg

[7] ha desarrollado un sistema de captura de movimiento que combina un sistema inercial y

un sistema magntico mediante un filtro de Kalman complementario.

Otra forma de combinar medidas de distintos sensores en un filtro de Kalman es definiendo

un modelo de observacin para cada sensor e incluyendo sus medidas en la fase de

correccin del filtro. De este modo, la fase de correccin del filtro de Kalman tendra un

canal para cada sensor con su modelo de observacin correspondiente. Los resultados de

cada uno de los canales seran ponderados entre s para obtener la medida final de la etapa

de correccin del filtro. Caron et al. [8] utilizan esta adaptacin del filtro de Kalman para

combinar las medidas de una IMU y un receptor GPS mientras que You y Neumann [9]

combinan las medidas de tres giroscopios y una cmara.

Ribo et al. [10, 11] utilizan otra adaptacin del filtro de Kalman que consiste en aplicar las

medidas de un sensor en la etapa de prediccin del filtro y las medidas del otro sensor

en la etapa de correccin. Han desarrollado un sistema de realidad aumentada

combinando las medidas de una IMU (que se aplican en la fase de prediccin) y de unacmara (que se aplican en la fase de correccin).

Los filtros de partculas son la otra tcnica bayesiana que se utiliza ms comnmente en la

fusin sensorial. Cuando se incluyen varios sensores en un filtro de partculas, se suele

definir un modelo de observacin para cada tipo de sensor. Mediante estos modelos de

observacin se representa la probabilidad de obtener una determinada medida de un sensor

cuando el sistema se encuentra en un estado determinado. Las partculas del filtro que

representan la probabilidad de cada estado del sistema sern ponderadas mediante unos

pesos obtenidos a partir de estos modelos de observacin. Ababsa y Mallem [12] han

desarrollado un filtro de partculas con dos canales de ponderacin de partculas distintos

para combinar las medidas de una IMU y una cmara en un sistema de seguimiento de una

cmara. Jourdan et al. [13] tambin definen dos modelos de observacin distintos (para

una IMU y un sistema UWB) cuyo producto se utiliza como peso para las partculas del

filtro. Hightower y Borriello [14] no slo establecen dos modelos de observacin distintos


6/24

PubInt14


5

(para un sistema de infrarrojos y un sistema de ultrasonidos) sino que tambin ajustan el

nmero de las partculas mediante un mtodo de muestreo adaptativo.

Los filtros de Kalman y de partculas no slo se utilizan de manera separada sino que

tambin se combinan en muchos sistemas para aprovechar sus ventajas. Evennou y Marx

[15] han desarrollado un sistema de localizacin en interiores a partir de dos sensores

inerciales (un giroscopio y un acelermetro) y un sistema de localizacin basado en WiFi.

Las medidas de los sensores inerciales son corregidas mediante un filtro de Kalman antes

de aplicarse en la etapa de prediccin de un filtro de partculas mientras que las medidas

WiFi se aplican en la etapa de correccin de dicho filtro. Schon et al. [16] definen un filtro

de partculas multi-frecuencia que integra medidas de dos sensores que tienen distintas

frecuencias de muestreo (sensor rpido y sensor lento). Se ejecuta un filtro de Kalman para

cada partcula del filtro cuando se recibe una medida del sensor rpido. Cuando se recibe

una medida del sensor lento, se ejecuta la etapa de correccin del filtro de partculas

(clculo de los pesos y remuestreo de partculas). De este modo, el muestreo de partculas

se reduce a la frecuencia del sensor lento y se mejora el rendimiento del sistema. Los

filtros de partculas Rao-Blackwellized son otra forma de combinar los filtros de Kalman ypartculas. Un filtro de partculas Rao-Blackwellized divide el vector de estado en dos

componentes: un componente gaussiano lineal (que se procesa con un filtro de Kalman) y

un componente no gaussiano no lineal (que se procesa con un filtro de partculas). Giremus

et al. [17] implementa un filtro de partculas Rao-Blackwellized para integrar las medidas

de sensores inerciales y de un GPS en un sistema de navegacin.

A continuacin vamos a describir los tres algoritmos que hemos desarrollado para

combinar las medidas de nuestros dos sistemas: sistema de captura de movimiento inercial

y sistema de localizacin basado en pulsos UWB. Estos tres algoritmos se fundamentan en

las tcnicas bayesianas que hemos descrito en este apartado: filtro de Kalman, filtro de

partculas y combinacin de ambos. Describiremos en detalle las adaptaciones de estos

filtros que hemos llevado a cabo para incorporar las medidas de los sensores y

realizaremos una comparativa de su funcionamiento.


7/24


6

3.-Algoritmos de fusin sensorial3.1.-Transformacin de coordenadasAntes de combinar las medidas de ambos sistemas, tenemos que representarlas en el

mismo sistema de coordenadas. Las medidas procedentes del sistema Ubisense son

representadas en un sistema de coordenadas fijo U en el rea de localizacin. Las medidas

del sistema GypsyGyro son representadas en un sistema de coordenadas dinmico G cuyo

origen se fija en el lugar en el que el usuario se encuentra cuando el sistema se inicializa.

Hemos seleccionado el sistema de coordenadas U del Ubisense ya que al ser fijo permite

comparar de manera sencilla la posicin del humano con la posicin de los objetos

estticos (como los robots) del entorno.

Los planos XY de los sistemas de coordenadas U y G son paralelos al plano del suelo del

entorno. Por ello, entre ambos sistemas de coordenadas slo hay una traslacin

( ), ,U U UG G Gx y z y una rotacin de un ngulo alrededor del eje Z. La ecuacin (1) se utiliza

para transformar un punto Gp del sistema G al sistema U:

( ) ( ), , ,U T G U U U U GG G G Gp T p Trans x y z Rot z p= = (1)

Desarrollando la ecuacin anterior obtenemos la siguiente ecuacin:

( ) ( )

( ) ( )

cos sin 0

sin cos 0

0 0 1

1 0 0 0 1 1

U U G

G

U U G

G

U U G

G

x x x

y x

z z x

=

(2)

El parmetro ser conocido ya que representa el ngulo entre la direccin del norte geo-

magntico (medido con los magnetmetros de las IMUs del GypsyGyro) y el eje Y del

sistema de coordenadas del Ubisense. El nico elemento desconocido ser la matriz de

traslacin UG

T entre ambos sistemas de coordenadas. Se puede calcular a partir de la

ecuacin (2) sustituyendo en ella dos medidas de ambos sistemas que se correspondan

entre s (vanse las ecuaciones (3), (4) y (5)):

( ) ( )cos sinU U G GGx x x y = + (3)


8/24

PubInt14


7

( ) ( )sin cosU U G GGy y x y = (4)

( ) ( )cos sinU U G GGx x x y = + (5)

Despus de obtener la matriz de transformacinU

GT , todas las medidas del sistema de

captura de movimiento se transformarn al sistema de coordenadas del sistema UWB

mediante la ecuacin (1).

3.2.-Algoritmo basado en filtro de KalmanEl filtro de Kalman es una de las tcnicas ms ampliamente utilizadas a la hora de

implementar un filtro bayesiano. Para poder incorporar las medidas de dos sistemas de

localizacin en un filtro de Kalman, se modificar el filtro genrico [18] siguiendo una

tcnica descrita en [19].

El estado del filtro de Kalman se modela mediante dos parmetros: la estimacin del

estado t

x y la matriz de covarianza del errort

P. El estado t

x est compuesto por las

coordenadas ( ), ,p x y z= de la posicin global del operador humano en el entorno. Estos

dos parmetros se calculan en el filtro de Kalman en dos etapas: prediccin y correccin.

En la etapa de prediccin, se aplica (vanse las ecuaciones (6) y (7)) un modelo lineal

(matriz A ) a la estimacin previa del estado para obtener una estimacin a priori del

estado ( ) ,t tx P . Q es la matriz de covarianza del error de este modelo de movimiento:

1

t tx Ax

= (6)

1T

t tP AP A Q

= + (7)

Las medidas de posicin registradas por el sistema de captura de movimiento se aplican

directamente en la etapa de prediccin al utilizar una matriz identidad como modelo de

movimiento A . En la etapa de correccin, la informacin sensorialt

z se incorpora en el

filtro mediante un modelo de observacin lineal (matriz H) para corregir el estado

estimado en la etapa de prediccin (vanse las ecuaciones (8), (9) y (10)). R es la matriz

de covarianza del error del modelo de observacin:


9/24


8

( )T Tt t tK P H HP H R

= + (8)

( )

t t t t t x K z Hx

= +

(9)

( )t t tP I K H P

= (10)

Las medidas de posicin obtenidas a partir del sistema UWB se utilizan como

observacionest

z en esta etapa del filtro con el objetivo de corregir la posicin estimada en

la etapa de prediccin a partir de las medidas del sistema de captura de movimiento. El

modelo de observacin H es una matriz identidad ya que las medidas del sistema UWB

tiene la misma dimensin que el estado. Las matrices de covarianza de ruido Q y R son

matrices diagonales ya que las variables del vector de estado no estn correlacionadas. Los

trminos en la diagonal de estas matrices representan el error medio de cada sistema

(sistema de captura de movimiento y sistema UWB) y se ajustan de manera emprica para

reducir los errores en las estimaciones finales del estado.

En la Tabla 2 se muestra en detalle la implementacin realizada del filtro de Kalman. La

etapa de prediccin se ejecuta cada vez que se registra una medida del sistema de captura

de movimiento (lnea 9). La etapa de correccin se ejecuta solamente cuando se recibe una

medida del sistema UWB (lnea 12). Despus de ejecutar la etapa de correccin, la matriz

de transformacin UG

T se re-calcula cuando se recibe la siguiente medida del sistema de

captura de movimiento (lnea 6). De este modo, la estimacin del estado obtenida a partir

de la etapa de correccin se aplica a las siguientes medidas del sistema de captura de

movimiento (lnea 8) y se elimina as el error acumulado. Entre cada par de medidas del

sistema UWB, las medidas procedentes del sistema de captura de movimiento se utilizarn

para mantener una tasa de muestreo alta.


10/24

PubInt14


9

Algoritmo basado en Filtro de Kalman

01: Inicializar los parmetros del filtro: A , H,1

P, Q , R ,

02: Inicializar U GT con las dos primeras medidas: 1p , 2p {Ec. (2)-(5)}

03:for each medidat

p

04: ift

p es una medida del sistema de captura de movimiento (MoCap)

05: if1t

p

es una medida del sistema UWB

06: Recalcular UG

T cont

p ,1t

x

{Ec. (2)-(5)}

07: end if

08: Transformart

p del sistema G al U {Ec. (1)}

09: [ ] ( )1, PrediccionKalman , , ,t t t t P A Q p P= {Ec. (6), (7)}

10: Almacenar tx como estimacin del estado en el instante t11: else if

tp es una medida del sistema UWB

12: [ ] ( )1 1, CorreccionKalman , , , ,t t t t t P H R p x P = {Ec. (8)-(10)}

13: Almacenart

x como estimacin del estado en el instante t

14: end if

15: end forTabla 2. Pseudocdigo del algoritmo de fusin sensorial basado en el filtro de Kalman.

3.3.-Algoritmo basado en filtro de partculasLos filtros de partculas [20] son mtodos secuenciales de Monte Carlo que se utilizan para

implementar filtros de Bayes. Aproximan la probabilidad ( )t

bel x de un determinado

estadot

x en el instante t por un conjuntot

S de muestras o partculas (vase la ecuacin

(11)). Cada partcula [ ]it representa una estimacin del estado (posicin del humano en

nuestro caso) y se le asocia un peso positivo [ ]it

w (denominado importance factor) que

representa la verosimilitud de la estimacin del estado:

( ) { }[ ] [ ], 1, ,i it t t t bel x S x w i n = = K (11)

El filtro de partculas implementado en esta investigacin sigue un esquema SIR (Sampling

Importance Resampling). Este algoritmo se compone de tres fases principales: prediccin,

clculo de pesos y remuestreo (resampling). En la fase de prediccin, se muestrea un

conjunto de partculastS (ecuacin (12)) a partir del modelo de movimiento ( )1t tp x x

(distribucin de probabilidad de la transicin de estados). En nuestro sistema en esta fase


11/24


10

se aade a la posicin devuelta por el sistema de captura de movimiento un ruido

gaussiano de media cero y matriz de covarianza Q .De este modo, se obtiene un conjunto

de npartculas a partir de una nica medida del sistema de captura de movimiento. La

matriz Q representa la covarianza del error del sistema de captura de movimiento y se

calcula de la misma forma que en el filtro de Kalman del apartado anterior.

{ } ( )[ ] 11, ,i

t t t t S x i n p x x

= = K (12)

En la fase de clculo de pesos, se obtiene el peso [ ]itw de cada partcula a partir del modelo

de observacin ( )[ ]i

t tp z x . Esta probabilidad se calcula como una distribucin normal

multi-variable ( ),tN z R en la que se utiliza la ltima medida tz del sistema UWB como

media y se utiliza la matriz R como covarianza. Esta distribucin normal se evala para

cada partcula [ ]it muestreada en la fase de prediccin. La matriz de covarianza R

representa la covarianza del error del sistema UWB y se calcula de la misma manera que

en el filtro de Kalman del apartado anterior. Finalmente, los pesos de todas las partculas

son normalizados para asegurar que suman uno en todo el espacio de las partculas tS

.

( )[ ] [ ]i it t tw p z x= (13)

Los filtros SIS (Sequential Importance Sampling) estn tambin compuestos por estas dos

fases. Sin embargo, en muchos casos, despus de varias iteraciones del filtro, slo quedan

unas pocas partculas con peso no nulo. Este problema se denomina degeneracin de los

pesos (weight degeneracy) o empobrecimiento de las muestras (simple impoverishment).

La etapa de remuestreo reduce este problema mediante la definicin de un nuevo conjunto

de partculast

S en el que las estimaciones del estado con mayores pesos se representarn

mediante varias partculas mientras que se eliminarn las estimaciones con pesos

pequeos. El remuestreo extrae (con reemplazo) n partculas del conjunto temporalt

S .

La probabilidad de extraer cada partcula es proporcional al peso de sta. Se ha utilizado el

algoritmo de remuestreo sistemtico (systematic resampling) presentado en [21] ya que

tiene un coste computacional reducido [22].


12/24

PubInt14


11

El filtro de partculas implementado se detalla en la Tabla 3. Tiene una estructura muy

similar al algoritmo del filtro de Kalman (Tabla 2): las medidas del sistema de captura de

movimiento se utilizan en la fase de prediccin (lnea 9) y las medidas del sistema UWB se

utilizan en la fase de clculo de los pesos (lnea 12). Despus de registrar una medida del

sistema UWB, se recalcular la matriz de transformacin U GT entre ambos sistemas de

coordenadas para corregir los errores del sistema de captura de movimiento. Las

estimaciones del estado del sistema (posicin del operador humano) se calcular a partir de

la media de las partculas.

Algoritmo basado en Filtro de Partculas

01: Inicializar los parmetros del filtro: n , Q , R ,

02: Inicializar UG

T con las dos primeras medidas:1

p ,2

p {Ec. (2)-(5)}

03:for each medidat

p

04: ift

p es una medida del sistema de captura de movimiento (MoCap)

05: if 1tp es una medida del sistema UWB

06: Recalcular UG

T cont

p ,1t

x

{Ec. (2)-(5)}

07: end if

08: Transformar tp del sistema G al U {Ec. (1)}

09: ( )PrediccionParticulas ,t tS p Q= {Ec. (12)}

10: Calculart

como la media de todas las partculas ent

S

11: else ift

p es una medida del sistema UWB

12: ( )1CalculoPesosParticulas , ,t t tW S p R= {Ec. (13)}

13: ( )1RemuestreoSistematico ,t t tS S W=

14: Calculart


S

15: end if

16: end forTabla 3. Pseudocdigo del algoritmo de fusin sensorial basado en el filtro de partculas.

3.4.-Algoritmo basado en una combinacin de un filtro deKalman y un filtro de partculas

El tercer algoritmo de fusin sensorial que hemos implementado combina las dos tcnicas

bayesianas anteriores: filtro de Kalman y filtro de partculas. En la Tabla 4 se describe en

detalle el algoritmo desarrollado. El filtro de Kalman se aplica a las medidas procedentesdel sistema de captura de movimiento mientras que se aplica un filtro de partculas SIR a


13/24


12

las medidas UWB. Las estimaciones previas del estado1t

se utilizan en los pasos de

prediccin de los filtros (lneas 9, 13) para intercambiar medidas entre ellos y de esta forma

corregir sus errores. La matriz de transformacin U GT se recalcula (lnea 6) de igual modo

que en los algoritmos previos.

Algoritmo basado en Filtro de Kalman y Filtro de Partculas

01: Inicializar los parmetros de los filtros: A , H,1

P,KF

Q ,KF

R , n ,PF

Q ,PF

R ,

02: Inicializar UG

T con las dos primeras medidas:1

p ,2

p {Ec. (2)-(5)}

03:for each medidat

p

04: if tp es una medida del sistema de captura de movimiento (MoCap)

05: if 1tp es una medida del sistema UWB

06: Recalcular UG

T cont

p ,1t

x

{Ec. (2)-(5)}

07: end if

08: Transformart

p del sistema G al U {Ec. (1)}

09: [ ] ( )1 1, PrediccionKalman , , ,t t KF t t P A Q x P = {Ec. (6), (7)}

10: [ ] ( ), CorreccionKalman , , , ,t t KF t t t P H R p x P= {Ec. (8)-(10)}

11: Almacenart

x como estimacin del estado en el instante t

12: else if tp es una medida del sistema UWB

13: ( )1PrediccionParticulas ,t t PF S x Q= {Ec. (12)}

14: ( )CalculoPesosParticulas , ,t t t PF W S p R= {Ec. (13)}

15: ( )RemuestreoSistematico ,t t tS S W=

16: Calculart


S

17: end if18: end forTabla 4. Pseudocdigo del algoritmo de fusin sensorial basado en los filtros de Kalman y de partculas.

Este algoritmo mejora el rendimiento del algoritmo basado en filtro de partculas del

apartado anterior ya que reduce la frecuencia de su ejecucin. Las medidas con altas

frecuencias de muestreo del sistema de captura de movimiento se procesan con el filtro de

Kalman mientras que el filtro de partculas slo se ejecuta cuando se obtienen medidas del

sistema UWB con una menor frecuencia. Adems, las medidas del sistema de captura de

movimiento se pueden representar fcilmente mediante los modelos lineales gaussianos de

un filtro de Kalman mientras que las medidas del sistema UWB se modelan mejor con una

tcnica no-lineal no-gaussiana (como el filtro de partculas) debido a su elevada latencia.


14/24

PubInt14


13

4.-Resultados experimentalesHemos llevado a cabo un conjunto de experimentos para comparar la precisin y el

rendimiento de los tres algoritmos bayesianos presentados en los apartados anteriores. En

estos experimentos un operador humano llevaba puesto el traje de captura de movimiento

GypsyGyro y una etiqueta UWB del sistema Ubisense. Este humano realizaba dos tipos de

trayectorias: una trayectoria lineal y una trayectoria rectangular. Cada trayectoria se ha

recorrido quince veces. Las medidas de traslacin global de ambos sistemas se han

registrado en un PC controlador donde estaban instalados sus componentes software y sus

libreras. Las medidas resultantes las hemos procesado con tres funciones desarrolladas en

Matlab que implementaban los tres algoritmos de fusin. Para poder comparar el

rendimiento de los algoritmos, los hemos ejecutado todos en el mismo ordenador con las

siguientes caractersticas: Intel Pentium 4 2.8GHz, 760MB RAM y sistema operativo

Microsoft Windows XP. Para los algoritmos basados en el filtro de partculas, utilizaremos

un conjunto de 100 partculas.

En la Figura 1 y la Figura 2, representamos las medidas originales del sistema UWB y el

sistema de captura de movimiento en el mismo sistema de coordenadas. Estas grficas

representan las ventajas y los inconvenientes de ambos sistemas de localizacin. Por una

parte, el sistema de captura de movimiento obtiene medidas con una baja latencia que

permiten definir de manera continua el movimiento del humano mientras que el sistema

UWB tiene una frecuencia de muestreo muy inferior. Por otra parte, las medidas del

sistema de captura de movimiento acumulan error (drift) a lo largo del tiempo mientras que

las medidas UWB mantienen aproximadamente un error constante.


15/24


14

0 0.5 1 1.5 2 2.5 3 3.5 42.8

2.85

2.9

2.95

3

3.05

3.1

3.15

3.2

X (m)

Y

(m)

MoCap Data

UWB Data

Predefined Path

Figura 1: Medidas de posicin originales procedentes del sistema de captura de movimiento y del sistema

UWB en una trayectoria lineal.

0 0.5 1 1.5 2 2.5 3 3.5 4 4.51.4

1.6

1.8

2

2.2

2.4

2.6

2.8

3

3.2

3.4

X (m)

Y

(m)

MoCap DataUWBDataPredefined Path

Figura 2: Medidas de posicin originales procedentes del sistema de captura de movimiento y del sistema

UWB en una trayectoria rectangular.

La Tabla 5, la Tabla 6 y la Tabla 7 enumeran las estadsticas del error (error medio y

desviacin estndar) y el tiempo de ejecucin de los tres algoritmos de fusin en las quince

repeticiones del experimento de la trayectoria lineal. Los valores de error del filtro de

Kalman y de la combinacin Kalman-partculas son prcticamente idnticos y ligeramente

inferiores a los errores del filtro de partculas. Sin embargo, estas diferencias de precisin

son mnimas, tal como se puede observar en la Figura 3, la Figura 4 y la Figura 5, en las

que se representan las trayectorias devueltas por los tres algoritmos de fusin en unos de

los experimentos. Por otra parte, el filtro de Kalman es el algoritmo con menor coste

computacional y el filtro de partculas es el algoritmo con mayor coste.


16/24

PubInt14


15

Exp. Tiempo (s) Error Medio (m) Desv. Est. Error (m)

1 0.0781 0.035 0.0309

2 0.0625 0.0350 0.0309

3 0.0625 0.0392 0.0296

4 0.0625 0.0437 0.0268

5 0.0781 0.0431 0.0286

6 0.0938 0.0408 0.0279

7 0.0781 0.0382 0.0360

8 0.0781 0.0347 0.0290

9 0.0469 0.0375 0.0300

10 0.0938 0.0331 0.0279

11 0.0938 0.0374 0.0258

12 0.0938 0.0401 0.0272

13 0.0781 0.0474 0.0349

14 0.0781 0.0319 0.0227

15 0.0781 0.0316 0.0195

Media 0.0771 0.0379 0.0285

Tabla 5: Estadsticas del error del filtro de Kalman en una trayectoria lineal.


1 1.3594 0.0441 0.0310

2 1.3125 0.0403 0.0317

3 1.2500 0.0358 0.0259

4 1.0313 0.0387 0.0239

5 1.5938 0.0352 0.0239

6 1.9375 0.0335 0.0238

7 1.3906 0.0403 0.0282

8 1.1719 0.0378 0.0304

9 0.7031 0.0433 0.0334

10 1.7969 0.0313 0.0199

11 2.3750 0.0463 0.0344


17/24


16

12 2.1406 0.065 0.0331

13 1.2188 0.0938 0.0419

14 1.6563 0.0350 0.0291

15 1.9063 0.0360 0.0244

Media 1.5229 0.0438 0.0290

Tabla 6: Estadsticas del error del filtro de partculas en una trayectoria lineal.


1 1.0313 0.0269 0.0255

2 1.0156 0.0312 0.0282

3 0.8125 0.0406 0.0351

4 0.6250 0.0460 0.0291

5 1.2500 0.0396 0.0293

6 1.5313 0.0327 0.0248

7 1.0313 0.0430 0.0414

8 0.8281 0.0327 0.0295

9 0.4844 0.0434 0.0330

10 1.3281 0.0316 0.0267

11 1.8281 0.0366 0.0254

12 1.7500 0.0381 0.0242

13 0.7500 0.0525 0.0367

14 1.3438 0.0313 0.0221

15 1.4375 0.0326 0.0209

Media 1.1365 0.0373 0.0288

Tabla 7: Estadsticas del error del algoritmo Kalman-partculas en una trayectoria lineal.


18/24

PubInt14


17

0 0.5 1 1.5 2 2.5 3 3.52.9

2.95

3

3.05

X (m)

Y(m)

Filtered MoCap Data

Filtered UWB Data

Pre-defined Path

Figura 3: Medidas devueltas por el filtro de Kalman en una trayectoria lineal.

0 0.5 1 1.5 2 2.5 3 3.52.85

2.9

2.95

3

X (m)

Y

(m)

Filtered MoCap DataFiltered UWB Data

Predefined Path

Figura 4: Medidas devueltas por el filtro de partculas en una trayectoria lineal.

0 0.5 1 1.5 2 2.5 3 3.5 42.92

2.94

2.96

2.98

3

3.02

3.04

3.06

X (m)

Y

(m)

Filtered MoCap Data

Filtered UWB Data

Predefined Data

Figura 5: Medidas devueltas por la combinacin Kalman-partculas en una trayectoria lineal.


19/24


18

La Tabla 8, la Tabla 9 y la Tabla 10 enumeran las estadsticas de los experimentos de la

trayectoria rectangular. En esta trayectoria ms compleja se detectan diferencias ms

importantes en la precisin de las medidas devueltas por los tres algoritmos. El algoritmo

Kalman-partculas obtiene las medidas con menor error mientras que las medidas

obtenidas por el filtro de partculas tienen un error tres veces mayor. Las medidas del filtro

de Kalman tienen valores de error intermedios entre los dos algoritmos anteriores. La

muestra como las posiciones estimadas por el algoritmo Kalman-partculas son ms

cercanas a la trayectoria terica que las devueltas por los otros dos algoritmos (vase

Figura 6, Figura 7 y Figura 8). En lo que respecta al coste computacional, el filtro de

Kalman vuelve a ser la mejor solucin mientras que la combinacin Kalman-partculas

tiene un coste intermedio entre el filtro de Kalman y el filtro de partculas.


1 0.7813 0.0894 0.0763

2 0.5156 0.1029 0.0665

3 0.3750 0.0999 0.0730

4 0.2969 0.1481 0.1034

5 0.3281 0.1417 0.0649

6 0.5625 0.1396 0.1089

7 0.7188 0.0705 0.0607

8 0.6563 0.0819 0.0559

9 0.5313 0.0876 0.0694

10 0.6875 0.1217 0.0940

11 0.5781 0.1111 0.0993

12 0.4844 0.0895 0.0611

13 0.7500 0.0721 0.0527

14 0.6094 0.1257 0.0993

15 0.5469 0.0977 0.0613

Media 0.5615 0.1053 0.0765

Tabla 8: Estadsticas del error del filtro de Kalman en una trayectoria rectangular.


20/24

PubInt14


19


1 8.3125 0.1449 0.0983

2 7.9063 0.1332 0.0880

3 4.5313 0.1733 0.1111

4 5.1875 0.2063 0.1196

5 5.4375 0.1791 0.1165

6 6.2656 0.2049 0.1314

7 6.9063 0.1305 0.0903

8 7.0313 0.1459 0.0931

9 6.0469 0.1483 0.0983

10 6.8594 0.3704 0.2151

11 4.9844 0.1662 0.1403

12 4.6094 0.1682 0.1275

13 5.3438 0.185 0.1354

14 5.4063 0.1973 0.1712

15 4.3750 0.2052 0.1734

Media 5.9469 0.1839 0.1273

Tabla 9: Estadsticas del error del filtro de partculas en una trayectoria rectangular.


1 6.1250 0.0555 0.0517

2 5.9688 0.0413 0.0323

3 3.2656 0.0405 0.0315

4 3.8750 0.0735 0.0750

5 4.0156 0.0446 0.0324

6 4.6094 0.0770 0.0787

7 5.0938 0.0600 0.0466

8 5.3594 0.0594 0.0638

9 4.6250 0.0524 0.0444

10 5.1563 0.0908 0.1239


21/24


20

11 3.4063 0.0792 0.0606

12 3.1875 0.0755 0.0576

13 3.8125 0.0550 0.0394

14 3.8906 0.0704 0.0488

15 2.9844 0.0635 0.0439

Media 4.3583 0.0626 0.0554

Tabla 10: Estadsticas del error del algoritmo Kalman-partculas en una trayectoria rectangular.

0.5 1 1.5 2 2.5 3 3.51.6

1.8

2

2.2

2.4

2.6

2.8

3

3.2

3.4

3.6

X (m)

Y

(m)

Figura 6: Medidas devueltas por el filtro de Kalman en una trayectoria rectangular.

0.5 1 1.5 2 2.5 3 3.51.6

1.8

2

2.2

2.4

2.6

2.8

3

3.2

3.4

3.6

X (m)

Y

(m)

Figura 7: Medidas devueltas por el filtro de partculas en una trayectoria rectangular.


22/24

PubInt14


21

0.5 1 1.5 2 2.5 3 3.51.4

1.6

1.8

2

2.2

2.4

2.6

2.8

3

3.2

3.4

X (m)

Y

(m)

Figura 8: Medidas devueltas por la combinacin Kalman-partculas en una trayectoria rectangular.

5.-ConclusionesEn este informe hemos descrito en detalle los tres algoritmos basados en filtros bayesianos

que hemos implementado para combinar las medidas de posicin obtenidas por los dos

sistemas de localizacin de los que disponemos: sistema de captura de movimiento

GypsyGyro y sistema de localizacin Ubisense.

Hemos desarrollado un conjunto de experimentos para comparar los algoritmos

desarrollados. En concreto, el operador humano que llevaba el traje de captura de

movimiento y la tarjeta UWB tena que seguir una trayectoria lineal y una trayectoria

rectangular.

Teniendo en cuenta los resultados obtenidos en estos experimentos, podemos concluir que,

en trminos generales, el algoritmo de combinacin Kalman-partculas es el ms idneo

debido a su mayor precisin. Sin embargo, el filtro de Kalman ser ms recomendable en

los sistemas en tiempo real ya que tiene un coste computacional considerablemente inferior

y ofrece una precisin muy similar. Por lo tanto, para nuestra aplicacin de fusin sensorial

ser ms conveniente utilizar el filtro de Kalman ya que preferimos que el sistema nos

d una respuesta en tiempo real aunque perdamos un poco de precisin.


23/24


22

6.-Referencias1. Candelas, F.A. and J.A. Corrales, Giroscopios en el Sistema GypsyGyro-18

(Publicacin Interna n 4). 2007.

2. Candelas, F.A. and J.A. Corrales, Ajuste de los Parmetros del Sistema Ubisense

(Publicacin Interna n 5). 2007.

3. Candelas, F.A. and J.A. Corrales, Fusin GypsyGyro-Ubisense (Publicacin

Interna n 8). 2007.

4. Fox, V., et al., Bayesian filtering for location estimation. IEEE Pervasive

Computing, 2003. 2(3): p. 24-33.

5. Foxlin, E. Inertial Head-Tracker Sensor Fusion by a Complementary Separate-

Bias Kalman Filter. in IEEE Virtual Reality Annual International Symposium.

1996. Santa Clara, California: IEEE Computer Society.

6. Foxlin, E., Pedestrian Tracking with Shoe-Mounted Inertial Sensors. IEEEComputer Graphics and Applications, 2005. 25(6): p. 38-46.

7. Roetenberg, D., P.J. Slycke, and P.H. Veltink, Ambulatory Position and

Orientation Tracking Fusing Magnetic and Inertial Sensing. IEEE Transactions on

Biomedical Engineering, 2007. 54(5): p. 883-890.

8. Caron, F., et al., GPS/IMU Data Fusion Using Multisensor Kalman Filtering:

Introduction of Contextual Aspects. Information Fusion, 2006. 7(2): p. 221-230.

9. You, S. and U. Neumann. Fusion of Vision and Gyro Tracking for Robust

Augmented Reality Registration. in IEEE Virtual Reality 2001 Conference. 2001.

Yokohama, Japan.

10. Ribo, M., et al., Hybrid Tracking for Outdoor Augmented Reality Applications.

IEEE Computer Graphics and Applications, 2002. 22(6): p. 54-63.11. Ribo, M., M. Brandner, and A. Pinz, A Flexible Software Architecture for Hybrid

Tracking. Journal of Robotic Systems, 2004. 21(2): p. 53-62.

12. Ababsa, F.E. and M. Mallem,Hybrid Three-Dimensional Camera Pose Estimation

Using Particle Filter Sensor Fusion. Advanced Robotics, 2007. 21(1): p. 165-181.

13. Jourdan, D.B., et al. Monte Carlo localization in dense multipath environments

using UWB ranging. in IEEE International Conference on Ultra-Wideband (ICU

2005). 2005. Zurich, Switzerland.

14. Hightower, J. and G. Borriello. Particle Filters for Location Estimation in

Ubiquitous Computing: A Case Study. in Sixth International Conference on

Ubiquitous Computing (Ubicomp 2004). 2004. Nottingham, England.

15. Evennou, F. and F. Marx, Advanced Integration of WIFI and Inertial NavigationSystems for Indoor Mobile Positioning. EURASIP Journal on Applied Signal

Processing, 2006. 2006(1): p. 164-164.

16. Schon, T.B., D. Tornqvist, and F. Gustafsson.Fast Particle Filters for Multi-Rate

Sensors. in European Signal Processing Conference (EUSIPCO 2007). 2007.

Poznan, Poland.

17. Giremus, A., et al. A Rao-Blackwellized particle filter for INS/GPS integration. in

IEEE International Conference on Acoustics, Speech, and Signal Processing

(ICASSP '04). 2004. Montreal, Quebec, Canada.

18. Simon, D., Kalman Filtering. Embedded Systems Programming, 2001. 14(6): p.

72-79.

19. Corrales, J.A., F.A. Candelas, and F. Torres, Hybrid tracking of human operators

using IMU/UWB data fusion by a Kalman filter, in Proceedings of the 3rd


24/24

PubInt14


ACM/IEEE international conference on Human robot interaction. 2008, ACM:

Amsterdam, The Netherlands. p. 193-200.

20. Arulampalam, M.S., et al., A tutorial on particle filters for online nonlinear/non-

Gaussian Bayesian tracking. IEEE Transactions on Signal Processing, 2002. 50(2):p. 174-188.

21. Carpenter, J., P. Clifford, and P. Fearnhead, Improved particle filter for nonlinear

problems. IEE Proceedings Radar, Sonar and Navigation, 1999. 146(1): p. 2-7.

22. Hol, J.D., T.B. Schon, and F. Gustafsson. On Resampling Algorithms for Particle

Filters. in Nonlinear Statistical Signal Processing Workshop. 2006. Cambridge,

United Kingdom.

comparativa algoritmos fusion

Documents