Tôi đang sử dụng EKF cho SLAM và tôi gặp một số vấn đề với bước cập nhật. Tôi đang nhận được một cảnh báo rằng K là số ít, rcondđánh giá near eps or NaN. Tôi nghĩ rằng tôi đã tìm ra vấn đề đối với sự đảo ngược của Z. Có cách nào để tính toán Kalman Gain mà không đảo ngược thuật ngữ cuối cùng không?
Tôi không tích cực 100% đây là nguyên nhân của vấn đề, vì vậy tôi cũng đã đặt toàn bộ mã của mình ở đây . Điểm vào chính là 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
Chỉnh sửa:
project(x(r), x(lmk))nên đã project(x(r), x(lmk_idx))và đang được sửa chữa ở trên.
K đi số ít sau một lúc, nhưng không ngay lập tức. Tôi nghĩ rằng đó là khoảng 20 giây hoặc lâu hơn. Tôi sẽ thử những thay đổi mà @josh đề xuất khi tôi về nhà tối nay và đăng kết quả.
Cập nhật 1:
Mô phỏng của tôi trước tiên quan sát 2 mốc, vì vậy K là . dẫn đến ma trận , do đó không thể thêm nó vào x trong dòng tiếp theo. (P(rl,rl) * E_rl') * inv( Z )
K trở thành số ít sau 4,82 giây, với các phép đo ở 50Hz (241 bước). Theo lời khuyên ở đây , tôi đã thử K = (P(:, rl) * E_rl')/Zkết quả trong 250 bước trước khi cảnh báo về K là số ít được tạo ra.
Điều này cho tôi biết vấn đề không phải là đảo ngược ma trận, nhưng nó ở đâu đó gây ra vấn đề.
Cập nhật 2
Vòng lặp chính của tôi là (với một đối tượng robot để lưu trữ x, P và các con trỏ mốc):
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
Ở cuối vòng lặp này, tôi lưu x và P trở lại robot, vì vậy tôi tin rằng tôi đang truyền bá hiệp phương sai qua mỗi lần lặp.
Nhiều chỉnh sửa Các giá trị bản địa chính xác (hy vọng) hiện đang ở đây . Có một số giá trị riêng là âm. Mặc dù cường độ của chúng không bao giờ lớn, nhưng tối đa , nó xảy ra ở lần lặp ngay sau khi mốc đầu tiên được quan sát và thêm vào bản đồ (trong phần "mốc mới" của vòng lặp chính).