Passaggio di aggiornamento EKF-SLAM, Kalman Gain diventa singolare

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 sono$10^{-2}$

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.

KP$(N_r + N_l)\times(N_r + N_l)$$N_r$$N_l$

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.