EKF-SLAM Krok aktualizacji, Kalman Gain staje się osobliwy

Używam EKF dla SLAM i mam problem z krokiem aktualizacji. Dostaję ostrzeżenie, że K jest liczbą pojedynczą, rcondocenia near eps or NaN. Myślę, że prześledziłem problem do inwersji Z. Czy istnieje sposób na obliczenie wzmocnienia Kalmana bez odwracania ostatniego terminu?

Nie jestem w 100% przekonany, że to jest przyczyną problemu, więc umieściłem tutaj cały mój kod . Głównym punktem wejścia jest 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

Edycje: project(x(r), x(lmk))powinny były project(x(r), x(lmk_idx))i zostały poprawione powyżej.

Po pewnym czasie K staje się osobliwy, ale nie od razu. Myślę, że to około 20 sekund. Spróbuję zmian zaproponowanych przez @josh, kiedy wrócę dziś wieczorem do domu i opublikuję wyniki.

Aktualizacja 1:

Moja symulacja najpierw obserwuje 2 punkty orientacyjne, więc K wynosi . daje w wyniku macierz , więc nie można jej dodać do x w następnym wierszu. $7\ x\ 2$(P(rl,rl) * E_rl') * inv( Z )$5\ x\ 2$

K staje się liczbą pojedynczą po 4,82 sekundach, z pomiarami przy 50 Hz (241 kroków). Postępując zgodnie z tą radą , próbowałem K = (P(:, rl) * E_rl')/Zuzyskać 250 kroków, zanim pojawi się ostrzeżenie o liczbie pojedynczej K.

To mówi mi, że problem nie dotyczy inwersji macierzy, ale problem jest spowodowany gdzieś indziej.

Aktualizacja 2

Moja główna pętla to (z obiektem robota do przechowywania wskaźników x, P i punktów orientacyjnych):

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

Na końcu tej pętli zapisuję x i P z powrotem do robota, więc wierzę, że propaguję kowariancję przez każdą iterację.

Więcej edycji (miejmy nadzieję) prawidłowe wartości własne są teraz tutaj . Istnieje wiele wartości własnych, które są ujemne. Chociaż ich wielkość nigdy nie jest bardzo duża, najwyżej , dzieje się to podczas iteracji natychmiast po zaobserwowaniu pierwszego punktu orientacyjnego i dodaniu go do mapy (w sekcji „nowych punktów orientacyjnych” głównej pętli).$10^{-2}$

Właśnie widzę teraz twój post i być może jest za późno, aby naprawdę ci pomóc ... ale w przypadku, gdy nadal jesteś zainteresowany: myślę, że zidentyfikowałem twój problem.

Macierz kowariancji innowacji pisze się w następujący sposób

E = jacobian measure * P * jacobian measure

Teoretycznie może być w porządku, ale dzieje się tak, że jeśli twój algorytm jest skuteczny, a zwłaszcza jeśli pracujesz nad symulacją: niepewności zmniejszą się, szczególnie w kierunkach pomiaru. Tak Ebędzie [[0,0][0,0]].

Aby uniknąć tego problemu, możesz dodać szum pomiarowy odpowiadający niepewnościom pomiaru, a kowariancja innowacyjności staje się

E= Jac*P*Jac'+R

gdzie Rjest kowariancja szumu pomiarowego (macierz diagonalna, gdzie terminy na przekątnej są kwadratami odchylenia standardowego hałasu). Jeśli tak naprawdę nie chcesz brać pod uwagę hałasu, możesz zmniejszyć go tak, jak chcesz.

Dodam również, że wasza aktualizacja kowariancji wydaje mi się dziwna, klasyczna formuła jest następująca:

P=P - K * jacobian measure * P

Nigdy nie widziałem twojej formuły napisanej nigdzie indziej, być może mam rację, ale jeśli nie jesteś tego pewien, powinieneś to sprawdzić.

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

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

które moim zdaniem powinno być

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

(patrz: podział macierzy ). Sprawdź rozmiar K.

Ponadto: Proszę podać trochę więcej informacji: Czy Kliczba pojedyncza staje się natychmiastowa, czy dopiero po pewnym czasie?

Martwi mnie to: project(x(r), x(lmk));ponieważ lmknie jest zdefiniowane.