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

16

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)

Munka
źródło
1
Czy propagujesz niepewność? Czy wartości własne kowariancji stają się arbitralnie małe czy duże?
Josh Vander Hook
1
To, co wkładasz do pastebin, to wektory własne, a nie wartości własne. zrób to: [v, d] = eig (P). disp (diag (d)). lub po prostu disp (eig (P)). Następnie możesz sprawdzić następujące niezbędne warunki: Czy wszystkie wartości własne> 0 na każdym kroku (powinny być w praktyce). Czy zwiększają się po propagacji i zmniejszają po pomiarach / korektach? Z mojego doświadczenia wynika, że ​​zwykle jest to problem.
Josh Vander Hook
2
Coś jest nie tak, jeśli twoje wartości własne są ujemne. Kiedy inicjujesz punkt orientacyjny, jaka jest niepewność związana z jego pierwszą szacunkową pozycją?
Josh Vander Hook
Obserwacja jest parą. Po zainicjowaniu pierwszego punktu orientacyjnego ma kowariancję [5.8938, 3.0941; 3.0941, 2.9562]. Po drugie, kowariancja wynosi [22.6630 -14,3822; -14.3822, 10.5484] Pełna macierz jest tutaj
munk

Odpowiedzi:

5

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ć.

Jaeger0
źródło
Ach, stara sztuczka „solić kowariancję”.
Josh Vander Hook
1

KP(N.r+N.l)×(N.r+N.l)N.rN.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.

Josh Vander Hook
źródło