Filtre de Kalman étendu ne converge pas

**totomania19** · 31/03/2014, 16h31

Bonjour à tous,

Je souhaite réalise un EKF et après l'avoir implémenter ce dernier ne converge pas... Je souhaite suivre la trajectoire d'un mobile, et j'ai donc placé deux capteurs défini par E.

Est-ce que quelq'un aurait une idée d'ou peut provenir le problème??

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
 
 
% KALMAN simulation file
close all;
clear all;
 
%%%%%%%%%%%%%%%%%%%%%%%%
% constant declaration %
%%%%%%%%%%%%%%%%%%%%%%%%
c=299792458;  % speed of light in m/s
delta_t=0.0001; % trajectory sampling step
t=0:delta_t:1; % time vector declaration
n=length(t); % total number of time samples
Vx=200; % velocity of the mobile along the x axis in m/s (line traj.)
Vy=20; % velocity of the mobile along the y axis in m/s (line traj.)
Ax=1; % acceleration of the mobile along the x axis in m/s (line traj.)
Ay=1; % acceleration of the mobile along the x axis in m/s (line traj.)
X0=0;
Y0=0;
f=15*10^9;% fréquence
c=3*10^8;%celerité 
lambda=c/f;%longueur d'onde% matrix containing the transmitters position
E=[0 20000;
    0 -20000];
sigma=[10;10]; % mesure noise std for each transmitter
N=length(E); % number of transmitters
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generation of the true trajectory %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n
    x(i)=X0+Vx*t(i)+1/2*Ax*t(i)^2; % x-coordinate of the mobile in meters
    y(i)=Y0+Vy*t(i)+1/2*Ay*t(i)^2; % y-coordinate of the mobile in meters
    %         x(i)=Rx*cos(2*pi*Fx*t(i)); % x-coordinate of the mobile in meters
    %         y(i)=Ry*sin(2*pi*Fy*t(i)); % y-coordinate of the mobile in meters
 
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generation of the measurements %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n
    % current time = t(i)
    for j=1:N
        % current transmitter = transmitter j
        distance_true = sqrt((E(j,1)-x(i))^2+(E(j,2)-y(i))^2);
        noise = sigma(j)*randn(1,1);
        Y(i,j) = distance_true + noise;
    end;
end;
 
 
 
 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% determination of the position + clock bias through EKF %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
R=diag(sigma.^2);
sig_ax = 0.05; % std velocity along x
sig_ay = 0.05; % std velocity along y
 
Q= zeros(6,6);
Q(1,1) = sig_ax^2*delta_t; Q(4,4) = sig_ay^2*delta_t; 
F=[1 delta_t 0 0 0 0 ; 0 1 delta_t 0 0 0 ; 0 0 1 0 0 0 ; ...
    0 0 0 1 delta_t 0 ; 0 0 0 0 1 delta_t ; 0 0 0 0 0 1 ]; % state transition matrix
S=diag([1e2 1e2 1e2 1e2 1e2 1e2]); % initialisation of the mat cov error state estimation
X_kf=zeros(n,6); % declaration of variable to contain successive estimations
DY = zeros(n,length(sigma));
X_kf(1,:)=[0 0 0 0 0 0]; % initialisation of state estimation
for i=1:n
    % 1st KF equation: prediction a priori of the state
    if i==1
        X_hat=F*X_kf(1,:)';
    else
        X_hat=F*X_kf(i-1,:)';
    end;
    % 2nd KF equation: mat cov error estimation a priori
    S=F*S*F'+Q;
    % calculation of measurement prediction & derivative observation matrix at point X_hat
    H = zeros(N,6);
    for j=1:N
        distance_pred(j)=sqrt((E(j,1)-X_hat(1))^2+(E(j,2)-X_hat(4))^2);
        mes_pred(j) = distance_pred(j);
       H(j,1)=(X_hat(1)-E(j,1))/distance_pred(j);
        H(j,4)=(X_hat(4)-E(j,2))/distance_pred(j);
 
    end;
   %Kalman
    K=S*H'*inv(H*S*H'+R);
 
   DY(i,:)=Y(i,:)-mes_pred;
 
    X_kf(i,:)=(X_hat+K*DY(i,:)')';
 
    S=S-K*H*S;
 
 
    X_hat=X_kf(i,:)';
end;
 
 
 
 
     % EKF only
        figure(1); hold on; grid on;
        plot(x, y, '*-k');
        plot(X_kf(:,1), X_kf(:,4),'o-g');
        xlabel('X (m)'); ylabel('Y (m)'); title('Comparison True / Estimated Trajectories');
        legend('True','KALMAN');

**totomania19** · 31/03/2014, 23h11

J'ai fait quelque test dans mon code. Notamment au niveau de l'emplacement des capteurs. Mon filtre converge un peu plus cependant j'aimerais savoir s'il est possible de se passer de ces capteurs... Personne pour répondre?

**thecrazydonut** · 02/04/2014, 01h32

Fais un plot de

Code :

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

1
2
 
plot(x, X_kf(:,4),'o-g');

Il semblerait que ça converge bien (j'ai pas trop le temps de regarder le code en détail et malheureusement je connais seulement KF et EnKF et pas EKF).
Donc a priori, ce serait ton X_kf(:,1) qui pose problème.

**totomania19** · 02/04/2014, 10h12

Envoyé par thecrazydonut

Fais un plot de

Code :

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

1
2
 
plot(x, X_kf(:,4),'o-g');

Il semblerait que ça converge bien (j'ai pas trop le temps de regarder le code en détail et malheureusement je connais seulement KF et EnKF et pas EKF).
Donc a priori, ce serait ton X_kf(:,1) qui pose problème.

Merci pour votre aide... je précise un peu mon modèle: Mon vecteur d'état est [x,vx,ax,y,vy,ay,z,vz,az]. Je viens de remarquer que l'erreur vient peut être du fait que mon sig_ay (écart type suivant l'axe y) est très faible par rapport aux autres.

Je précise que j'ai des écarts types en azimut, distance et élévation. J'ai voulu tout mettre en coordonnées cartésiennes, j'ai donc transformé mes écarts types en cartésien en utilisant "bêtement" les changements de repères.

Est-ce possible de faire cela?

**shaiHulud** · 02/04/2014, 15h28

Bonjour,

J'ai déjà noté de mauvais comportement d'un Kalman pour des ecart-types trop faibles. Dans mon cas, il m'a suffit de centrer réduire la série avant d'appliquer le filtre.

**totomania19** · 02/04/2014, 15h34

Envoyé par VV33D

Bonjour,

J'ai déjà noté de mauvais comportement d'un Kalman pour des ecart-types trop faibles. Dans mon cas, il m'a suffit de centrer réduire la série avant d'appliquer le filtre.

Vous voulez dire quoi par centrer et réduire la serie?

**shaiHulud** · 02/04/2014, 16h00

Je parle d'une série réelle X_t modélisée par un SWARCH (la variance conditionelle dépend du passé de la série ET d'une chaine de markov caché- l'espace d'état est discret) sur des séries financières.

centrer et réduire = retrancher la moyenne empirique et diviser par l'écart type empirique.

Filtre de Kalman étendu ne converge pas

MATLAB

Discussions similaires

Partager

Partager