Filtrage de Kalman

Application au suivi des paramètres de problèmes de

vision

1/90

Cadre de l’étude

• Un système physique.

• Distinction entre paramètres:– qui modélisent le système– que l’on peut mesurer.

• Estimer les premiers à partir des seconds.

Cadre de l’étude

Les paramètres mesurables observation xi

Les paramètres internes (décrivant le modèle) vecteur d’état si

Prise en compte des incertitudes

• Les mesures sont imprécises/incertaines

• Le modèle est simpliste

• On veut savoir la fiabilité d’une estimation si de l’état courant si.

ce que le filtre de Kalman permet.

Formalisation: mesure et vecteur d’état

• On a accès a: • bruit blanc additif et:

• Evolution du modèle:

• Modèle « lâche »:

iiii xx ˆ

].[ tiii ER

].[ tiii nnEQ

iiii nshs )(1

• Filtrage de Kalman: processus itératif d’estimation du vecteur

• Estimation à l’instant i:

• traduit la confiance que l’on a en l’estimation• Si on connaît , la meilleure estimation possible de

est:

Formalisation: mesure et vecteur d’état

is

])ˆ)(ˆ[(ˆ tiiiiii ssssEPs

isiP

1ˆ is is

)ˆ(ˆ 11/ iiii shs

Formalisation: les équations de mesures

• Liens entre les observations et le vecteur d’état:

• Cas linéaire:

ii pnmi

iii

f

sxf

:

0),(

iiii sFx .ˆ

Formalisation: les équations de mesures

• Dans le cas non-linéaire, on s’y ramène: développement de Taylor à l’ordre 1 de

• on a bien:

if

...)ˆ.()ˆ.()ˆ,(),ˆ( 1/1/

iiii

iii

i

iiiiiiii ss

s

fxx

x

fsxfsxf

iiiii

iiiiii

i

ii

iiii

ii

iiiiii

iiiiii

iiii

sxx

fxxsx

x

fw

sxs

fM

ssxs

fsxfy

wsMy

).ˆ,()ˆ).(ˆ,(

)ˆ,(

ˆ).ˆ,()ˆ,(

avec

.

1/1/

1/

1/1/1/

1/

1/1/

)..()ˆ..(ˆˆ

iiiii

iiiiiiii

PMKIPsMyKss

• 3 étapes, propagation des incertitudes.

• Initialisation:• Prédiction:

• Calcul du gain de Kalman:

• Mise à jour:

• Un pas du filtre.

L’algorithme

),ˆ( 00 Ps

11111/

11/

)ˆ(.).ˆ(

)ˆ(ˆ

it

ii

iii

i

iii

iiii

Qss

hPs

s

hP

shs

11/1/ )...(.

ti

tiiii

tiiii WMPMMPK

Algorithme: interprétation

• Compromis entre la contribution de la prédiction et de la mesure:

• On peut réécrire:

• Grande incertitude sur le vecteur d’état « grande » « grand » innovation favorisée.

• Grande incertitude sur la mesure « grande » « petit » prédiction privilégiée.

1.. itiii WMPK

iPiK

1iW

iK

innovation

)ˆ.(.ˆˆ 1/1/ iiiiiiii sMyKss

Algorithme: améliorations possibles

• Filtres de Kalman itérés– Globalement– Localement

• Objectif: limiter l’influence des linéarisations

Utilisation pour l’estimation robuste

• Réjection des mesures aberrantes:

v.a. gaussienne de moyenne nul, de covariance

• Distance de Mahalanobis associée:

• Seuil de réjection tel que:

)ˆ,ˆ( 1/ iiii sxf

i

21/

11/1/ ~)ˆ,ˆ(..)ˆ,ˆ()ˆ,ˆ(

iiiii

tiiiiiii sxfsxfsx

95.0])ˆ,ˆ([ 1/ iii sxP

Applications

• Suivi de la trajectoire d’une particule dans le plan

• Estimation des paramètres d’une ellipse à partir d’un nuage de points

• Suivi des paramètres externes d’une caméra en trou d’épingle.

Suivi de la trajectoire d’une particule dans le plan

• Le système: mouvement à accélération constante

• Equation de mesures:

• Incertitude: Meyer a montré que

iit

iiiiiii X

t

tt

t

tt

XyyyxxxX .

110

21

101

21

][ 1

iit

iii XYyxY .001000000001 ][

ttt

ttt

ttt

Qi

26

236

6820

23

233

345

Suivi de la trajectoire d’une particule dans le plan

• Résultats:

• influence des incertitudes

• influence des incertitudes

• Réjection des valeurs aberrantes

Estimation des paramètres d’une ellipse à partir d’un nuage de points

• Vecteur d’état:• Mesures:

(points du nuage)

• Equations de mesures:

tiii

tiiiii

t

yxYbayxsbayxs

][],,0,0[ˆ],,0,0[

1)0()0(

),( 2

2

2

2

i

ii

i

iiii

b

yy

a

xxYsf

Estimation des paramètres d’une ellipse à partir d’un nuage de points• résultats• comparaison filtre cla

ssique / itéré localement 1

• comparaison filtre classique / itéré localement 2

• Mais des fois…

Suivi des paramètres externes d’une caméra en trou d’épingle.

• Rappel - modèle de caméra:

• Calibrage…

1

.~1 z

yx

Pvu

esextrinsèqu paramètres

esintrinsèqu paramètres

0

0

..100

0 tRRvfuf

P v

u

Suivi des paramètres externes d’une caméra en trou d’épingle.

• Une caméra en mouvement autour d’un cube.exemple

• 2 possibilités:– suivi sur les points 2D, puis estimation de P (svd)

grâce aux 8 correspondances 2D 3D» Résultats 1 | 2

– Suivi sur les paramètres (R,t):• rotation 3 angles • translation 3 composantes

» Résultats 1 | 2

Suivi des paramètres externes d’une caméra en trou d’épingle.

• La suite …