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. (P(rl,rl) * E_rl') * inv( Z )
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).