Vision Par Ordinateur James L. Crowley DEA IVR Premier Bimestre 2000/01 Séance 8: 7 décembre 2000 Plan de la Séance: Le Filtre de Kalman Le Filtre de Kalman...2 Prédiction de l'évolution temporelle de l'état...4 Modèle du capteur...6 Aproximations des modèle non-linéaires...6 Validation...7 Mise à Jour...8 L'Innovation...9
Le Filtre de Kalman Le filtre de Kalman est un estimateur récursif optimal. Son origine se trouve dans le problème de l'estimation de la trajectoire des satellites des aéronefs. Dans un tel problème, on cherche à estimer la position (x, y, z) la vitesse (v x, v y, v z ) à partir de l'observation de deux angles : Site azimut. (θ, ϕ) Dans la terminologie de Kalman : la position vitesse est un vecteur d'état : X l'observation est Y x y z X = v x v y v z Y = θ ϕ Pour chaque instant, t, le filtre donne une estimation X^ t ainsi que sa précision P t, à partir de l'observation l'estimation précédente, X^ t-1, P t-1, l'observation Y, sa précision, P y. Les précisions sont des covariances. X^ t, P t := F { X^ t-1, P t-1, Y, P y } Ce perm d'estimer un vecteur de N variable aléatoire à partir de M< N observations. Les précisions sont des 2ieme moments de l'erreur. Le filtre récursif est un processus cyclique avec 5 phases : 1) Prédiction de l'évolution temporelle de l'état. 2) Observation 3) Prédiction de l'observation 4) Validation de l'observation. 5) Mise à jour de l'estimation. 8-2
Observations Y, P Y Y, P Y Y, P Y Validation X, P x Mise à Jour Prediction Estimation ^ X, P ^ x Techniques : Prédictions: validation : Mise à Jour Modèle Linéaire Distance de Mahalanobis (Distance normalisée par Covariance) Estimation linéaire Notation: Vecteur d'état a temp t : Estimation du vecteur d'état : X t X^ t, P^ t Observation : Y τ, P y Prédiction : X t, P t Modèle du capteur : H Y X avec précision P y Modèle du processus : Affectation : := Définition : ϕ( t) avec précision Q t Le modèle de Kalman suppose que les estimations sont corrompues par un bruit B. X^ = X + B Le bruit est une variable aléatoire de moyenne nul. La précision est le deuxième moment de ce bruit. P E{ B B T} = E{(X X^ ^ )(X X^ ^ ) T } On ne peut connaître ni X ni B. Kalman nous fourni les estimations P^ t 8-3
Prédiction de l'évolution temporelle de l'état La phase de prédiction projs X^ t-1 P^ t-1 à temps t afin de produire X t P t On utilise les dérivées temporelles de X. Ces dérivées peuvent êtres inclus dans le vecteur X^ t-1 Soit une propriété, x^, du vecteur X^, avec variance σ^ x 2. La prédiction de premier ordre x(t) exige la dérivée temporel x^ '(t t). x^ ' t x^ t t Par série de Taylor, on peut écrire : x t := x^t t + x^ ' t t t + r avec l'hypothese que E{r} = 0. Soit un vecteur de deux propriétés x^ 1 x^ 2 avec une prédiction de premier ordre. X^ t x^1 x^1' x^2 x^2' Le prédiction est : X t := ϕ ( t)x^t t + R avec le matrice de transition, ϕ( t) 1 t 0 0 0 1 0 0 0 0 1 t 0 0 0 1 La précision de X t est la covariances les propriétés x^ où P t σ xx' σ^x 2 1 σ^x 1 'x 1 σ^x 2 x 1 σ^x 2 'x 1 σ^x 1 x 1 ' σ^x 1 ' 2 σ^x 2 x 1 ' σ^x 2 'x 1 ' σ^x 1 x 2 σ^x 1 'x 2 σ^x 2 2 σ^x 2 'x 2 σ^x 1 x 2 ' σ^x 1 'x 2 ' σ^x 2 x 2 ' σ^x 2 ' 2 E{ (x - x^ )(x' - x^ ')} σ x' 2 E{ (x' - x^ ') 2 } 8-4
La covariance P t est prédite par P t := ϕ( t)p^ t- t ϕ( t) T + E{ R t R t T} Le processus peut avoir les dérivés d'ordre supérieur à le modèle. Ces dérivés donnent lieu au "résidu" R. R est supposée d'être moyenne nul. Son deuxième moment indique la perte de précision dû au modèle linéaire. X t := X t t + X t t t T + R t. avec Q E{R t R t T} Prédiction : I : II : X t := ϕ ( t)x^ t t P t := ϕ( t)p^ t-1 ϕ( t) T + Q x 8-5
Modèle du capteur Un capteur proj l'état du monde vers un vecteur d'observation : III : Y τ := H X Y X t IV : P y E{W t W t T } La matrice de H Y X est un modèle linéaire du capteur. Les observations sont corrompues par les bruits aléatoires ainsi que une erreur d'approximation. Ceci est représenté par l'erreur du capteur : W t. On suppose que E{W t } = 0 tandis que P y E{W t W t T } Par exemple, considère l'observation Y d'un état X t Y = H Y y X X => 1 y 2 = 1 0 0 0 0 0 1 0 x 1 x 1 ' x 2 x 2 ' deuxième exempl, la matrice de projection d'une caméra : w X i = M s i X s Aproximations des modèle non-linéaires Souvent le vrai capteur n'est pas linéaire. Y = F(X) Dans ce cas, on peut faire une approximation linéaire avec la dérivée première, (la Jacobienne), calculé au tour de l'estimation actuelle. H Y Y X J X = F(X^t) y 1 y 1 y 1 y 1 = x 1 x 2 x 3 x 4 X^t y 2 y 2 y 2 y 2 x 1 x 2 x 3 x 4 donc : y 1 y = 2 y 1 y 1 y 1 y 1 x 1 x 2 x 3 x 4 y 2 y 2 y 2 y 2 x 1 x 2 x 3 x 4 x 1 x 1 ' x 2 x 2 ' 8-6
Validation Validation repose sur une mesure de similarité. Une telle mesure est la "Distance de Mahalanobis" ou "Différence normalisée par covariance" Soit la prédiction X t avec sa précision P t l'observation Y τ avec sa précision P y Le distance de Mahalanobis est V : d 2 = 1 2 { ( H X Y X t Y τ ) T (P t + P y ) -1 (H X Y X t Y τ)} Pour une scalaire : d 2 = (x y)2 2 (σ x 2 +σy 2 ) On considère que X t, P t définie une probabilité d'une observation Y. Si il y a K prédictions : X kt, P kt, l'observation est associée a : Arg k Min {d k 2 } tel que d k seuil. Le seuil est un multiple de l'écart type. Son choix depend du problème 8-7
Mise à Jour Contraint d'un modèle par une Observation Soit un modèle X t, P t une observation Y τ, P y Y X ^ X Y fournis une contrainte sur X. La nouvelle valeur est donnée par un moyen, pondérée par le variance: Les variances combine comme les résistances parallèles : σ^x 2 = X^nt = σ^x 2 ( σ x 2σ y 2 σ y 2 + σ x 2 X σ x 2 + Y σ y 2 ) Formulation Kalman (estimation récursif): Le Gain de Kalman Puis : K = σ x 2 σ y 2 + σ x 2 σ^x 2 = σ x 2 K σ x 2 X^ = X + K (Y X ) En plusieurs dimensions, le gain de Kalman est une matrice. Supposons que Y X sont dans le même espace. (Même nombre de variables. puis : K := P t ( P t + P y )-1 X^ ^ t = X t + K [ Y X t ]. P^ t = P t - K P t. 8-8
Et si X Y ne sont pas du même repère, il faut le transformation H X Y son inverse H Y X = H X Y T Équations VI, VII VIII du Filtre de Kalman : VI : K := P t H X Y T [ H X Y P t H X Y T + P y ] -1 VII : X^ t := X t + K [Y τ H X Y X t ] VIII : P^ t := P t K H X Y P t L'Innovation Le différence (Y τ H X Y X t )est l'innovation de l'observation. Le gain K est une matrice qui donne le poids pour une mise à jour de x i à partir de chaque composant de l'innovation. X^ t := P t K H X Y P t x 1 x 2 x = 3 k x3y1 k x3y2 x 4 k x4y1 k x4y2 k x1y1 k x1y2 k x2y1 k x2y2 y 2 H 2 X t = y 1 H 1 X t x 1 x 1 y 1 y 2 x 2 x 2 y 1 y 2 x 3 x 3 y 1 y 2 x 4 x 4 y 1 y 2 y 1 H 1 X t y 2 H 2 X t K := P t H X Y T [ H X Y P t H X Y T + P y ] -1 k x1y1 k x1y2 k x2y1 k x2y2 k x3y1 k = x3y2 k x4y1 k x4y2 σ 11 2 σ 12 2 σ 13 2 σ 14 2 σ 2 21 σ 2 22 σ 2 23 σ 2 24 σ 2 31 σ 2 32 σ 2 33 σ 2 34 σ 2 41 σ 2 42 σ 2 43 σ 2 44 h 11 h 21 h 12 h 22 h 13 h 23 h 14 h 24 1 ou =( h 11 h 12 h 13 h 14 h 21 h 22 h 23 h 24 σ 2 11 σ 2 12 σ 2 13 σ 2 14 σ 2 21 σ 2 22 σ 2 23 σ 2 24 σ 2 31 σ 2 32 σ 2 33 σ 2 34 σ 2 41 σ 2 42 σ 2 43 σ 2 44 h 11 h 21 h 12 h 22 h 13 h + 23 h 14 h 24 σ 2 11 σ 2 12 σ 2 21 σ 2 ) 22 8-9