16
$\begingroup$

I'm using an EKF for SLAM and I'm having some problem with the update step. I'm getting a warning that K is singular, rcond evaluates to near eps or NaN. I think I've traced the problem to the inversion of Z. Is there a way to calculate the Kalman Gain without inverting the last term?

I'm not 100% positive this is the cause of the problem, so I've also put my entire code here. The main entry point is 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

Edits: project(x(r), x(lmk)) should have been project(x(r), x(lmk_idx)) and is now corrected above.

K goes singular after a little while, but not immediately. I think it's around 20 seconds or so. I'll try the changes @josh suggested when I get home tonight and post the results.

Update 1:

My simulation first observes 2 landmarks, so K is $7\ x\ 2$. (P(rl,rl) * E_rl') * inv( Z ) results in a $5\ x\ 2$ matrix, so it can't be added to x in the next line.

K becomes singular after 4.82 seconds, with measurements at 50Hz (241 steps). Following the advice here, I tried K = (P(:, rl) * E_rl')/Z which results in 250 steps before a warning about K being singular is produced.

This tells me the problem isn't with matrix inversion, but it's somewhere else that's causing the problem.

Update 2

My main loop is (with a robot object to store x,P and landmark pointers):

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

At the end of this loop, I save x and P back to the robot, so I believe I'm propagating the covariance through each iteration.

More edits The (hopefully) correct eigenvalues are now here. There are a number of eigenvalues that are negative. Although their magnitude is never very large, $10^{-2}$ at most, it happens on the iteration immediately after the first landmark is observed and added to the map (in the "new landmarks" section of the main loop).

$\endgroup$
  • 1
    $\begingroup$ Are you propagating the uncertainty? Does the eigenvalues of your covariance get arbitrarily small or large? $\endgroup$ – Josh Vander Hook Nov 27 '12 at 9:39
  • 1
    $\begingroup$ What you put in pastebin is the eigenvectors, not the eigenvalues. do this: [v,d]=eig(P). disp(diag(d)). or just disp(eig(P)). Then, you can check the following necessary conditions: Are all eigenvalues >0 at every step (they should be in practice). Do they increase after propagation and decrease after measurements / corrections? In my experience, this is usually the problem. $\endgroup$ – Josh Vander Hook Nov 29 '12 at 16:15
  • 2
    $\begingroup$ Something is wrong if your eigenvalues are negative. When you initialize a landmark, what is the uncertainty associated with it's first estimated position? $\endgroup$ – Josh Vander Hook Dec 2 '12 at 17:58
  • $\begingroup$ The observation is a pair. When the first landmark is initialized, it has a covariance of [5.8938, 3.0941; 3.0941, 2.9562]. For the second, it's covariance is [22.6630 -14.3822; -14.3822, 10.5484] The full matrix is here $\endgroup$ – munk Dec 6 '12 at 1:58
5
$\begingroup$

I just see your post now and it is maybe too late to really help you... but in case you are still interested in this: I think that I identified your problem.

You write the innovation covariance matrix in the following way

E = jacobian measure * P * jacobian measure

It might be alright in theory but what happens is that if your algorithm is effective and especially if you are working on a simulation: the uncertainties will decrease, especially in the directions of your measurement. So E will tend to [[0,0][0,0]].

To avoid this problem what you can do is to add a measurement noise corresponding to the uncertainties in the measurement and your innovation covariance becomes

E= Jac*P*Jac'+R

where R is the covariance of the measurement noise (diagonal matrix where the terms on diagonal are the squares of the standard deviation of the noise). If you actually don't want to consider noise you can make it as small as you want.

I also add that your covariance update seems strange to me the classical formula is:

P=P - K * jacobian measure * P

I never saw your formula written anywhere else, I might be correct but if you are not sure of it you should check it.

$\endgroup$
  • $\begingroup$ Ah, the old "salt the covariance" trick. $\endgroup$ – Josh Vander Hook Apr 19 '13 at 19:35
1
$\begingroup$

If you are only updating the covariance sub-matrix associated with the robot and landmark (as is typical), then K and P should be $(N_r + N_l)\times(N_r + N_l)$ for robot state size $N_r$ and landmark size $N_l$. Note that you have:

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

which I think should be

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

(see: matrix division). Check the size of K.

Also: Please provide a little more information: Does K go singular immediately or only after some time?

This worries me: project(x(r), x(lmk)); since lmk is not defined.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.