Filtre de Kalman Etendu, Matlab

[HKPK]

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 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 :

In CodeKalman at 135
Warning: Matrix is
singular to working
precision.

(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 :

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');

-----