আমি স্লামের জন্য একটি ইকেএফ ব্যবহার করছি এবং আপডেটের পদক্ষেপে আমার কিছু সমস্যা হচ্ছে। আমি একটি সতর্কতা পাচ্ছি যে কে একবাক্য, rcondমূল্যায়ন করে near eps or NaN। আমি মনে করি আমি জেড এর বিপরীত দিকে সমস্যাটি চিহ্নিত করেছি last শেষ শব্দটি উল্টিয়ে না দিয়ে কলমেন গেইনের কোনও উপায় আছে কি?
আমি এই সমস্যাটির কারণ হিসাবে 100% ইতিবাচক নই, তাই আমি আমার সম্পূর্ণ কোডটি এখানেও রেখেছি । মূল এন্ট্রি পয়েন্টটি স্ল্যাম 2 ডি।
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))এবং এখন উপরে সংশোধন করা হয়েছে ।
কে কিছুক্ষণ পরে একবচন হয়ে যায়, তবে তাৎক্ষণিকভাবে নয়। আমার মনে হয় এটি প্রায় 20 সেকেন্ডেরও বেশি। আমি আজ রাতে বাসায় পৌঁছে ফলাফলগুলি পোস্ট করার পরে @ জোশের প্রস্তাবিত পরিবর্তনগুলি চেষ্টা করব।
আপডেট 1:
(P(rl,rl) * E_rl') * inv( Z )
কে 50Hz (241 ধাপ) পরিমাপের সাথে কে 4.82 সেকেন্ড পরে একবচনে পরিণত হয়। এখানে পরামর্শ অনুসরণ করে , আমি চেষ্টা করেছি K = (P(:, rl) * E_rl')/Zযে কে একবিন্দু হওয়ার বিষয়ে সতর্কতা উত্পন্ন হওয়ার আগে 250 টি ধাপে ফলাফল হয়।
এটি আমাকে বলছে যে সমস্যাটি ম্যাট্রিক্স বিপরীতে নয়, তবে অন্য কোথাও সমস্যা সৃষ্টি করছে that's
আপডেট 2
আমার প্রধান লুপটি হল (এক্স, পি এবং ল্যান্ডমার্ক পয়েন্টার সঞ্চয় করার জন্য একটি রোবট অবজেক্ট সহ):
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
এই লুপটির শেষে, আমি এক্স এবং পি কে রোবোটে ফিরে সঞ্চয় করি, তাই আমি বিশ্বাস করি যে আমি প্রতিটি পুনরাবৃত্তির মাধ্যমে সমবায় প্রচার করছি।