EKF-SLAM Güncelleme adımı, Kalman Gain tekil oldu

SLAM için bir EKF kullanıyorum ve güncelleme adımıyla ilgili bir sorun yaşıyorum. K'nin tekil olduğunu rconddeğerlendiren bir uyarı alıyorum near eps or NaN. Sanırım problemi Z'nin tersine çevirdim. Kalman Kazançını son terimi tersine çevirmeden hesaplamanın bir yolu var mı?

Sorunun nedeni bu% 100 pozitif değilim, bu yüzden de tüm kodumu buraya koydum . Ana giriş noktası slam2d'dir.

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

Düzenlemeler: Yukarıda düzeltilmiş project(x(r), x(lmk))olmalı project(x(r), x(lmk_idx))ve şimdi düzeltilmiştir.

K bir süre sonra tekil olur, ama hemen değil. Sanırım yaklaşık 20 saniye. Bu gece eve geldiğimde ve sonuçları gönderdiğimde @josh'un önerdiği değişiklikleri deneyeceğim.

Güncelleme 1:

Simülasyonum önce 2 yer işaretini gözlemlediğinden K . sonuçta 5 x 2 matris bulunur, bu nedenle sonraki satıra x eklenemez. $7\ x\ 2$(P(rl,rl) * E_rl') * inv( Z )$5\ x\ 2$

K 4,82 saniye sonra tekil hale gelir ve 50Hz'de ölçümler (241 adım). Buradaki tavsiyeyi takiben, K = (P(:, rl) * E_rl')/ZK'nin tekil olduğu hakkında bir uyarı yapılmadan önce hangi sonuçları 250 adımda denedim .

Bu bana sorunun matris inversiyonu ile olmadığını söylüyor, ama soruna neden olan başka bir yerde.

Güncelleme 2

Benim ana döngü (x, P ve yer işareti işaretlerini saklamak için bir robot nesnesiyle):

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

Bu döngünün sonunda, x ve P'yi robota geri kaydederim, bu yüzden her yinelemede kovaryans yaydığımı düşünüyorum.

$10^{-2}$

Gönderinizi şimdi görüyorum ve size gerçekten yardımcı olmak için belki çok geç ... ama hala bununla ilgileniyorsanız: Sorununuzu belirlediğimi düşünüyorum.

İnovasyon kovaryans matrisini aşağıdaki şekilde yazıyorsunuz

E = jacobian measure * P * jacobian measure

Teoride iyi olabilir, ancak algoritmanız etkili ve özellikle bir simülasyon üzerinde çalışıyorsanız: belirsizlikler, özellikle ölçümünüzün yönlerinde azalacaktır. Böylece Eeğilimi olacaktır [[0,0][0,0]].

Bu problemden kaçınmak için yapabileceğiniz şey, ölçümdeki belirsizliklere karşılık gelen bir ölçüm gürültüsü eklemektir ve inovasyon kovaryansınız

E= Jac*P*Jac'+R

burada R(diyagonal terimler gürültü standart sapma kareler diyagonal matris) ölçüm gürültüsünün kovaryans olup. Aslında gürültüyü düşünmek istemiyorsanız, istediğiniz kadar küçük yapabilirsiniz.

Kovaryans güncellemenizin bana klasik formülün garip geldiğini de ekliyorum:

P=P - K * jacobian measure * P

Formülünüzü başka hiçbir yerde yazılı görmedim, doğru olabilirim ama emin değilseniz kontrol etmelisiniz.

Yalnızca robot ve yer işaretiyle ilişkilendirilmiş kovaryans alt matrisini (tipik olarak olduğu gibi) güncelliyorsanız, KP$(N_r + N_l)\times(N_r + N_l)$$N_r$$N_l$

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

bence olmalı

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

(bkz: matris bölümü ). Boyutunu kontrol edin K.

Ayrıca: Lütfen biraz daha bilgi verin: KHemen mı yoksa sadece bir süre sonra tekil mi?

Bu beni endişelendiriyor: project(x(r), x(lmk));çünkü lmktanımlanmadı.