Paso de actualización de EKF-SLAM, Kalman Gain se convierte en singular

Estoy usando un EKF para SLAM y tengo algún problema con el paso de actualización. Recibo una advertencia de que K es singular, rcondevalúa a near eps or NaN. Creo que he rastreado el problema hasta la inversión de Z. ¿Hay alguna forma de calcular la ganancia de Kalman sin invertir el último término?

No estoy 100% seguro de que esta sea la causa del problema, así que también he puesto mi código completo aquí . El punto de entrada principal es 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

Ediciones: project(x(r), x(lmk))debería haber sido project(x(r), x(lmk_idx))y ahora está corregido anteriormente.

K se vuelve singular después de un tiempo, pero no de inmediato. Creo que son alrededor de 20 segundos más o menos. Probaré los cambios que @josh sugirió cuando llegue a casa esta noche y publique los resultados.

Actualización 1:

Mi simulación primero observa 2 puntos de referencia, por lo que K es . da como resultado una matriz , por lo que no se puede agregar a x en la siguiente línea. $7\ x\ 2$(P(rl,rl) * E_rl') * inv( Z )$5\ x\ 2$

K se vuelve singular después de 4,82 segundos, con mediciones a 50Hz (241 pasos). Siguiendo el consejo aquí , probé los K = (P(:, rl) * E_rl')/Zresultados en 250 pasos antes de que se produzca una advertencia sobre K siendo singular.

Esto me dice que el problema no es con la inversión de la matriz, sino que está causando el problema en otro lugar.

Actualización 2

Mi bucle principal es (con un objeto de robot para almacenar x, P y punteros de referencia):

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

Al final de este ciclo, guardo xy P de nuevo en el robot, así que creo que estoy propagando la covarianza a través de cada iteración.

Más ediciones Los (con suerte) valores propios correctos ya están aquí . Hay una serie de valores propios que son negativos. Aunque su magnitud nunca es muy grande, como máximo, ocurre en la iteración inmediatamente después de que se observa el primer punto de referencia y se agrega al mapa (en la sección "nuevos puntos de referencia" del bucle principal).$10^{-2}$

Acabo de ver tu publicación ahora y tal vez sea demasiado tarde para realmente ayudarte ... pero en caso de que aún estés interesado en esto: creo que identifiqué tu problema.

Escribe la matriz de covarianza de innovación de la siguiente manera

E = jacobian measure * P * jacobian measure

Podría estar bien en teoría, pero lo que sucede es que si su algoritmo es efectivo y especialmente si está trabajando en una simulación: las incertidumbres disminuirán, especialmente en las direcciones de su medición. Así Etenderá a hacerlo [[0,0][0,0]].

Para evitar este problema, lo que puede hacer es agregar un ruido de medición correspondiente a las incertidumbres en la medición y su covarianza de innovación se convierte en

E= Jac*P*Jac'+R

donde Res la covarianza del ruido de medición (matriz diagonal donde los términos en diagonal son los cuadrados de la desviación estándar del ruido). Si realmente no desea considerar el ruido, puede hacerlo tan pequeño como desee.

También agrego que su actualización de covarianza me parece extraña, la fórmula clásica es:

P=P - K * jacobian measure * P

Nunca vi su fórmula escrita en ningún otro lado, podría estar en lo correcto, pero si no está seguro de eso, debe verificarla.

Si sólo está actualizando el sub-matriz de covarianza asociada con el robot y punto de referencia (como es típico), entonces Ky Pdebería ser para el tamaño del Estado robot y el tamaño de punto de interés . Tenga en cuenta que tiene:$(N_r + N_l)\times(N_r + N_l)$$N_r$$N_l$

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

que creo que debería ser

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

(ver: división matricial ). Comprueba el tamaño de K.

Además: proporcione un poco más de información: ¿se Kvuelve singular inmediatamente o solo después de un tiempo?

Esto me preocupa: project(x(r), x(lmk));ya lmkque no está definido.