Discussion :

[kronanberg]

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.

[Black Templar]

Bonjour ! Je n'ai pas regardé ni vraiment compris sur quoi tu travailles (je ne vois pas les équations :p )
Par contre, je vois une petite erreur dans l'algorithme !

La matrice de transition (la jacobienne de tes fonctions de transition : matrice F) doit être calculé à l'aide du vecteur d'état estimé à l'instant t-1.
La matrice d'observation (la jacobienne de tes fonctions d'observations : matrice H) doit être calculé entre la phase de prédiction et celle de mise à jour à l'aide du vecteur d'état prédit !


Dans ton cas, les équations d'observation sont linéaires, donc la matrice H est constante.
Par contre, tu dois recalculer ta matrice F avec l’estimation à l'instant t-1 !! (et non avec ton vecteur de mesure comme tu le fais !)


Dans Kalman, tu as trois informations à distinguer :

• la vrai valeur à estimer (valeur qui t'es inaccessible !)
• l'estimation de tes paramètres (que tu espère être pour le mieux)
• les mesures, bruité, biaisé et déformée (utilisé uniquement dans la phase de mise à jour pour calculer l'estimation)

Dans ton cas, tu sembles utiliser les mesures pour calculer la matrice F alors que ces mesures ne doivent intervenir que pour mettre à jour ton vecteur d'état ! (d'ailleurs, tu ne sembles pas avoir initialiser ton vecteur d'état)

++
Black Templar

[kronanberg]

Bonjour!

Merci pour la réponse. Et effectivement je n'utilisais pas le bon vecteur lorsque je mettais ma matrice de transition F à jour.

J'ai donc modifié mon code mais j'ai toujours le même problème, je n'arrive pas à retrouver mes mesures de départ.

J'ai ré-expliqué le début du problème dans le code ci dessous.

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
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                       Filtre de Kalman Etendu                      %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%                                                                   %
% Application d'un filtre de Kalman sur un système de mesure du vent %  
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
% Mesure Disponible :                                                %
%           Angle(angle du vent en °)                                %
%           Vitesse (Vitesse du vent en noeuds)                      %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
% BUT : Calculer les coordonnées x et y du vecteur du vent           %
%                                                                    %
%           x = Vitesse * cos(Angle*DegToRad)                        %
%           y = Vitesse * sin(Angle*DegToRad)                        %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
% Equation d'etat :                                                  %
%                                                                    %
%           Xk = Fk * Xk-1 + Wk                                      %
%                                                                    %
%           Xk = [  Angle                                            %
%                   Vitesse                                          %
%                   x                                                %
%                   y]                                               %
%                                                                    %
%           Fk = [  1   0   0   0;                                   %
%                   0   1   0   0;                                   %
%                   dx/dAngle dx/dvitesse dx/dx  dx/dy;              %
%                   dy/dAngle dy/dvitesse dy/dx  dy/dy]              %
%                                                                    %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
% Equation de mesure :                                               % 
%                                                                    %
%           Yk = Hk * Xk + Vk                                        %
%                                                                    %
%           Yk = [  Angle                                            % 
%                   Vitesse]                                         %
%                                                                    %
%           Hk = [  1   0   0   0;                                   % 
%                   0   1   0   0]                                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%    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 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];  
% Matrice de covariance d'état   
Pp = [1 0 0 0;
      0 1 0 0;
      0 0 1 0;
      0 0 0 1];
 
% Matrice Identité 
Id = [1 0 0 0;
      0 1 0 0;
      0 0 1 0;
      0 0 0 1]; 
 
% Gain de Kalman   
K = [0 0;
     0 0;
     0 0;
     0 0];
 
Xe = [Y_k(:,1);0;0];                           % initialisation du vecteur d'état pour l'estimation
Xp = [Y_k(:,1);0;0];                           % initialisation du vecteur d'état pour la prediction
Xe(:,2) = Xe(:,1);
Xp(:,2) = Xp(:,1);
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                       Filtre de Kalman                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=3:N   
 
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Calcul de la mise à jour à l'instant i %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    Xe(:,i) = Xp(:,i-1) + K * (Y_k(:,i-1) - H*Xp(:,i-1));
    K = Pp*H'*(inv(H*Pp*H' + R));
    Pe = (Id - K*H) * Pp;
 
    c1 = - DegToRad * Xe(2,i)* sin(Xe(1,i)*DegToRad);
    c2 = cos(Xe(1,i)*DegToRad);
    c3 = DegToRad * Xe(2,i)* cos(Xe(1,i)*DegToRad);
    c4 = sin(Xe(1,i)*DegToRad);
    F = [1  0  0 0;
         0  1  0 0;
         c1 c2 0 0;
         c3 c4 0 0];
 
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Calcul de la prediction à l'instant i  %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    Xp(:,i) = F * Xe(:,i);                   % estimation de l'état
    Pp = F*Pe*F' + Q;                        % matrice de covariance de l'état     
 
end
 
x_cal = Xe(3,:)';
y_cal = Xe(4,:)';
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%    Calcul de l'angle du vent                                 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
cal_Angle = RadToDeg * atan2(y_cal,x_cal);
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%    Calcul de la vitesse du vent                              %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
cal_Vitesse = sqrt( x_cal.^2 + y_cal.^2);

Le soucis est que dans mon résultat, j'ai toujours la valeur de l'angle ou de la vitesse calculée d'après les coordonnées qui est supérieure à la mesure. Voici un exemple de résultat : en bleu le signal mesuré en rouge le signal calculé d'après les résultats.

Je vais essayer de jouer sur les paramètres du bruits des signaux pour voir si j'arrive pas à corriger le problème !! MAIS ce qui est difficile c'est de trouver pourquoi ça ne fonctionne pas ? (erreur dans la covariance du bruit, erreur engendré par l' approximation linéaire, erreur dans le système).

Sinon est ce que tu aurai un exemple de la boucle for ou tu mets en place le filtre de Kalman etendu. C'est peut être la dedans que j'ai fais une erreur.

Merci.

[Black Templar]

Bonjour,

Il me semble que ce n'est pas encore très clair au niveau de l'équation de transition (X^+ = f(X))

Premièrement, ces équations de transitions doivent lier l'instant précédent à l'instant courant ! (On a donc une relation faisant intervenir le temps)
Dans ton programme, tu utilises les équations liant x et y à v et alpha alors qu'il faudrait trouver des équations liant x(t-1), y(t-1), alpha(t-1) et v(t-1) à x(t), y(t), alpha(t) et v(t).
Je me doute que tu ne possèdes pas ces relations (ni même un modèle simpliste de l'évolution temporelle du vent). Du coup, tu dois les inventer toi même et supposer que x(t-1) = x(t), que y(t-1) = y(t), que alpha(t-1) = alpha(t) et que v(t-1) = v(t).
Tu obtiens donc une matrice identité.
Ce résultat peut être déroutant, mais il faut espérer que la matrice de covariance du bruit de commande Q te permettra de rectifier les erreurs introduit avec ces équations.

Deuxièmement, je suis d'accord que ta matrice H est bonne. Mais dans ce cas là, ça ne sert strictement à rien d'intégrer x et y dans le vecteur d'état. Autant utiliser Kalman pour estimer alpha et v puis d'utiliser ces estimations pour calculer x et y à part. (mais dans ce cas là, un filtre de Kalman n'est peut être pas le meilleur estimateur à adopter...)
Ce que tu peux essayer, c'est de triturer tes équations x=v.cos(alpha) et y=v.sin(alpha) pour obtenir v et alpha en fonction de x et de y
(spoiler : alpha = arctg(y/x) et v = racine(x²+y²) )
A partir de ces nouvelles équations, tu peux essayer de réécrire la jacobienne de H (mais je ne suis pas sur que ça changera quelque chose (à tester))

Enfin, troisièmement, les jacobiennes sont utilisé uniquement pour calculer le gain de Kalman K et la matrice de covariance de l'erreur d'estimation P.
Pour la prédiction de X et sa mise à jour, il faut utiliser les équations non linéaires de f et de h !!! (on a les équations non linéaires sous la main, autant les utiliser au maximum. Les jacobiennes sont utilisé uniquement car on ne peux pas appliquer les équations non linéaires lors du calcul de K et de P)


En résumé : Kalman n'est peut être pas approprié à ce que tu cherches à faire car tu ne connais aucun modèle de l'évolution temporel du vent.
Si au contraire, tu peux modéliser cette évolution (même de façon grossière), Kalman serait déjà bien plus intéressent.

++
Black Templar

[kronanberg]

Bonjour,

Merci pour ta réponse, je crois que ça commence à être plus clair pour moi.

Finalement mon exemple ne me parait pas approprié pour utiliser le filtre de Kalman ! Le but pour moi était de valider l'application du filtre de Kalman étendu sur un exemple de manière à l'utiliser par la suite dans ma chaine de correction des mesures du vent.

Je m'aperçois maintenant que ça ne sera peut être pas le filtre à utiliser car j'ai très peu de relation dans le temps. Une des relations que je peux faire est de considérer le vent constant en fonction du temps.

En tout cas, merci pour tous ces conseils.

A +