Filtrage de Kalman appliqué à un système linéaire à
Transcription
Filtrage de Kalman appliqué à un système linéaire à
UPMC Master 2 – SSIR 2005/06 UE Informatique pour la Robotique Filtrage de Kalman appliqué à un système linéaire à accélération constante. PARTIE THEORIQUE : Nous vous rappelons les équations du filtre de Kalman ci-dessous : établies à partir de l’équation d’état : où x(k) est l’état à l’instant k, u(k) est le vecteur de commande, v(k) est un bruit centré. et en considérant l’équation de mesure : où w(k) est un bruit gaussien centré. PARTIE EXPERIMENTALE : 1 Description du problème On désire estimer la position et la vitesse d'un mobile se déplaçant à accélération constante. Cette accélération est soumise à des accélérations aléatoires (variation du régime moteur, variation du coefficient d'adhérence,...). L'ensemble de ces accélération aléatoires sont supposées indépendantes et de moyenne nulle. On peut donc écrire entre deux instants d'échantillonnage de la mesure (tn≤t < tn+1) : γ(t) = γ0+γn E[γn] = 0 E[γnγn+k] = Γ.δk . 1. Écrire l'équation de récurrence donnant la vitesse x n+1 à l’échantillon tn+1 en fonction . de la vitesse x n à l’instant tn,γ0 l’accélération à l’instant 0, et γn un bruit gaussien sur l’accélération de moyenne E[γn] = 0 et de variance Γ (on posera T = tn+1−tn). 2. Écrire de même l'équation de récurrence donnant xn+1 la position à l’instant tn+1 en fonction de xn et des variables précédentes. 3. Montrer que la représentation d'état du système peut se mettre sous la forme (En supposant que seule la position est accessible à la mesure et que cette mesure est bruitée par un bruit gaussien et moyenne nulle et de variance Ω) Xk+1 = F.Xk+Buk+Gvk Zk = HXk+Wk . avec Xk = [ xk , x k ] T . Donner les matrices F, H ainsi que les vecteurs B, G, Wk et des scalaires uk et vk ; en précédant par identification avec les équations développées dans les questions 1 et 2. 4. 5. Programmer les équations des filtres prédicteur et estimateurs. On remarquera que les équations sont plus simples car le système est stationnaire. Générer un ensemble d'observations bruitées en utilisant les paramètres suivants : T = 0.1s , γ0 = 1ms−2 , Γ = 1 , Ω = 100 , X0 = [ 0 -10 ]T On générera la séquence d'accélérations aléatoires en utilisant la fonction randn. On supposera par la suite que les variances de bruit d'état et de mesures nous sont inconnues. 2 Influence des conditions initiales Pour Γ = 1 et Ω = 100, examiner l'influence du choix des conditions initiales. On considérera les cas suivants : Etat initial X0 = [ 0 – 10]T X0 = [ z0, (z1−z0)/(T) ]T X0 = [ z0, (z1−z0)/(T) ]T X0 = [ z0, (z1−z0)/(T) ]T Variance initiale Λ 0 ⎡0 ⎢0 ⎣ ⎡0 ⎢0 ⎣ 0⎤ 0⎥⎦ 0⎤ 0⎥⎦ 0 ⎤ ⎡Ω ⎢ 0 4Ω / T 2 ⎥ ⎣ ⎦ 0 ⎤ ⎡Ω 100 ⎢ 2⎥ ⎣ 0 4Ω / T ⎦ 3 Influence du choix des variances des bruits Pour des valeurs de X0 et Λ0 bien choisies, examiner l'effet du choix arbitraire des variances de bruit de mesure et d'état. On considérera en particulier les valeurs suivantes et on suivra l'évolution de la variance d'erreur d'estimation sur la position. bruit de mesure Ω 100 10000 1 100 100 10000 1 bruit d'état Γ 1 1 1 100 0.01 100 0.01