rcond
的计算结果为near eps or NaN
。我想我已经将问题归结为Z的倒数了。有没有一种方法可以计算Kalman增益而不倒数最后一项? 我不是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更改并发布结果。 x \ 2 $。
(P(rl,rl) * E_rl') * inv( Z )
结果为$ 5 \ x \ 2 $矩阵,因此无法将其添加到下一行的x中。 4.82秒后K变为奇数,以50Hz(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保存回机器人,所以我相信我会在每次迭代中传播协方差。
更多编辑
(希望)正确的特征值现在在这里。有许多特征值是负的。尽管它们的大小永远不会很大,最多$ 10 ^ {-2} $,但它会在观察到第一个地标并将其添加到地图后(在主循环的“新地标”部分中)立即在迭代中发生。 br />
#1 楼
我现在才看到您的帖子,要真正为您提供帮助可能为时已晚...但是如果您仍然对此感兴趣,我想我已经确定了您的问题。您编写创新协方差矩阵以下面的方式
E = jacobian measure * P * jacobian measure
从理论上讲可能还可以,但是发生的是,如果您的算法有效,尤其是在进行仿真时:不确定性会减少,尤其是在您的测量方向上。因此,
E
趋向于[[0,0][0,0]]
。要避免此问题,您可以添加与测量不确定性相对应的测量噪声,并且创新协方差变为<43。
其中
E= Jac*P*Jac'+R
是测量噪声的协方差(对角矩阵,对角线上的项是噪声标准偏差的平方)。如果您实际上不想考虑噪声,则可以将其设置为您想要的小。 /> R
我从未在其他地方看到过您的公式,这也许是正确的,但如果不确定,则应进行检查。
评论
$ \ begingroup $
啊,古老的“盐度协方差”技巧。
$ \ endgroup $
–乔什·范德·胡克(Josh Vander Hook)
13年4月19日在19:35
#2 楼
如果仅更新与机器人和地标相关联的协方差子矩阵(通常),则对于机器人状态大小$ N_r $和K
和P
应为$(N_r + N_1)\ times(N_r + N_1)$。界标大小$ N_l $。请注意,您具有:K = P(:, rl) * E_rl' * Z^-1
我认为应该是
(P(rl,rl) * E_rl') * inv(Z)
。(请参阅:矩阵除法) )。
检查
K
的大小。还:
请提供更多信息:
K
是立即变单还是仅在一段时间后变单? 这让我担心:
project(x(r), x(lmk));
,因为未定义lmk
。
评论
您是否正在传播不确定性?您协方差的特征值会变小还是变大?您在pastebin中输入的是特征向量,而不是特征值。这样做:[v,d] = eig(P)。显示(diag(d))。或只是显示(eig(P))。然后,您可以检查以下必要条件:每个步骤的所有特征值均大于0(它们应该在实践中)。它们在传播后会增加,在测量/校正后会减少吗?以我的经验,这通常是问题所在。
如果您的特征值是负数,那是不对的。初始化地标时,与它的第一个估计位置相关的不确定性是什么?
观察是一对。初始化第一个界标时,其协方差为[5.8938,3.0941; 3.0941,2.9562]。第二,它的协方差是[22.6630 -14.3822; -14.3822,10.5484]完整矩阵在这里