EKF-SLAM 업데이트 단계, Kalman Gain이 단 수화

SLAM에 EKF를 사용하고 있으며 업데이트 단계에 문제가 있습니다. K가 단수이고로 rcond평가 된다는 경고가 표시 됩니다 near eps or NaN. Z의 반전에 대한 문제를 추적했다고 생각합니다. 마지막 항을 뒤집지 않고 칼만 게인을 계산하는 방법이 있습니까?

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))이제 위의 수정됩니다.

K는 잠시 후 단수로 진행되지만 즉시는 아닙니다. 나는 약 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')/ZK가 단수라는 경고가 생성되기 전에 250 단계의 결과를 시도 했습니다.

이것은 문제가 매트릭스 반전과 관련이 없지만 문제를 일으키는 다른 곳이라고 알려줍니다.

업데이트 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

나는 당신의 공식이 다른 곳에서 작성된 것을 보지 못했습니다. 정확 할 수도 있지만 확실하지 않은 경우 확인해야합니다.

당신은 단지 로봇과 랜드 마크 (전형적인이다)와 관련된 공분산 서브 매트릭스를 업데이트하는 경우, 다음 K과 P해야$(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정의되지 않았기 때문 입니다.