Introducción a la Percepción Cooperativajdedios/asignaturas/Clase5.pdf · Filtros iterativos de...

1/50

PERCEPCIÓN AVANZADA

Introducción a la Percepción Cooperativa

MASTER EN Automática, Robótica y Telemática

Dr. J. RAMIRO MARTINEZ DE DIOS

2/50

• Introducción

• Filtros Bayesianos

• Filtro de Kalman

• Filtro de Información

• Filtro de partículas


3/50

• Objetivos:

– Explotar las sinergias entre entidades que cooperan con un objetivo común

– Se sirve de métodos de fusión sensorial (lección anterior)

• Consideraciones:

– Asociación: determinar si datos de dos o más sensores provienen del mismo fenómeno

– Filtrado de datos: evitar datos contradictorios

– Carga computacional: velocidad de repuesta necesaria

Percepción Cooperativa

4/50

Detección y localización cooperativa Monitorización cooperativa

Robots heterogéneos– Plataformas: varios UAVs o robots

terrestres– Sensores

Percepción Cooperativa en Entornos Multirrobot

5/50

Misión general:1) Vigilancia y detección 2) Confirmación 3) Monitorización


Vigilancia y detección– Área se divide dependiendo de

características de UAVs y sus sensores– Cada UAV recorre su área buscando

aplicando técnicas de detección

6/50

Demostración del sistema para la detección de fuegos– Sensores: cámara IR, cámara visual, sensor de fuego

Vigilancia y detección


560 570 580 590 600 610 620 630 640 650 6600

20

40

60

80

100

120

140

7/50

Confirmación


460 480 500 520 540 560 580 600 6200

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Pro

babi

lity

0.2821.5Desviación estándar estimada

200.044443961.4564628.9Posición estimada (fusión)

2004443961564627Posición real del fuego

AlturaLatitudLongitud

8/50

• Introducción






9/50

Fusión sensorial para estimación

• Determinar un valor desconocido a partir de observaciones

– Cantidad desconocida: estado de un sistema dinámico: xk

– Observaciones o medidas

Incertidumbre

modelos probabilísticos

Filtros Bayesianos

10/50

• Modelo de un sistema (o proceso) dinámico

• Caso lineal:

• xk: vector de estados del sistema en el instante k

– Estado es completo: contiene toda la información sobre el pasado del sistema que

es útil para predecir su fututo (Hipótesis de Markov)

• wk: incertidumbre del modelo (desconocido modelo probabilista)

– Perturbaciones sobre el sistema

– Imperfecciones en el modelo

kkkkkkk wGuBxAx ++=+1

),,(1 kkkkk vuxfx =+

Filtros Bayesianos

11/50

• Modelo de medida

• Caso lineal:

• zk: vector de medidas en el instante k

• rk: ruido de medida (desconocido modelo probabilista)

– Inexactitud de la observación

– Imperfecciones del modelo de medida

kkkk rxhz += )(

kkkk rxCz +=

Filtros Bayesianos

12/50

Filtros iterativos de Bayes

La estimación del estado en el instante k, se obtiene a partir de la

estimación en el instante k-1 y la nueva observación:

Se basa en aplicar el Teorema de Bayes:

))|(),|(()|( 1 xzpZxpfZxp kkk −=

)|(

)|()|()|(

1

1

−

−=kk

kkk

Zzp

ZxpxzpZxp

)|()|()|( 1−∝ kkk ZxpxzpZxp

Estimación para k

función de verosimilitudrelaciona la observación actual con el estado

Estimación para k-1

Filtros Bayesianos

13/50

• Introducción






14/50

Filtros iterativos de Bayes. Filtro de Kalman

Sistema dinámico lineal pero no necesariamente TI:

Ruidos blancos e independientes:

Ruidos Gaussianos

kkkkkkk wGuBxAx ++=+1

kkkk rxCz +=

[ ] QwwE iT

i = [ ] RrrE iT

i = [ ] 0=iT

iwrE[ ] [ ] 0== ii rEwE

( )QNwi ,0 ⋯ ( )RNri ,0 ⋯

Teorema central del límite

Filtro de Kalman

15/50

Fases:

– Predicción: predice estado del sistema• Modelo de predicción

– Actualización: compara predicción con medidas y corrige• Modelo de observación

En cada paso estima:-

-

kkx /

kkP /

medidasyk

Filtro

Predicción

Actualización

Filtro de Kalman

16/50

• Fase de predicción:

• Fase de actualización

Predicción del estado

Matriz de covarianza de la estimación del estado

kkkkk uBxAx +=+ //1

QAPAP T

kkkk +=+ //1

Corrección de la estimación del estado futuro

Ganancia del filtro de Kalman

[ ]kkkkkkkk xCyLxx /111/11/1 ++++++ −+=

kkkkkkk PCLPP /11/11/1 +++++ −=

[ ] 1

/1/11

−+++ += RCPCCPL T

kk

T

kkk

Filtro de Kalman

17/50

Modelos no lineales: Filtro de Kalman extendido (EKF)

• Modelos no lineales: linealizar

• Resto queda igual

( )( )

( )( )

( )( )

,

,

k k k

k

k k k

k

k k

k

A F x ux

B F x uu

C G xx

∂=∂∂=

∂∂=

∂

kkkk wBuAxx ++=+1

kkk rCxy +=

( )1 ,k k k kx F x u w+ = +

( )k k ky G x r= +

F o G son no lineales

Filtro de Kalman Extendido

18/50

Vista frontalVista aérea

Vista lateral B

Vista lateral A

Fusión sensorial mediante Filtro de Kalman con diagnosis de sensores

19/50


Enfoque:

– Fusionar las medidas para mejorar estimación

– Método: Filtro de Kalman

• Modelo lineal de propagación de incendio

• Ruidos Gaussianos

• Ventajas:

– Implantación sencilla

– Carga computacional asequible tiempo real

• Desventajas:

– Modelo lineal

20/50


Esquema:

• Funciones del filtro de Kalman:

– Diagnosis de sensores

– Fusión sensorial

medidask

Diagnósticoj (FK)

FK

Estimación

estadoPredicción

AsociaciónActualización

Se integran las medidas cuyo sensor

ha sido diagnosticado como correcto

21/50

Vector de estados


Sentido de avance

(xj, yj, hj, anchoj)

[ ]kNkNkNkNkkkkk anchohyvelanchohyx ,,,,,1,1,1,1 vel⋯=

Posición del frente en el instanteAltura de llamas en el corteAncho del frente en el corteVelocidad local del frente

Un único vector de estados para representar todo el frente en un instante

22/50

Modelo del proceso

- Modelo lineal en cada punto- Posición evoluciona con velocidad constante

- Altura de llamas y ancho constante

- Matricialmente


kkk wAxx +=+1

=

10000000

01000000

00100000

10010000

00001000

00000100

00000010

00001001

⋯

⋯

⋯

⋯

⋮⋮⋮⋮⋮⋮⋮⋮

⋯

⋯

⋯

⋯

A

kzkz xCy ,, =

=

01000000

00100000

00010000

00000100

00000010

00000001

⋯

⋯

⋯

⋮⋮⋮⋮⋮⋮⋮⋮

⋯

⋯

⋯

C

+=

+=

+=

+=

+

+

+

+

3,1,

2,1,

1,1,

,,1,

kkzkz

kkzkz

kkzkz

kzkzkz

wvelvel

wanchoancho

whh

velyy

Otros efectos, topografía, vegetación

23/50

- N sensores que observan el frente- Modelo de observación:

- Fase de actualización


ikk

iik rxCy += iTi

kik RrrE =

∑∈

++++++

−+=

kSi

kkii

kikkkkk xCyLxx /111/11/1

Solo se consideran sensores válidos

Diagnosis de sensores

24/50


- Errores del sensor: e.g. humo oculta imágenes visuales

- Ruptura súbita de la tendencia: en un solo intervalo de tiempo

- Este tipo de rupturas no puede estar originado por fenómeno físico- El fuego no se apaga con tanta rapidez

- No se trata de ruido Gaussiano- Incumple hipótesis del FK- Será muy fácil de detectar con FK

- Objetivo: identificar rupturas súbitasde tendencias mediante FK


0 50 100 150 200 250 300820

830

840

850

860

870

880

890

900

Experimento de campo

25/50


Considera los cortes con cada recta z de la malla observado por el sensor i

El vector de estados es:

Supongamos proceso con modelo lineal:


[ ]Tikz

ikz

ikz

ikz anchohyy ,,,, =

[ ]Tkzkzkzkzkz velanchohyx ,,,,, =

kkk wAxx +=+1

=

1000

0100

0010

1001

Akz

ikz xCm ,, =

=0100

0010

0001

C

A efectos de diagnosis cada punto es independiente

+=

+=

+=

+=

+

+

+

+

3,1,

2,1,

1,1,

,,1,

kkzkz

kkzkz

kkzkz

kzkzkz

wvelvel

wanchoancho

whh

velyy

(xj, yj, hj, anchoj)

26/50

Errores en el sensor originan errores de predicción altos:


kkk xCye /11 ++ −=

0 50 100 150 200 250 300820

830

840

850

860

870

880

890

900

0 50 100 150 200 250 300−1000

−500

0

500

1000

1500

Experimento de campo e

)max(RkT =Umbral depende del ruido del sensor:

k=1.96 Se toman 95% de puntos

27/50


28/50

360 380 400 420 440 460 480 500

480

500

520

540

560

580

X

Y

Fire front shape G520 Aerial

Vista IR

Se pierden datos

360 380 400 420 440 460 480 500

480

500

520

540

560

580

X

Y

Fire front shape G520 AerialVista aérea

360 380 400 420 440 460 480 500

480

500

520

540

560

580

X

Y

Fire front shape G520 AerialVista frontal


Frentes interpolados

Fusión con KF con diagnosis

29/50

• Introducción






30/50

Filtro de Información

• Versión dual del Filtro de Kalman

• Supone incertidumbre Gaussina

• La principal diferencia es la parametrización:

– FK: representación de probabilidad

– IF: representación canónica

• Parametrización canónica (IF):

– Ω = Σ-1 (matriz de información)

– ξ = Σ-1 * µ (vector de información)

• Representación de probabilidad (KF):

– p(x) = η * exp -1/2*xT*Ω*x – xT*ξ

31/50


• Filtro de Información( ξ(t-1), Ω(t-1), u(t),z(t) )

– Ω(t)' = (A(t)*Ω(t-1)-1*A(t) + R(t))-1

– ξ(t)' = Ω(t)'*(A(t)*Ω(t-1)-1*ξ(t-1) + B(t)*u(t))

– Ω(t) = C(t)'*Q(t)-1*C(t) + Ω(t)'

– ξ(t) = C(t)'*Q(t)-1*z(t)+ ξ(t)'

– Return ξ(t), & Ω(t);

• Complejidad computacional:

– Predicción: KF es poco costoso, IF es costoso

– Actualización: KF es costoso, IF es poco costoso

actualización

predicción

32/50


• Ventajas del IF frente al FK:

– Permite modelar la incertidumbre total: Ω = 0

– IF es numéricamente más estable

– IF permite integrar información de forma directa sin necesidad de

convertirla a probabilidades

– Menos carga computacional que el FK al aumentar el número de medidas

• Desventajas:

– Más carga computacional en la actualización

– Kalman filter es más conocido

EIF: Filtro de Información Extendido: modelo no lin eal

KF e IF requieren modelos de ruido Gaussiano

33/50

• Introducción






34/50

• Limitaciones del FK y IF:

– Emplean modelos paramétricos lineales o no lineales

– Ruido Gaussiano

– Es un sistema mono-hipótesis: 1 sola gaussiana

• Filtro de partículas: aproxima FDP por sus partículas

– Es no paramétrico:

• Admite cualquier modelo y cualquier tipo de ruido

• Permite modelar multi-hipótesis

Filtro de partículas

35/50

• Representa la Función de Densidad de Probabilidad por un

conjunto suficientemente grade de partículas con pesos

Particle Filter

Each with weight 1/N,

N = Nr. Of particles


36/50

• En cada iteración aplica el filtro Bayesiano sobre cada partícula

aumentando el peso de las partículas más probables


37/50

• Se permite la creación y eliminación de partículas

• Paso “Importance sampling”:

– Se escoge un nuevo conjunto de partículas a partir del conjunto

existente

– Se utiliza el peso para dicha elección. Partículas con peso mayor

tienen más probabilidad de ser escogidas

– Las partículas con peso menor tienden a desaparecer

– El peso de las partículas del nuevo conjunto se ponen todos iguales.

– Es computacionalmente pesado. Se hace con cierta frecuencia, no en

cada paso

• Cuando el conjunto contiene pocas partículas efectivas


38/50

• Obtención de nuevas partículas

• Actualización de pesos

• Resampling


39/50

One Cycle…of the basic algorithm …


40/50


One Cycle…of the basic algorithm …

41/50

No requiere hipótesis de sistema lineal ó Gaussiano

Conduce a una proximación de la FDP completa

No obtiene estado sino la FDP completa. Es necesario obtenerestado de la FDP

La versión básica es muy sencilla de implantar: modularidad


42/50

• Vuelo de UAV afectado por perturbaciones

• Objetivo: – generar maypoints intermedios para asegurar:

• Probabilidad de pasar por zonas de paso >PwzMIN

• Probabilidad de pasar por zonas prohibidas<PfzMAX

• Entradas: – conjunto de zonas de paso y zonas prohibidas

• Salida: – conjunto de waypoints de la trayectoria

Planificación de trayectorias de UAVs

43/50

Filtros de partículas para planificación de trayectorias de UAVs

44/50

Simulación de trayectorias de UAV mediante Filtros de partículas

Simulación de trayectorias de UAV

- Modelo de un UAV en B.C.- Modelo de viento

Particle Filters- Modelos de complejidad arbitraria

- ruido no-Gaussian

- Esquemas multihipótesis

45/50

Trajectory simulation

10099,110099,510096,299,410098,910098,3Pwz,i(%)

WZ11

WZ10

WZ9

WZ8

WZ7

WZ6

WZ5

WZ4

WZ3

WZ2

WZ1

Way-zone

Experimentos

46/50

Real experiments- Bollullos (Sevilla, Spain) 2008 and 2009

- 28 XBow Mica2 with MTS400 sensorboards

Experimentos reales

47/50

Experiment 1: Waypoints are cluster centroids (no intermediate waypoints)

Error=28 m

Error=25 m

Intermediate waypoints are required

Experimentos reales

48/50

Experiment 2: Proposed method

UAV received 91,8% of the messages

UAV passed by wayzones with Prob.>95%

96,997,199,298,5Pwz,i(%)

WZ4

WZ3

WZ2

WZ1

Way-zone

Receivedmessages

Experimentos reales

49/50

PERCEPCIÓN AVANZADA


MASTER EN Automática, Robótica y Telemática

Dr. J. RAMIRO MARTINEZ DE DIOS

Introducción a la Percepción Cooperativajdedios/asignaturas/Clase5.pdf · Filtros iterativos de...

Documents

Transcript of Introducción a la Percepción Cooperativajdedios/asignaturas/Clase5.pdf · Filtros iterativos de...