मैं SLAM के लिए एक EKF का उपयोग कर रहा हूं और मुझे अद्यतन चरण में कुछ समस्या हो रही है। मुझे एक चेतावनी मिल रही है कि K एकवचन है, का rcondमूल्यांकन करता है near eps or NaN। मुझे लगता है कि मैंने समस्या को जेड के व्युत्क्रम में ट्रेस किया है। क्या अंतिम कार्यकाल के बिना कलमैन लाभ की गणना करने का एक तरीका है?
मैं 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 सेकंड या तो है। मैं आज रात को घर आने पर और परिणामों को पोस्ट करने के लिए सुझाए गए @ जोश में बदलाव की कोशिश करूँगा।
अपडेट 1:
(P(rl,rl) * E_rl') * inv( Z )
K 4.82 सेकंड के बाद 50Hz (241 कदम) पर माप के साथ एकवचन बन जाता है। यहाँ सलाह के बाद , मैंने कोशिश की K = (P(:, rl) * E_rl')/Zकि K विलक्षण होने के बारे में चेतावनी देने से पहले 250 चरणों में परिणाम दिया जाए।
यह बताता है कि समस्या मैट्रिक्स के उलट होने के साथ नहीं है, लेकिन यह कहीं और है जो समस्या का कारण है।
अपडेट २
मेरा मुख्य लूप है (एक्स, पी और लैंडमार्क पॉइंटर्स को स्टोर करने के लिए एक रोबोट ऑब्जेक्ट के साथ):
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 को रोबोट में वापस सहेजता हूं, इसलिए मुझे विश्वास है कि मैं प्रत्येक प्रवाह के माध्यम से सहसंयोजक का प्रचार कर रहा हूं।