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

4

1 回答 1

0

你需要给det一个方阵而不是一个 3d 的(S是一个 3d 数组)。我假设您打算改写:

den = 2*pi*sqrt(det(S(:,:,n));

因此,在给定的循环迭代中det,正在计算 S 的切片。

于 2013-05-09T17:13:29.023 回答