Langkah Pembaruan EKF-SLAM, Kalman Gain menjadi singular

Saya menggunakan EKF untuk SLAM dan saya mengalami beberapa masalah dengan langkah pembaruan. Saya mendapat peringatan bahwa K adalah singular, yang rconddievaluasi near eps or NaN. Saya pikir saya telah melacak masalah ke inversi Z. Apakah ada cara untuk menghitung Keuntungan Kalman tanpa membalikkan istilah terakhir?

Saya tidak 100% positif ini adalah penyebab masalah, jadi saya juga meletakkan seluruh kode saya di sini . Titik masuk utama adalah 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

Suntingan: project(x(r), x(lmk))seharusnya project(x(r), x(lmk_idx))dan sekarang diperbaiki di atas.

K menjadi tunggal setelah beberapa saat, tetapi tidak segera. Saya pikir sekitar 20 detik atau lebih. Saya akan mencoba perubahan yang disarankan saat saya pulang malam ini dan memposting hasilnya.

Pembaruan 1:

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

K menjadi tunggal setelah 4,82 detik, dengan pengukuran pada 50Hz (241 langkah). Mengikuti saran di sini , saya mencoba K = (P(:, rl) * E_rl')/Zyang menghasilkan 250 langkah sebelum peringatan tentang K menjadi tunggal diproduksi.

Ini memberitahu saya masalahnya bukan dengan inversi matriks, tetapi ada di tempat lain yang menyebabkan masalah.

Perbarui 2

Loop utama saya adalah (dengan objek robot untuk menyimpan pointer x, P dan 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

Pada akhir dari loop ini, saya menyimpan x dan P kembali ke robot, jadi saya percaya saya menyebarkan kovarian melalui setiap iterasi.

Lebih banyak hasil edit Nilai eigen (semoga) yang benar sekarang ada di sini . Ada sejumlah nilai eigen yang negatif. Meskipun besarnya tidak pernah sangat besar, paling banyak , itu terjadi pada iterasi segera setelah landmark pertama diamati dan ditambahkan ke peta (di bagian "landmark baru" dari loop utama).$10^{-2}$

Saya baru saja melihat posting Anda sekarang dan mungkin sudah terlambat untuk benar-benar membantu Anda ... tetapi jika Anda masih tertarik dengan ini: Saya pikir saya telah mengidentifikasi masalah Anda.

Anda menulis matriks kovarians inovasi dengan cara berikut

E = jacobian measure * P * jacobian measure

Mungkin secara teori baik-baik saja tetapi yang terjadi adalah jika algoritma Anda efektif dan terutama jika Anda mengerjakan simulasi: ketidakpastian akan berkurang, terutama dalam arah pengukuran Anda. Jadi Eakan cenderung [[0,0][0,0]].

Untuk menghindari masalah ini, apa yang dapat Anda lakukan adalah menambahkan noise pengukuran yang sesuai dengan ketidakpastian dalam pengukuran dan kovarians inovasi Anda menjadi

E= Jac*P*Jac'+R

di mana Rkovarians dari noise pengukuran (matriks diagonal di mana istilah diagonal adalah kuadrat dari standar deviasi dari noise). Jika Anda sebenarnya tidak ingin mempertimbangkan kebisingan, Anda dapat membuatnya sekecil yang Anda inginkan.

Saya juga menambahkan bahwa pembaruan kovarians Anda terasa aneh bagi saya rumus klasiknya adalah:

P=P - K * jacobian measure * P

Saya tidak pernah melihat formula Anda ditulis di tempat lain, saya mungkin benar tetapi jika Anda tidak yakin Anda harus memeriksanya.

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

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

yang saya pikir seharusnya

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

(lihat: pembagian matriks ). Periksa ukuran K.

Juga: Harap berikan sedikit informasi lebih lanjut: Apakah Klangsung tunggal atau hanya setelah beberapa waktu?

Ini membuat saya khawatir: project(x(r), x(lmk));karena lmktidak didefinisikan.