Я использую 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:
(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 обратно в робота, поэтому я считаю, что я распространяю ковариацию через каждую итерацию.