for j=1:length(idf)
dx = xf(1,j)- xv(1);
dy = xf(2,j)- xv(2);
d2 = dx^2 + dy^2;
d = sqrt(d2);
Z_est(:,j) = [d;atan2(dy,dx)-xv(3)];
S(:,:,j) = Hf(:,:,j) * pf(:,:,j) * Hf(:,:,j)' + R
end
v = zf - Z_est; %innovation
v(2,:) = pi_to_pi(v(2,:));
w= 1;
for n = 1:size(zf,2)
den = 2*pi*sqrt(det(S));
neu = exp(-0.5 * v(:,n)' * inv(S) * v(:,n));
w = w*(neu/den);
end
我的程序是根据粒子过滤计算我的状态粒子的重量,在它开始运行并计算W,bt在某个时候它显示错误??? Undefined function or method 'det' for input arguments of type 'double' and attributes 'full 3d real''
。
我无法弄清楚问题。
答案 0 :(得分:0)
你需要给det
一个方形矩阵而不是三维矩阵(S
是一个3d数组)。
我认为你打算写一下:
den = 2*pi*sqrt(det(S(:,:,n));
因此,在给定的循环迭代中,正在计算一片S的det
。