Étape de mise à jour EKF-SLAM, Kalman Gain devient singulier

16

J'utilise un EKF pour SLAM et j'ai un problème avec l'étape de mise à jour. J'obtiens un avertissement que K est singulier, rcondévalue à near eps or NaN. Je pense que j'ai tracé le problème à l'inversion de Z. Existe-t-il un moyen de calculer le gain de Kalman sans inverser le dernier terme?

Je ne suis pas sûr à 100% que c'est la cause du problème, j'ai donc également mis tout mon code ici . Le point d'entrée principal est slam2d.

function [ x, P ] = expectation( x, P, lmk_idx, observation)
    % expectation
    r_idx = [1;2;3];
    rl = [r_idx; lmk_idx];

    [e, E_r, E_l] = project(x(r), x(lmk_idx)); 
    E_rl = [E_r E_l];
    E = E_rl * P(rl,rl) * E_rl';

    % innovation
    z = observation - e;
    Z = E;

    % Kalman gain
    K = P(:, rl) * E_rl' * Z^-1;

    % update
    x = x + K * z;
    P = P - K * Z * K';
end


function [y, Y_r, Y_p] = project(r, p)     
    [p_r, PR_r, PR_p] = toFrame2D(r, p);
    [y, Y_pr]   = scan(p_r);
    Y_r = Y_pr * PR_r;
    Y_p = Y_pr * PR_p;    
end


function [p_r, PR_r, PR_p] = toFrame2D(r , p)
    t = r(1:2);
    a = r(3);
    R = [cos(a) -sin(a) ; sin(a) cos(a)];
    p_r = R' * (p - t);
    px = p(1);
    py = p(2);
    x = t(1);
    y = t(2);
    PR_r = [...
        [ -cos(a), -sin(a),   cos(a)*(py - y) - sin(a)*(px - x)]
        [  sin(a), -cos(a), - cos(a)*(px - x) - sin(a)*(py - y)]];
    PR_p = R';    
end


function [y, Y_x] = scan(x)
    px = x(1);
    py = x(2);
    d = sqrt(px^2 + py^2);
    a = atan2(py, px);
    y = [d;a];
    Y_x =[...
    [     px/(px^2 + py^2)^(1/2), py/(px^2 + py^2)^(1/2)]
    [ -py/(px^2*(py^2/px^2 + 1)), 1/(px*(py^2/px^2 + 1))]];
end

Modifications: project(x(r), x(lmk))aurait dû être project(x(r), x(lmk_idx))et est maintenant corrigé ci-dessus.

K devient singulier après un petit moment, mais pas immédiatement. Je pense que c'est environ 20 secondes environ. J'essaierai les changements suggérés par @josh quand je rentrerai ce soir et publierai les résultats.

Mise à jour 1:

sept X 2(P(rl,rl) * E_rl') * inv( Z )5 X 2

K devient singulier après 4,82 secondes, avec des mesures à 50 Hz (241 pas). En suivant les conseils ici , j'ai essayé K = (P(:, rl) * E_rl')/Zce qui se traduit par 250 étapes avant qu'un avertissement concernant K étant singulier ne soit produit.

Cela me dit que le problème n'est pas avec l'inversion de matrice, mais c'est ailleurs qui cause le problème.

Update 2

Ma boucle principale est (avec un objet robot pour stocker des pointeurs x, P et des points de repère):

for t = 0:sample_time:max_time
    P = robot.P;
    x = robot.x;
    lmks = robot.lmks;
    mapspace = robot.mapspace;

    u = robot.control(t);
    m = robot.measure(t);

    % Added to show eigenvalues at each step
    [val, vec] = eig(P);
    disp('***')
    disp(val)

    %%% Motion/Prediction
    [x, P] = predict(x, P, u, dt);

    %%% Correction
    lids = intersect(m(1,:), lmks(1,:));  % find all observed landmarks
    lids_new = setdiff(m(1,:), lmks(1,:));
    for lid = lids
        % expectation
        idx = find (lmks(1,:) == lid, 1);
        lmk = lmks(2:3, idx);
        mid = m(1,:) == lid;
        yi = m(2:3, mid);

        [x, P] = expectation(x, P, lmk, yi);
    end  %end correction

    %%% New Landmarks

    for id = 1:length(lids_new)
    % if id ~= 0
        lid = lids_new(id);
        lmk = find(lmks(1,:)==false, 1);
        s = find(mapspace, 2);
        if ~isempty(s)
            mapspace(s) = 0;
            lmks(:,lmk) = [lid; s'];
            yi = m(2:3,m(1,:) == lid);

            [x(s), L_r, L_y] = backProject(x(r), yi);

            P(s,:) = L_r * P(r,:);
            P(:,s) = [P(s,:)'; eye(2)];
            P(s,s) = L_r * P(r,r) * L_r';
        end
    end  % end new landmarks

    %%% Save State
    robot.save_state(x, P, mapspace, lmks)
    end  
end

À la fin de cette boucle, je sauvegarde x et P dans le robot, donc je crois que je propage la covariance à chaque itération.

dix-2

munk
la source
1
Propagez-vous l'incertitude? Les valeurs propres de votre covariance deviennent-elles arbitrairement petites ou grandes?
Josh Vander Hook
1
Ce que vous mettez dans pastebin, ce sont les vecteurs propres, pas les valeurs propres. faites ceci: [v, d] = eig (P). disp (diag (d)). ou tout simplement disp (eig (P)). Ensuite, vous pouvez vérifier les conditions nécessaires suivantes: Toutes les valeurs propres sont-elles> 0 à chaque étape (elles devraient être en pratique). Augmentent- ils après propagation et diminuent-ils après mesures / corrections? D'après mon expérience, c'est généralement le problème.
Josh Vander Hook
2
Quelque chose ne va pas si vos valeurs propres sont négatives. Lorsque vous initialisez un point de repère, quelle est l'incertitude associée à sa première position estimée?
Josh Vander Hook
L'observation est une paire. Lorsque le premier repère est initialisé, il a une covariance de [5,8938, 3,0941; 3.0941, 2.9562]. Pour le second, sa covariance est [22,6630 -14,3822; -14.3822, 10.5484] La matrice complète est ici
munk

Réponses:

5

Je viens de voir votre message maintenant et il est peut-être trop tard pour vraiment vous aider ... mais au cas où cela vous intéresserait toujours: je pense avoir identifié votre problème.

Vous écrivez la matrice de covariance de l'innovation de la manière suivante

E = jacobian measure * P * jacobian measure

Cela peut être correct en théorie mais ce qui se passe c'est que si votre algorithme est efficace et surtout si vous travaillez sur une simulation: les incertitudes vont diminuer, surtout dans les directions de votre mesure. Ainsi Eaura tendance à [[0,0][0,0]].

Pour éviter ce problème, vous pouvez ajouter un bruit de mesure correspondant aux incertitudes de la mesure et votre covariance d'innovation devient

E= Jac*P*Jac'+R

Rest la covariance du bruit de mesure (matrice diagonale où les termes en diagonale sont les carrés de l'écart-type du bruit). Si vous ne voulez pas vraiment tenir compte du bruit, vous pouvez le rendre aussi petit que vous le souhaitez.

J'ajoute également que votre mise à jour de covariance me semble étrange la formule classique est:

P=P - K * jacobian measure * P

Je n'ai jamais vu votre formule écrite ailleurs, je pourrais avoir raison, mais si vous n'en êtes pas sûr, vous devriez la vérifier.

Jaeger0
la source
Ah, le vieux truc "salez la covariance".
Josh Vander Hook
1

KP(Nr+Nl)×(Nr+Nl)NrNl

K = P(:, rl) * E_rl' * Z^-1

qui je pense devrait être

(P(rl,rl) * E_rl') * inv(Z).

(voir: division matricielle ). Vérifiez la taille de K.

Aussi: Veuillez fournir un peu plus d'informations: devient K-il singulier immédiatement ou seulement après un certain temps?

Cela m'inquiète: project(x(r), x(lmk));puisque lmkn'est pas défini.

Josh Vander Hook
la source