[Débutant] Adaptive Klaman filter

Bonjour,

j'ai dévéloppé mon code de filtre de kalman et eil converge bien mais quand j'ai lui ajout un bout de progamme pour le rendre un adaptive kalman filter il ne converge plus !!

Code:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
 
  M=30;
  A = eye(4,4);
  Q = [10 0 0 0; 0 1 0 0 ; 0 0 1 0 ; 0 0 0 1];
  R= 0.01;
  x = [0 0 0 0]';
  P = [10000 0 0 0; 0 10000 0 0 ; 0 0 10000 0 ; 0 0 0 10000];
  b=zeros(M,1);
 
for k=3:N
    xp = A*x ;
    Y = FI(k,:)*xp;
    z(k)=Y;
 
 
% _____________PArtie pour le rendre adaptive kalman filter
if (k >= M)
 
    for j=1:k
    e(j)= Ym(j)-z(j)'; 
    end
 
    for i=k-M+1:k
        for j=1:M
        b(j)=e(i);
        end
    end
    inov=(b'*b)/M;
%     R= inov-FI(k,:)*P*FI(k,:)';
    Pp = A*P*A' + Q;
    K = Pp*FI(k,:)'*inv(FI(k,:)*Pp*FI(k,:)' + R);
%     Q= K*inov*K';
    x = xp + K*(Ym(k) - Y);
    P = Pp - K*FI(k,:)*Pp + K*R*K';
    X (:,k)= x;
end
        %______________Fin _____________________
 
    Pp = A*P*A' + Q;
    K = Pp*FI(k,:)'*1/(FI(k,:)*Pp*FI(k,:)' + R);
    x = xp + K*(Ym(k) - Y);
    P = Pp - K*FI(k,:)*Pp;