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


16

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 엑스 2(P(rl,rl) * E_rl') * inv( Z )5 엑스 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를 다시 로봇에 저장하므로 각 반복을 통해 공분산을 전파한다고 생각합니다.

102


1
불확실성을 전파하고 있습니까? 공분산의 고유 값이 임의로 작거나 커 집니까?
Josh Vander Hook

1
pastebin에 넣는 것은 고유 값이 아닌 고유 벡터입니다. 이렇게하십시오 : [v, d] = eig (P). disp (diag (d)). 또는 disp (eig (P)). 그런 다음 다음과 같은 필요한 조건을 확인할 수 있습니다. 모든 단계에서 모든 고유 값이> 0입니다 (실제로 있어야 함). 그들은 마십시오 증가 전파 이후 감소 측정 / 보정 후? 내 경험상 이것은 일반적으로 문제입니다.
Josh Vander Hook

2
고유 값이 음수이면 문제가 있습니다. 랜드 마크를 초기화 할 때 첫 번째 예상 위치와 관련된 불확실성은 무엇입니까?
Josh Vander Hook

관측치는 쌍입니다. 첫 번째 랜드 마크가 초기화되면 [5.8938, 3.0941; 3.0941, 2.9562]. 두 번째로 공분산은 [22.6630 -14.3822; -14.3822는 10.5484] 전체 행렬은 여기
멍크

답변:


5

방금 글을 보았는데 실제로 도움이 되기에는 너무 늦었을 것입니다 ...하지만 여전히 관심이있는 경우 : 문제를 확인한 것 같습니다.

다음과 같은 방법으로 혁신 공분산 행렬을 작성합니다.

E = jacobian measure * P * jacobian measure

이론 상으로는 괜찮을 수도 있지만 알고리즘이 효과적이며 특히 시뮬레이션 작업을하는 경우 불확실성이 특히 측정 방향에서 감소합니다. 따라서 E경향이 [[0,0][0,0]]있습니다.

이 문제를 피하려면 측정의 불확실성에 해당하는 측정 노이즈를 추가하면 혁신 공분산이됩니다.

E= Jac*P*Jac'+R

여기서 R측정 노이즈의 공분산 (대각선의 용어가 노이즈의 표준 편차의 제곱 인 대각선 매트릭스)입니다. 실제로 소음을 고려하지 않으려면 원하는만큼 작게 만들 수 있습니다.

또한 공분산 업데이트가 고전적인 공식이 이상하다고 생각합니다.

P=P - K * jacobian measure * P

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


아, 예전의 "공분산에 소금을 뿌려"
Josh Vander Hook

1

당신은 단지 로봇과 랜드 마크 (전형적인이다)와 관련된 공분산 서브 매트릭스를 업데이트하는 경우, 다음 KP해야(아르 자형+)×(아르 자형+) 로봇 상태 크기 아르 자형 랜드 마크 크기 . 당신은 가지고 있습니다 :

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

내가해야한다고 생각

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

( 행렬 나누기 참조 ). 의 크기를 확인하십시오 K.

또한 : 조금 더 많은 정보를 제공하십시오 : K즉시 또는 한동안 후에 단수하게 되나요?

이것은 나를 걱정합니다 : project(x(r), x(lmk));왜냐하면 lmk정의되지 않았기 때문 입니다.

당사 사이트를 사용함과 동시에 당사의 쿠키 정책개인정보 보호정책을 읽고 이해하였음을 인정하는 것으로 간주합니다.
Licensed under cc by-sa 3.0 with attribution required.