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''
。我无法弄清楚问题所在。