SLAMにEKFを使用していますが、更新手順に問題があります。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に追加することはできません。 5 x 2(P(rl,rl) * E_rl') * inv( Z )
Kは4.82秒後に特異になり、50Hz（241ステップ）で測定されます。ここでのアドバイスに従って、K = (P(:, rl) * E_rl')/ZKが特異であるという警告が生成される前に、250のステップが発生することを試みました。
これは、問題がマトリックスの反転ではないことを教えてくれますが、問題を引き起こしているのは他のどこかです。
更新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をロボットに保存します。そのため、各反復で共分散を伝播していると思います。
その他の編集 （できれば）正しい固有値がここにあります。負の固有値がいくつかあります。それらの大きさは決してほど大きくなることはありませんが、最初のランドマークが観察され、マップに追加された直後の反復で発生します（メインループの「新しいランドマーク」セクション）。