IdentifiantMot de passe
Loading...
Mot de passe oublié ?Je m'inscris ! (gratuit)
Navigation

Inscrivez-vous gratuitement
pour pouvoir participer, suivre les réponses en temps réel, voter pour les messages, poser vos propres questions et recevoir la newsletter

MATLAB Discussion :

Filtre de Kalman étendu ne converge pas


Sujet :

MATLAB

  1. #1
    Nouveau Candidat au Club
    Homme Profil pro
    Étudiant
    Inscrit en
    Mars 2014
    Messages
    4
    Détails du profil
    Informations personnelles :
    Sexe : Homme
    Âge : 33
    Localisation : France

    Informations professionnelles :
    Activité : Étudiant

    Informations forums :
    Inscription : Mars 2014
    Messages : 4
    Points : 1
    Points
    1
    Par défaut Filtre de Kalman étendu ne converge pas
    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');

  2. #2
    Nouveau Candidat au Club
    Homme Profil pro
    Étudiant
    Inscrit en
    Mars 2014
    Messages
    4
    Détails du profil
    Informations personnelles :
    Sexe : Homme
    Âge : 33
    Localisation : France

    Informations professionnelles :
    Activité : Étudiant

    Informations forums :
    Inscription : Mars 2014
    Messages : 4
    Points : 1
    Points
    1
    Par défaut
    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?

  3. #3
    Membre confirmé
    Homme Profil pro
    Éternel universitaire
    Inscrit en
    Avril 2012
    Messages
    421
    Détails du profil
    Informations personnelles :
    Sexe : Homme
    Localisation : Canada

    Informations professionnelles :
    Activité : Éternel universitaire

    Informations forums :
    Inscription : Avril 2012
    Messages : 421
    Points : 639
    Points
    639
    Par défaut
    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.

  4. #4
    Nouveau Candidat au Club
    Homme Profil pro
    Étudiant
    Inscrit en
    Mars 2014
    Messages
    4
    Détails du profil
    Informations personnelles :
    Sexe : Homme
    Âge : 33
    Localisation : France

    Informations professionnelles :
    Activité : Étudiant

    Informations forums :
    Inscription : Mars 2014
    Messages : 4
    Points : 1
    Points
    1
    Par défaut
    Citation Envoyé par thecrazydonut Voir le message
    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?

  5. #5
    Membre éprouvé
    Inscrit en
    Août 2010
    Messages
    1 124
    Détails du profil
    Informations forums :
    Inscription : Août 2010
    Messages : 1 124
    Points : 1 277
    Points
    1 277
    Par défaut
    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.

  6. #6
    Nouveau Candidat au Club
    Homme Profil pro
    Étudiant
    Inscrit en
    Mars 2014
    Messages
    4
    Détails du profil
    Informations personnelles :
    Sexe : Homme
    Âge : 33
    Localisation : France

    Informations professionnelles :
    Activité : Étudiant

    Informations forums :
    Inscription : Mars 2014
    Messages : 4
    Points : 1
    Points
    1
    Par défaut
    Citation Envoyé par VV33D Voir le message
    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?

  7. #7
    Membre éprouvé
    Inscrit en
    Août 2010
    Messages
    1 124
    Détails du profil
    Informations forums :
    Inscription : Août 2010
    Messages : 1 124
    Points : 1 277
    Points
    1 277
    Par défaut
    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.

Discussions similaires

  1. Filtre de Kalman étendu
    Par souna87 dans le forum Signal
    Réponses: 0
    Dernier message: 12/05/2015, 16h51
  2. Filtre de Kalman ne converge pas
    Par Épanouissement dans le forum Signal
    Réponses: 1
    Dernier message: 20/12/2014, 03h15
  3. filtre de kalman étendu
    Par Épanouissement dans le forum MATLAB
    Réponses: 0
    Dernier message: 12/12/2014, 15h33
  4. Filtre de Kalman étendu: calcul de la jacobienne
    Par amanichennoufi dans le forum Simulink
    Réponses: 5
    Dernier message: 15/02/2012, 19h11
  5. Filtre de Kalman (exemple qui ne marche pas)
    Par elraton dans le forum OpenCV
    Réponses: 2
    Dernier message: 21/05/2010, 13h22

Partager

Partager
  • Envoyer la discussion sur Viadeo
  • Envoyer la discussion sur Twitter
  • Envoyer la discussion sur Google
  • Envoyer la discussion sur Facebook
  • Envoyer la discussion sur Digg
  • Envoyer la discussion sur Delicious
  • Envoyer la discussion sur MySpace
  • Envoyer la discussion sur Yahoo