Bonjour,

Voilà je travaille actuellement sur le filtre de Kalman étendu et je me trouve confronté à un problème lors de la linéarisation de mon système. Je passe par les matrices Jacobiennes mais je n'arrive pas à obtenir le résultat souhaité.

Voici l'exemple simple sur lequel je m'entraine :

J'ai deux mesures, la vitesse du vent (V) ainsi que l'angle du vent (A). J'essaie de calculer les coordonnées du vecteur vent correspondant.

x = V x cos(A)
y = V x sin(A)

Les vecteurs du système de kalman sont donc :
vecteur de mesure : Yk = [ A; V]
vecteur d'état : Xk = [A; V; x; y]

La matrice de transition de mesure est linéaire (donc pas d'évolution dans le temps) alors que la matrice de transition de l'équation d'état n'est pas linéaire, elle évoluera dans le temps. A chaque étape je recalcule les jacobienne de la matrice de transition.

Voici le code que j'utilise :

Code : Sélectionner tout - Visualiser dans une fenêtre à part
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
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%    Facteur de conversion                                     %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DegToRad = (2*pi) / 360;            % Degrés en Radians
RadToDeg = 360 / (2*pi);            % Radians en Degrés
 
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%   
%                      KALMAN FILTER                           %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                       Initialisation                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Taille du vecteur
N = size(Angle,1);
 
% Vecteur des mesures
Y_k = [Angle';  Vitesse'];
 
% Matrice de transition de l'equation d'état
c1 =  - DegToRad*Vitesse(1)*sin(Angle(1)*DegToRad);
c2 = cos(Angle(1)*DegToRad);
c3 = DegToRad*Vitesse(1)*cos(Angle(1)*DegToRad);
c4 = sin(Angle(1)*DegToRad);
F = [1  0  0 0;
     0  1  0 0;
     c1 c2 0 0;
     c3 c4 0 0];
 
% Matrice de covariance du bruit d'etat
Q = [1  0  0  0;
     0  1  0  0;
     0  0  1  0;
     0  0  0  1];
 
% Matrice de transition de l'equation de mesure
H = [1  0   0  0 ;
     0  1   0  0]; 
 
% Matrice de covariance du bruit de mesure
R = [1  0;
     0  1];  
 
Pp = [1 0 0 0;
      0 1 0 0;
      0 0 1 0;
      0 0 0 1];
 
Id = [1 0 0 0;
      0 1 0 0;
      0 0 1 0;
      0 0 0 1];   
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                       Filtre de Kalman                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=2:N 
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %       Calcul de la prediction          %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    Xp(:,i) = F * Xe(:,i);                   % estimation de l'état
    Pp = F*Pe*F' + Q;                        % matrice de covariance de l'état    
 
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %       Calcul de la mise à jour         %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    Xe(:,i) = Xp(:,i-1) + K * (Y_k(:,i-1) - H*Xp(:,i-1));
    K = Pp*H'*(inv(H*Pp*H' + R));            % Calcul du gain
    Pe = (Id - K*H) * Pp;                    % matrice de covariance de l'état
 
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %       Calcul de la jacobienne          %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    c1 =  - DegToRad*Vitesse(i)*sin(Angle(i)*DegToRad);
    c2 = cos(Angle(i)*DegToRad);
    c3 = DegToRad*Vitesse(i)*cos(Angle(i)*DegToRad);
    c4 = sin(Angle(i)*DegToRad);
    F = [1  0  0 0;
        0  1  0 0;
        c1 c2 0 0;
        c3 c4 0 0];
end
 
Angle2  = Xe(1,:)';
Vitesse2  = Xe(2,:)';
x_cal = Xe(3,:)';
y_cal = Xe(4,:)';
Je recalcule ensuite la vitesse et l'angle du vent à partir des coordonnées x et y calculé par le filtre de Kalman. Les signaux varient bien en même temps mais il n'ont pas la même amplitude. Je me demande donc d'ou vient cette erreur.

Est la linéarisation par les matrices jacobiennes qui induit cet erreur ? Est ce que cela peut provenir des matrices de covariances du bruit qui sont initialiser à 1 ? Ou est ce que c'est une erreur dans mon système ?

Voilà si quelqu'un à le courage de jeter un coup d'oeil sur mon code ou alors si vous avez une explication je suis preneur.

Merci d'avance.