Langkah Pembaruan EKF-SLAM, Kalman Gain menjadi singular


16

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


1
Apakah Anda menyebarkan ketidakpastian? Apakah nilai eigen kovarians Anda berubah menjadi kecil atau besar?
Josh Vander Hook

1
Apa yang Anda masukkan ke pastebin adalah vektor eigen, bukan nilai eigen. lakukan ini: [v, d] = eig (P). disp (diag (d)). atau hanya disp (eig (P)). Kemudian, Anda dapat memeriksa kondisi yang diperlukan berikut ini: Apakah semua nilai eigen> 0 pada setiap langkah (harus ada dalam praktiknya). Apakah mereka meningkat setelah propagasi dan berkurang setelah pengukuran / koreksi? Dalam pengalaman saya, ini biasanya masalahnya.
Josh Vander Hook

2
Ada yang salah jika nilai eigen Anda negatif. Ketika Anda menginisialisasi tengara, apa ketidakpastian yang terkait dengan posisi estimasi pertama itu?
Josh Vander Hook

Pengamatan adalah sepasang. Ketika tengara pertama diinisialisasi, ia memiliki kovarians [5,8938, 3,0941; 3.0941, 2.9562]. Untuk yang kedua, kovariansnya adalah [22,6630 -14,3822; -14.3822, 10.5484] Matriks lengkapnya ada di sini
munk

Jawaban:


5

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.


Ah, trik "asin kovarian" lama.
Josh Vander Hook

1

KP(Nr+Nl)×(Nr+Nl)NrNl

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.

Dengan menggunakan situs kami, Anda mengakui telah membaca dan memahami Kebijakan Cookie dan Kebijakan Privasi kami.
Licensed under cc by-sa 3.0 with attribution required.