ABCelectronique : portail d'information dans le domaine de l'électronique

Merci de ne pas poster des messages en relation avec le piratage.


    Forums de Abcelectronique > Software / Péri-informatique
  » filtre de Kalman étendu, Matlab
Identifiant Se souvenir de moi ?
Mot de passe
Répondre     Nouvelle discussion
filtre de Kalman étendu, Matlab

 

shag
shag ☆☆☆☆☆☆ 02/08/2015, 04h08 #1  
Bonjour,

je cherche à implémenter sous Matlab un filtre de Kalman étendu. De base je n'y connaissais pas grand chose, je suis donc parti d'un exemple de code, malgré ça j'ai beaucoup de mal.

Voici quelques informations :

L'équation dynamique qui régit le système est la suivante :

dx/dt = y/C
dy/dt = 0

Seule la variable y est mesurée (avec du bruit).
C est une constante qui vaut 1000 dans le code.
La fréquence de mesure est 3 Hz.
Temps de mesure : 1400 seconds
y évolue comme un bruit blanc de variance 1 (en fait pas exactement mais pour l'instant je prend ça parce que je sais pas faire le cas "réel" qui est que y est une série temporelle dont l'amplitude spectrale de puissance suit une loi type [TEX]1.10^{-5} \frac{1}{f}\frac{1}{\sqrt{Hz}}[/TEX] et que je ne sais pas comment créer cette série temporelle donc si au passage quelqu'un peut m'aider ici ce serait cool, j'ai essayé un truc mais finalement ça ne marche pas vraiment..)

La valeur initiale de y est : 1*randn donc un échantillon de bruit blanc de variance 1
La valeur initiale de x est : 0.1*randn donc un échantillon de bruit blanc de variance 0.1

Cependant les résultats sont complètements faux, la prévision par le filtre est complètement dans les choux, plusieurs ordres de grandeurs au dessus de la valeur réelle, en plus de ça les prévisions s'arrêtent à 479 secondes environ alors que les valeurs "réelles" vont bien jusqu'à 1400 .
J'ai également un warning pendant l'exécution :



(pour info la ligne 135 c'est le gain de Kalman K = P * H' * inv(H * P * H' + R); )

Mais je ne suis pas sur du tout du code.
Par exemple la Jacobienne F, j'ai utilisé :

F = [1 xhat(2)/C;
0 1];

car si F = df/dx (où x n'est pas la variable x mais x = [x y]) alors dx/dx = 1, dx/dy = Y/C, dy/dx = 0, dy/dy = 1

Je ne suis pas sur non plus pour les xdot, xhatdot etc.
Pour H j'ai pris = [0 1] car on mesure pas la 1ere variable mais la 2eme (y).

Bref je rame énormément et j'aurai vraiment besoin qu'une personne compétente dans le domaine m'aide un peu :/
Merci d'avance pour votre aide et n'hésitez pas à demander s'il faut d'autres précisions, j'ai vraiment besoin d'un coup de pouce sur ce truc^^

Voici le code en entier que j'utilise :

Code:
clear all close all C = 1000; R = 1; x = [0.1*randn; 1*randn]; %[x y] xhat = x; H = [0 1]; sigma_model = 1; P = [sigma_model^2 0; 0 sigma_model^2]; T = 1/3; % measurement time step tf = 1400; % measurement time dt = tf/10000; % time step for integration xArray = x; xhatArray = xhat; zArray = [0]; for t = T : T : tf % Simulate the system. for tau = dt : dt : T xdot(2,1) = 0; xdot(1,1) = x(2)/C; x = x + xdot * dt; end % Simulate the measurement. z = H * x + sqrt(R) * randn; for tau = dt : dt : T xhatdot(2,1) = 0; xhatdot(1,1) = xhat(2)/C; xhat = xhat + xhatdot * dt; F = [1 xhat(2)/C; 1 0]; Pdot = F * P + P * F'; P = P + Pdot * dt; end K = P * H' * inv(H * P * H' + R); xhat = xhat + K * (z - H * xhat); P = (eye(2) - K * H) * P * (eye(2) - K * H)' + K * R * K'; % Save data for plotting. zArray = [zArray z]; xArray = [xArray x]; xhatArray = [xhatArray xhat]; end close all; t = 0 : T : tf; figure plot(t,xArray(1,:)); hold on; plot(t,xhatArray(1,:),'r'); figure plot(t,zArray,'g');hold on;plot(t,xArray(2,:)); plot(t,xhatArray(2,:),'r');
Répondre