EKF-SLAM Update Schritt, Kalman Gain wird singulär

Ich verwende ein EKF für SLAM und habe ein Problem mit dem Aktualisierungsschritt. Ich bekomme eine Warnung, dass K Singular ist, rcondbewertet zu near eps or NaN. Ich glaube, ich habe das Problem auf die Inversion von Z zurückgeführt. Gibt es eine Möglichkeit, den Kalman-Gewinn zu berechnen, ohne den letzten Term zu invertieren?

Ich bin nicht zu 100% sicher, dass dies die Ursache des Problems ist, daher habe ich auch meinen gesamten Code hier abgelegt . Der Haupteingangspunkt ist 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

Änderungen: project(x(r), x(lmk))sollten project(x(r), x(lmk_idx))und werden jetzt oben korrigiert.

K wird nach einer Weile singulär, aber nicht sofort. Ich glaube, es sind ungefähr 20 Sekunden. Ich werde die von @josh vorgeschlagenen Änderungen ausprobieren, wenn ich heute Abend nach Hause komme und die Ergebnisse veröffentliche.

Update 1:

$7\ x\ 2$(P(rl,rl) * E_rl') * inv( Z )$5\ x\ 2$

K wird nach 4,82 Sekunden mit Messungen bei 50 Hz (241 Schritte) singulär. Nach den hier gegebenen Ratschlägen habe ich versucht, K = (P(:, rl) * E_rl')/Zwas zu 250 Schritten führt, bevor eine Warnung über K als Singular ausgegeben wird.

Dies sagt mir, dass das Problem nicht mit der Matrixinversion zusammenhängt, sondern an einer anderen Stelle, die das Problem verursacht.

Update 2

Meine Hauptschleife ist (mit einem Roboterobjekt zum Speichern von x-, P- und Orientierungspunktzeigern):

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

Am Ende dieser Schleife speichere ich x und P zurück auf den Roboter, also glaube ich, dass ich die Kovarianz durch jede Iteration propagiere.

$10^{-2}$

Ich sehe gerade Ihren Beitrag und es ist vielleicht zu spät, um Ihnen wirklich zu helfen ... aber falls Sie noch daran interessiert sind: Ich glaube, ich habe Ihr Problem identifiziert.

Sie schreiben die Innovationskovarianzmatrix folgendermaßen

E = jacobian measure * P * jacobian measure

Theoretisch mag es in Ordnung sein, aber wenn Ihr Algorithmus effektiv ist und Sie gerade an einer Simulation arbeiten, werden die Unsicherheiten, insbesondere in Richtung Ihrer Messung, geringer. Werde also Edazu neigen [[0,0][0,0]].

Um dieses Problem zu vermeiden, können Sie ein Messrauschen hinzufügen, das den Messunsicherheiten entspricht und Ihre Innovationskovarianz erhöht

E= Jac*P*Jac'+R

wo Rist die Kovarianz des Messrauschens (Diagonalmatrix, wobei die Terme auf der Diagonale die Quadrate der Standardabweichung des Rauschens sind). Wenn Sie Geräusche eigentlich nicht berücksichtigen möchten, können Sie sie so klein machen, wie Sie möchten.

Ich füge hinzu, dass mir Ihr Kovarianz-Update seltsam vorkommt. Die klassische Formel lautet:

P=P - K * jacobian measure * P

Ich habe Ihre Formel nirgendwo anders geschrieben gesehen, vielleicht habe ich recht, aber wenn Sie sich nicht sicher sind, sollten Sie sie überprüfen.

Wenn Sie nur die Kovarianz-Submatrix aktualisieren, die dem Roboter und dem Orientierungspunkt zugeordnet ist (wie es typisch ist), dann KP$(N_r + N_l)\times(N_r + N_l)$$N_r$$N_l$

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

was ich denke sollte sein

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

(siehe: Matrixeinteilung ). Überprüfen Sie die Größe von K.

Auch: Bitte geben Sie ein wenig mehr Informationen: Geht KSingular sofort oder erst nach einiger Zeit?

Das macht mir Sorgen: project(x(r), x(lmk));da lmkist nicht definiert.