我正在为SLAM使用EKF,并且更新步骤遇到了一些问题。我收到警告说K奇异,rcond的计算结果为near eps or NaN。我想我已经将问题归结为Z的倒数了。有没有一种方法可以计算Kalman增益而不倒数最后一项?

我不是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更改并发布结果。 x \ 2 $。 (P(rl,rl) * E_rl') * inv( Z )结果为$ 5 \ x \ 2 $矩阵,因此无法将其添加到下一行的x中。

4.82秒后K变为奇数,以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} $,但它会在观察到第一个地标并将其添加到地图后(在主循环的“新地标”部分中)立即在迭代中发生。 br />

评论

您是否正在传播不确定性?您协方差的特征值会变小还是变大?

您在pastebin中输入的是特征向量,而不是特征值。这样做:[v,d] = eig(P)。显示(diag(d))。或只是显示(eig(P))。然后,您可以检查以下必要条件:每个步骤的所有特征值均大于0(它们应该在实践中)。它们在传播后会增加,在测量/校正后会减少吗?以我的经验,这通常是问题所在。

如果您的特征值是负数,那是不对的。初始化地标时,与它的第一个估计位置相关的不确定性是什么?

观察是一对。初始化第一个界标时,其协方差为[5.8938,3.0941; 3.0941,2.9562]。第二,它的协方差是[22.6630 -14.3822; -14.3822,10.5484]完整矩阵在这里

#1 楼

我现在才看到您的帖子,要真正为您提供帮助可能为时已晚...但是如果您仍然对此感兴趣,我想我已经确定了您的问题。

您编写创新协方差矩阵以下面的方式

E = jacobian measure * P * jacobian measure

从理论上讲可能还可以,但是发生的是,如果您的算法有效,尤其是在进行仿真时:不确定性会减少,尤其是在您的测量方向上。因此,E趋向于[[0,0][0,0]]

要避免此问题,您可以添加与测量不确定性相对应的测量噪声,并且创新协方差变为<43。

其中E= Jac*P*Jac'+R是测量噪声的协方差(对角矩阵,对角线上的项是噪声标准偏差的平方)。如果您实际上不想考虑噪声,则可以将其设置为您想要的小。 /> R

我从未在其他地方看到过您的公式,这也许是正确的,但如果不确定,则应进行检查。

评论


$ \ begingroup $
啊,古老的“盐度协方差”技巧。
$ \ endgroup $
–乔什·范德·胡克(Josh Vander Hook)
13年4月19日在19:35

#2 楼

如果仅更新与机器人和地标相关联的协方差子矩阵(通常),则对于机器人状态大小$ N_r $和KP应为$(N_r + N_1)\ times(N_r + N_1)$。界标大小$ N_l $。请注意,您具有:

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

我认为应该是

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

(请参阅:矩阵除法) )。
检查K的大小。

还:
请提供更多信息:K是立即变单还是仅在一段时间后变单?

这让我担心:project(x(r), x(lmk));,因为未定义lmk