Я використовую EKF для SLAM, і у мене є проблеми з кроком оновлення. Я отримую попередження про те, що K є одниною, rcondоцінюється near eps or NaN. Я думаю, що я простежив проблему до інверсії Z. Чи є спосіб обчислити кальманський посилення, не перевертаючи останній термін?
Я не на 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 секунд. Я спробую зміни @josh запропонував, коли я сьогодні приїду додому і опублікую результати.
Оновлення 1:
У моєму моделюванні спочатку спостерігаються 2 орієнтири, тож K - . приводить до матриці , тому її не можна додати до x у наступному рядку. (P(rl,rl) * E_rl') * inv( Z )
K стає сингулярним через 4,82 секунди з вимірюванням на 50 ГГц (241 крок). Дотримуючись поради тут , я спробував, K = (P(:, rl) * E_rl')/Zщо призводить до 250 кроків, перш ніж надходить попередження про те, що K є єдиним.
Це говорить про те, що проблема не в інверсії матриці, але це десь інше, що викликає проблему.
Оновлення 2
Мій основний цикл (з об'єктом робота для зберігання x, P та орієнтирів):
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 назад до робота, тому я вважаю, що я поширюю коваріацію через кожну ітерацію.
Більше редагувань Тут (сподіваємось) правильні власні значення тепер тут . Існує ряд власних значень, які є негативними. Хоча їх величина ніколи не буває дуже великою, максимум , , це відбувається під час ітерації одразу після того, як спостерігається перший орієнтир та додається до карти (у розділі «нові орієнтири» головного циклу).