Passaggio di aggiornamento EKF-SLAM, Kalman Gain diventa singolare


16

Sto usando un EKF per SLAM e ho qualche problema con il passaggio di aggiornamento. Ricevo un avvertimento che K è singolare, rcondvaluta near eps or NaN. Penso di aver fatto risalire il problema all'inversione di Z. Esiste un modo per calcolare il guadagno di Kalman senza invertire l'ultimo termine?

Non sono sicuro al 100% che questa sia la causa del problema, quindi ho inserito anche qui tutto il mio codice . Il punto di ingresso principale è 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

Modifiche: project(x(r), x(lmk))avrebbe dovuto essere project(x(r), x(lmk_idx))ed è ora corretto sopra.

K diventa singolare dopo un po ', ma non immediatamente. Penso che siano circa 20 secondi circa. Proverò le modifiche suggerite da @josh quando tornerò a casa stasera e pubblicherò i risultati.

Aggiornamento 1:

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

K diventa singolare dopo 4,82 secondi, con misurazioni a 50Hz (241 passi). Seguendo i consigli qui , ho provato K = (P(:, rl) * E_rl')/Zquali risultati in 250 passaggi prima che venisse emesso un avviso sul fatto che K fosse singolare.

Questo mi dice che il problema non è con l'inversione della matrice, ma è altrove che sta causando il problema.

Aggiornamento 2

Il mio loop principale è (con un oggetto robot per memorizzare i puntatori x, P e landmark):

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

Alla fine di questo ciclo, salvo x e P nel robot, quindi credo di propagare la covarianza attraverso ogni iterazione.

Altre modifiche Gli autovalori (si spera) corretti sono10-2


1
Stai propagando l'incertezza? Gli autovalori della tua covarianza diventano arbitrariamente piccoli o grandi?
Josh Vander Hook,

1
Quello che metti in pastebin sono gli autovettori, non gli autovalori. fai questo: [v, d] = eig (P). DISP (diag (d)). o semplicemente disp (eig (P)). Quindi, puoi verificare le seguenti condizioni necessarie: Sono tutti autovalori> 0 ad ogni passaggio (dovrebbero essere in pratica). Hanno aumentano dopo la propagazione e la diminuzione dopo misurazioni / correzioni? Nella mia esperienza, questo è di solito il problema.
Josh Vander Hook,

2
Qualcosa non va se i tuoi autovalori sono negativi. Quando si inizializza un punto di riferimento, qual è l'incertezza associata alla sua prima posizione stimata?
Josh Vander Hook,

L'osservazione è una coppia. Quando il primo punto di riferimento viene inizializzato, ha una covarianza di [5.8938, 3.0941; 3.0941, 2.9562]. Per il secondo, la sua covarianza è [22.6630 -14.3822; -14.3822, 10.5484] La matrice completa è qui
Munk il

Risposte:


5

Adesso vedo solo il tuo post ed è forse troppo tardi per aiutarti davvero ... ma nel caso tu sia ancora interessato a questo: penso di aver identificato il tuo problema.

Scrivi la matrice della covarianza dell'innovazione nel modo seguente

E = jacobian measure * P * jacobian measure

In teoria potrebbe andare bene, ma ciò che accade è che se il tuo algoritmo è efficace e soprattutto se stai lavorando su una simulazione: le incertezze diminuiranno, specialmente nelle direzioni della tua misurazione. CosìE tenderà a [[0,0][0,0]].

Per evitare questo problema, ciò che puoi fare è aggiungere un rumore di misurazione corrispondente alle incertezze nella misurazione e la tua covarianza dell'innovazione diventa

E= Jac*P*Jac'+R

dove R è la covarianza del rumore di misurazione (matrice diagonale in cui i termini sulla diagonale sono i quadrati della deviazione standard del rumore). Se in realtà non vuoi considerare il rumore, puoi renderlo piccolo quanto vuoi.

Aggiungo anche che il tuo aggiornamento sulla covarianza mi sembra strano, la formula classica è:

P=P - K * jacobian measure * P

Non ho mai visto la tua formula scritta altrove, potrei essere corretta, ma se non sei sicuro dovresti controllarla.


Ah, il vecchio trucco "salare la covarianza".
Josh Vander Hook,

1

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

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

che penso dovrebbe essere

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

(vedi: divisione matrice ). Controlla la dimensione diK .

Inoltre: Fornisci qualche informazione in più: diventa Ksingolare immediatamente o solo dopo qualche tempo?

Questo mi preoccupa: project(x(r), x(lmk));poiché lmknon è definito.

Utilizzando il nostro sito, riconosci di aver letto e compreso le nostre Informativa sui cookie e Informativa sulla privacy.
Licensed under cc by-sa 3.0 with attribution required.