ขั้นตอนการอัปเดต EKF-SLAM, Kalman Gain กลายเป็นเอกพจน์

ฉันใช้ EKF สำหรับ SLAM และฉันมีปัญหากับขั้นตอนการอัปเดต ฉันได้รับคำเตือนว่า K เป็นเอกพจน์rcondประเมินnear eps or NaNว่า ฉันคิดว่าฉันได้ติดตามปัญหาของการผกผันของ Z มีวิธีคำนวณ Kalman Gain โดยไม่ย้อนกลับคำสุดท้ายหรือไม่?

ฉันไม่ 100% บวกนี้เป็นสาเหตุของปัญหาที่เกิดขึ้นดังนั้นฉันได้ยังใส่ทั้งหมดของฉันรหัสที่นี่ จุดเข้าหลักคือ 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

การแก้ไข: project(x(r), x(lmk))ควรได้รับproject(x(r), x(lmk_idx))และได้รับการแก้ไขด้านบนแล้ว

เคไปเอกพจน์หลังจากนั้นซักครู่ แต่ไม่ใช่ในทันที ฉันคิดว่าประมาณ 20 วินาทีหรือมากกว่านั้น ฉันจะลองเปลี่ยนแปลง @josh แนะนำเมื่อฉันกลับถึงบ้านคืนนี้และโพสต์ผลลัพธ์

อัปเดต 1:

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

K กลายเป็นเอกเทศหลังจาก 4.82 วินาทีด้วยการวัดที่ 50Hz (241 ขั้นตอน) ทำตามคำแนะนำที่นี่ฉันลองK = (P(:, rl) * E_rl')/Zซึ่งผลใน 250 ขั้นตอนก่อนที่จะเตือนเกี่ยวกับ K กำลังผลิตเอกพจน์

สิ่งนี้บอกฉันว่าปัญหาไม่ได้เกิดจากการผกผันของเมทริกซ์ แต่มีที่อื่นที่ทำให้เกิดปัญหา

อัปเดต 2

ลูปหลักของฉันคือ (ที่มีวัตถุหุ่นยนต์เพื่อเก็บตัวชี้ x, P และจุดสังเกต):

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

ในตอนท้ายของลูปนี้ฉันบันทึก x และ P กลับไปที่หุ่นยนต์ดังนั้นฉันเชื่อว่าฉันเผยแพร่ความแปรปรวนร่วมผ่านการวนซ้ำแต่ละครั้ง

$10^{-2}$

ฉันเพิ่งเห็นโพสต์ของคุณตอนนี้และอาจจะสายเกินไปที่จะช่วยคุณจริงๆ ... แต่ในกรณีที่คุณยังสนใจอยู่: ฉันคิดว่าฉันพบปัญหาของคุณแล้ว

คุณเขียนเมทริกซ์ความแปรปรวนร่วมนวัตกรรมด้วยวิธีต่อไปนี้

E = jacobian measure * P * jacobian measure

มันอาจจะไม่เป็นไรในทางทฤษฎี แต่สิ่งที่เกิดขึ้นคือถ้าอัลกอริทึมของคุณมีประสิทธิภาพและโดยเฉพาะอย่างยิ่งถ้าคุณกำลังทำงานกับแบบจำลอง: ความไม่แน่นอนจะลดลงโดยเฉพาะในทิศทางของการวัดของคุณ ดังนั้นจะมีแนวโน้มที่จะE[[0,0][0,0]]

เพื่อหลีกเลี่ยงปัญหานี้สิ่งที่คุณสามารถทำได้คือการเพิ่มเสียงการวัดที่สอดคล้องกับความไม่แน่นอนในการวัดและความแปรปรวนร่วมนวัตกรรมของคุณจะกลายเป็น

E= Jac*P*Jac'+R

ซึ่งRความแปรปรวนร่วมของเสียงการวัดอยู่ที่ไหน (เมทริกซ์แนวทแยงที่ข้อตกลงในแนวทแยงเป็นกำลังสองของการเบี่ยงเบนมาตรฐานของเสียง) หากคุณไม่ต้องการพิจารณาเสียงคุณสามารถทำให้มันเล็กตามที่คุณต้องการ

ฉันยังเพิ่มว่าการปรับปรุงความแปรปรวนร่วมของคุณดูเหมือนจะแปลกสำหรับฉันสูตรดั้งเดิมคือ:

P=P - K * jacobian measure * P

ฉันไม่เคยเห็นสูตรของคุณเขียนที่อื่นฉันอาจจะถูกต้อง แต่ถ้าคุณไม่แน่ใจคุณควรตรวจสอบ

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

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

ซึ่งฉันคิดว่าควรจะเป็น

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

(ดู: การหารเมทริกซ์ ) ตรวจสอบขนาดของKตรวจสอบขนาดของ

นอกจากนี้: โปรดให้ข้อมูลเพิ่มเติมเล็กน้อย: ใช่ Kเอกพจน์ทันทีหรือหลังจากผ่านไปสักระยะหนึ่ง

สิ่งนี้ทำให้ฉันกังวล: project(x(r), x(lmk));เนื่องจากlmkไม่ได้กำหนดไว้