Discussion :

[A_hind]

salut a tous ,
je voudrais obtenir votre aide sur un problemme que j'ai rencontré en utilisant le filtre de KALMAN etendu .
le modèle d’état utilisé est le suivant (modèle a vitesse constante d'un avion )

X(k+1)=AX(K)+W; equation d'etat
Z(K+1)=h(X)+V ; equation de mesure qui est non lienaire (donnée radar R,tehta)

vecteur d'etat x=[X vx y xy]


et h=[sqrt(x²+y²) %distance R
artan(y/x)] %azimut
le code est le suivant

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
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
 
close all;
clear all;
clc;
 
dt=0.1;
duration=1000;
 
measnoise1 = 50;  
accelnoise1 = .125;
measnoise2=pi/180;
accelnoise2=.125;
 
a1 = [1 dt; 0 1]; % transition matrix
b1 = [dt^2/2; dt]; % input matrix
c1 = [1 0]; % measurement matrix
a2=a1;b2=b1;c2=c1;
x1 = [0; 200]; % initial state vector
x2= [0;300];
 
a = [a1 zeros(2,2); zeros(2,2) a2];
x = [x1;x2];
%---------------------------------------------------------------------
 
xhat = x; % initial state estimate
 
 
 
Sz=[measnoise1^2 0;0 measnoise2^2 ]%measurement error covariance
 
W = [accelnoise1*dt^2/2; accelnoise1*dt; accelnoise2*dt^2/2; accelnoise2*dt];
Sw = W*W';% process noise cov
 
 
P = Sw; % initial estimation covariance
 
% Initialize arrays for later plotting.
pos1 = [];pos2 = []; % true position array
poshat1 = [];poshat2 = []; % estimated position array
posmeas1 = [];posmeas2 = []; % measured position array
vel1 = [];vel2 = []; % true velocity array
velhat1 = [];velhat2 = []; % estimated velocity array
 
for t = 0 : dt: duration,
% Use a constant commanded acceleration of 1 m/sec^2.
u = [0; 0];
% Simulate the linear system.
ProcessNoise1 = accelnoise1 * [(dt^2/2)*randn; dt*randn];
ProcessNoise2 = accelnoise2 * [(dt^2/2)*randn; dt*randn];
ProcessNoise = [ ProcessNoise1 ;  ProcessNoise2];
x = a * x  + ProcessNoise;
 
% Simulate the noisy measurement
MeasNoise1 = measnoise1 * randn;
MeasNoise2 = measnoise2* randn ;
MeasNoise = [MeasNoise1 ; MeasNoise2];
 
y = [sqrt((x(1)^2)+(x(3)^2)) ; atan2(x(3),x(1))] + MeasNoise;
 
% Extrapolate the most recent state estimate to the present time.
xhat = a * xhat + b * u;
 
yc = [sqrt((xhat(1)^2)+(xhat(3)^2)) ; atan2(xhat(3),xhat(1))] + MeasNoise;
 
h = [xhat(1)/sqrt((xhat(1)^2)+(xhat(3)^2)) 0 xhat(3)/sqrt((xhat(1)^2)+(xhat(3)^2)) 0 ; 
     (-xhat(3)/((xhat(1)^2)+(xhat(3)^2))) 0 (xhat(1)/((xhat(1)^2)+(xhat(3)^2))) 0];%4X4
 
% Form the Innovation vector.
 
Inn = y -yc;
% Compute the covariance of the Innovation.
s = h * P * h' + Sz;
% Form the Kalman Gain matrix.
K = a * P * h' * inv(s);
% Update the state estimate.
xhat = xhat + K * Inn;
% Compute the covariance of the estimation error.
P = a * P * a' - a * P * h' * inv(s) * h * P * a' + Sw;
% Save some parameters for plotting later.
pos1 = [pos1; x(1)];
posmeas1 = [posmeas1; y(1)*cos(y(2))];
poshat1 = [poshat1; xhat(1)];
vel1 = [vel1; x(2)];
velhat1 = [velhat1; xhat(2)];
 
pos2 = [pos2; x(3)];
posmeas2 = [posmeas2; y(1)*sin(y(2))];
poshat2 = [poshat2; xhat(3)];
vel2 = [vel2; x(4)];
velhat2 = [velhat2; xhat(4)];
 
% plot(pos1,pos2,'o', posmeas1,posmeas2,'*', poshat1,poshat2,'+');
% grid;
% xlabel('X (m)');
% ylabel('Y (m)');
% title('Target Position (True, Measured, and Estimated)')
% legend('True','Measured','Estimated');
% 
% pause();
end
 
t = 0 : dt : duration;
 
close all;
figure;
subplot(2,2,1);
plot(t,pos1, t,posmeas1, t,poshat1);
grid;
xlabel('Time (sec)');
ylabel('X (m)');
title('Target Position following X axis (True, Measured, and Estimated)')
legend('True','Measured','Estimated');
subplot(2,2,2);
plot(t,pos1-posmeas1, t,pos1-poshat1);
grid;
xlabel('Time (sec)');
ylabel('Position Error following X axis (m)');
title('Position Measurement Error and Position Estimation Error');
legend('Measurement Error','Estimation Error');
subplot(2,2,3);
plot(t,pos2, t,posmeas2, t,poshat2);
grid;
xlabel('Time (sec)');
ylabel('Y (m)');
title('Target Position following Y axis (True, Measured, and Estimated)')
legend('True','Measured','Estimated');
subplot(2,2,4);
plot(t,pos2-posmeas2, t,pos2-poshat2);
grid;
xlabel('Time (sec)');
ylabel('Position Error following Y axis (m)');
title('Position Measurement Error and Position Estimation Error');
legend('Measurement Error','Estimation Error');
 
figure;
subplot(2,2,1);
plot(t,vel1, t,velhat1);
grid;
xlabel('Time (sec)');
ylabel('Velocity (m/sec)');
title('Velocity following X axis (True and Estimated)');
legend('true','Estimated');
subplot(2,2,2);
plot(t,vel1-velhat1);
grid;
xlabel('Time (sec)');
ylabel('Velocity Error following X axis (m/sec)');
title('Velocity Estimation Error');
subplot(2,2,3);
plot(t,vel2, t,velhat2);
grid;
xlabel('Time (sec)');
ylabel('Velocity (m/sec)');
title('Velocity following Y axis (True and Estimated)');
legend('true','Estimated');
subplot(2,2,4);
plot(t,vel2-velhat2);
grid;
xlabel('Time (sec)');
ylabel('Velocity Error following Y axis (m/sec)');
title('Velocity Estimation Error');
 
figure;
%plot(pos1,pos2, posmeas1,posmeas2,'*', poshat1,poshat2);
plot(pos1,pos2,'o', posmeas1,posmeas2,'*', poshat1,poshat2,'+');
grid;
xlabel('X (m)');
ylabel('Y (m)');
title('Target Position (True, Measured, and Estimated)')
legend('True','Measured','Estimated');

j'ai constaté deux choses:
-l'erreur de mesures augmente avec la distance !! (j'aurais dit que c'est peut etre parce que mes mesures sont généré en coordonné polaire !!!)
-kalman diverge vers la fin je sais pourquoi!!!

-et si je change l'initialisation du filter de kalman Xhat au lieu que ça soit égal a X=[0 200 0 300] je le met différent ...les résultats ne sont pas du tout cohérent (KALMAN diverge completement)

Est ce que l'initialisation doit toujours être la méme que la trajectoire réelle!!!!!???


alors SVP si une personne a déja travailler sur un cas similaire ,ça serait très gentil et généreux de m'aider ...MERCI D'AVANCE.

[FLB]

Salut,
j'ai essayé de lire ton code mais il est dur d'accès : tu multiplies les variables alors qu'un filtre de Kalman ne devrait pas être si complexe et fouillis.
Pour te répondre : un filtre de Kalman ne devrait pas diverger si tu l'initialises à côté de l'état initial, heureusement d'ailleurs. Dans le doute initialise la matrice de covariance (P) à une grande valeur pour exprimer ton incertitude sur l'état initial. De même un filtre de Kalman ne diverge pas tant qu'il a des mesures et que le modèle d'erreur de mesure correspond à la vraie erreur de mesure.
Je te conseille de reprendre les équations, les poser sur papier et ne pas multiplier les variables.
De plus je te conseille de clairement séparer la génération de ta trajectoire et de tes mesures de la partie estimation. Le mieux pour ne pas te tromper est de générer une trajectoire et des mesures, les enregistrer, et ensuite travailler comme si tu récupérais des données de vol.

[A_hind]

bonjour
Merci pour votre réponse
je pense que l'erreur est dans la modélisation de la matrice de covariance du bruit de mesure que je prends (et que j'ai trouvé dans la littérature ainsi) c'est la diagonale des deux variances (variance sur la distance- variance sur l'angle)

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
 
Sz=[measnoise1^2 0;0 measnoise2^2 ]

apparemment ça valeur est trop grande du coup quand il calcul le gain de kalman

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
 
% Compute the covariance of the Innovation.
s = h * P * h' + Sz;
% Form the Kalman Gain matrix.
K = a * P * h' * inv(s);

il trouve le gain K petit du moment que la matirce S est grande (a cause de Sz <=>matrice de covaiance du bruit de mesure)
donc quand il vient a corriger la valeur prédite en utilisant la mesure

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
 
xhat = xhat + K * Inn;%calcul de la valeur estimé a partir de la valeur prédite et des mesures

avec Inn=valeur mesur-valeur predite
vu que K est petit ça revient a dire que la valeurs estimé est pratiquement la valeur predite ,du cout il fait prend jamais en considération la mesure ..
je ne sais pas si j'ai pu expliquer le problème correctement .

[FLB]

Salut,
2 choses sont importantes lorsque tu règles un filtre de Kalman :
-quel est le modèle d'erreur de ta mesure : est ce un bruit blanc? de quelle -amplitude?
-quelle est l'erreur de ta propagation? De même est ce un bruit blanc et de quelle amplitude?
Si tu ne colle pas bien à la réalité tu risques en effet soit
-de trop coller à ta mesure, donc le filtre ne filtre rien
-de trop coller à ton modèle, donc tu n'utilises pas tes mesures

[A_hind]

BONSOIR
merci pour votre réponse je vais essayer de voir de ce coté