Ich verwende ein EKF für SLAM und habe ein Problem mit dem Aktualisierungsschritt. Ich bekomme eine Warnung, dass K Singular ist, rcond
bewertet zu near eps or NaN
. Ich glaube, ich habe das Problem auf die Inversion von Z zurückgeführt. Gibt es eine Möglichkeit, den Kalman-Gewinn zu berechnen, ohne den letzten Term zu invertieren?
Ich bin nicht zu 100% sicher, dass dies die Ursache des Problems ist, daher habe ich auch meinen gesamten Code hier abgelegt . Der Haupteingangspunkt ist 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
Änderungen:
project(x(r), x(lmk))
sollten project(x(r), x(lmk_idx))
und werden jetzt oben korrigiert.
K wird nach einer Weile singulär, aber nicht sofort. Ich glaube, es sind ungefähr 20 Sekunden. Ich werde die von @josh vorgeschlagenen Änderungen ausprobieren, wenn ich heute Abend nach Hause komme und die Ergebnisse veröffentliche.
Update 1:
(P(rl,rl) * E_rl') * inv( Z )
K wird nach 4,82 Sekunden mit Messungen bei 50 Hz (241 Schritte) singulär. Nach den hier gegebenen Ratschlägen habe ich versucht, K = (P(:, rl) * E_rl')/Z
was zu 250 Schritten führt, bevor eine Warnung über K als Singular ausgegeben wird.
Dies sagt mir, dass das Problem nicht mit der Matrixinversion zusammenhängt, sondern an einer anderen Stelle, die das Problem verursacht.
Update 2
Meine Hauptschleife ist (mit einem Roboterobjekt zum Speichern von x-, P- und Orientierungspunktzeigern):
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
Am Ende dieser Schleife speichere ich x und P zurück auf den Roboter, also glaube ich, dass ich die Kovarianz durch jede Iteration propagiere.
Antworten:
Ich sehe gerade Ihren Beitrag und es ist vielleicht zu spät, um Ihnen wirklich zu helfen ... aber falls Sie noch daran interessiert sind: Ich glaube, ich habe Ihr Problem identifiziert.
Sie schreiben die Innovationskovarianzmatrix folgendermaßen
E = jacobian measure * P * jacobian measure
Theoretisch mag es in Ordnung sein, aber wenn Ihr Algorithmus effektiv ist und Sie gerade an einer Simulation arbeiten, werden die Unsicherheiten, insbesondere in Richtung Ihrer Messung, geringer. Werde also
E
dazu neigen[[0,0][0,0]]
.Um dieses Problem zu vermeiden, können Sie ein Messrauschen hinzufügen, das den Messunsicherheiten entspricht und Ihre Innovationskovarianz erhöht
E= Jac*P*Jac'+R
wo
R
ist die Kovarianz des Messrauschens (Diagonalmatrix, wobei die Terme auf der Diagonale die Quadrate der Standardabweichung des Rauschens sind). Wenn Sie Geräusche eigentlich nicht berücksichtigen möchten, können Sie sie so klein machen, wie Sie möchten.Ich füge hinzu, dass mir Ihr Kovarianz-Update seltsam vorkommt. Die klassische Formel lautet:
P=P - K * jacobian measure * P
Ich habe Ihre Formel nirgendwo anders geschrieben gesehen, vielleicht habe ich recht, aber wenn Sie sich nicht sicher sind, sollten Sie sie überprüfen.
quelle
Wenn Sie nur die Kovarianz-Submatrix aktualisieren, die dem Roboter und dem Orientierungspunkt zugeordnet ist (wie es typisch ist), dann( Nr+ Nl) × ( Nr+ Nl) Nr Nl
K
P
K = P(:, rl) * E_rl' * Z^-1
was ich denke sollte sein
(P(rl,rl) * E_rl') * inv(Z)
.(siehe: Matrixeinteilung ). Überprüfen Sie die Größe von
K
.Auch: Bitte geben Sie ein wenig mehr Informationen: Geht
K
Singular sofort oder erst nach einiger Zeit?Das macht mir Sorgen:
project(x(r), x(lmk));
dalmk
ist nicht definiert.quelle