J'arrive pas a trouver le problème aidez moi svp

Fermé
jihene - Modifié par jihene le 10/04/2013 à 16:52
voici le code que j 'ai écris pour une observation par filtre de kalman, l'exécution de me signale aucune erreur sauf que la simulation est loin d'etre juste aidez moi (je suis débutante avec la théorie de kalman)

clear
clc
close all
%% Modele
% X(t) = A * X(t-1) + V(t) ---> V=N(Mv,Q) : bruit d'etat
% y(t) = C * X(t)+N(t) ---> N=N(Mr,R) : bruit d'observation

StateDim = 2; % Number of states ( size(A,1) )
ObsDim = 1; % Number of observations ( size(C,1) )

R=0.39;
L=0.0023;
c=3.2e-6;

A=[-R/L -1/L ; 1/c 0];
B=[1/L 0;0 -1/c];
C=[0 1];

N = 10; % nombre de points
X = zeros(StateDim,N);
y = zeros(ObsDim,N);
u=zeros(2,N)
u(1,1)=100

%% bruit de mesure
Var_PNoise = 0.1; % variance
Mu_PNoise = 0; % moyenne
Std_PNoise = sqrt(Var_PNoise)'; % ecart type
PNoise = Std_PNoise * randn(StateDim,N) + Mu_PNoise*ones(StateDim,N); % bruit gaussien
Q = cov(PNoise'); % matrice de Covariance

%% bruit d'observation
Var_ONoise = 2; % variance
Mu_ONoise = 0; % moyenne
Std_ONoise = sqrt(Var_ONoise)'; % ecart type
ONoise = Std_ONoise * randn(ObsDim,N) + Mu_ONoise*ones(ObsDim,N); % bruit gaussien
R = cov(ONoise'); % matrice Covariance

%initialisationn
X(:,1) = [1 0]' % etat
y(1) = C * X(:,1) + ONoise(:,1); % observation

%EQUATIONS D'ETAT

for i = 2 : N
X(:,i) = A * X(:,i-1) +B*u(:,i)+ PNoise(:,i);
y(:,i) = C * X(:,i) + ONoise(:,i); %etat reel
end

%% Kalman
xh(:,1) = 0.01*randn(StateDim,1); % etat initial
Px = eye(StateDim); % matrice de covariance initiale à matice d'identité

for i = 1 :N
% etat estimé
xh_(:,i) = A * xh(:,i)+B*u(:,i);

% matrice de covariance de l'etat
Px_ = A*Px*A' + Q;

% gain du filtre
K = Px_ * C' * inv(C*Px_*C' + R);

%estimation de l'observation
yh(:,i) = C * xh_(:,i) + R;

% estimation de l'etat a l'instant t+1
xh(:,i+1) = xh_(:,i) + K * (y(:,i) - yh(:,i));

% matrice de covariance de l'etat
Px = Px_ - K*C*Px_;


end

%% tracage
figure(1)
plot(y,'b')
figure(2)
plot(yh,'r')