基于TDOA\FDOA的Taylor级数程序错误

我想问的是，我编写程序虽然能够运行，但是delta运算出来后显示的是NAN，而且结果错误，能否帮我看一下，谢谢_

target_guji=[50,50,50,20,10,0];%辐射源估计值
target_zhenshi=[20,20,20,40,20,0];%辐射源真实值
BS=[100,100,100;200,0,0;-100,170,150;-100,-170,150];%基站坐标
Noise=0.1*randn(3,1);%噪音
%%距离
Rb = sqrt((target_zhenshi(1) - BS(1,1))^2+(target_zhenshi(2) - BS(1,2))^2+(target_zhenshi(3) - BS(1,3))^2);%目标真实到主站的距离
BSN = size(BS,1)-1;      
RD = zeros(BSN,1);
for i = 1:BSN                
    RD(i) = -Rb+sqrt((target_zhenshi(1)- BS(i+1,1))^2+(target_zhenshi(2) - BS(i+1,2))^2+(target_zhenshi(3) - BS(i+1,3))^2)+Noise(i);%目标真实到辅站的距离与目标真实到主站距离差值
end
%%速度
Vb=[target_zhenshi(4),target_zhenshi(5),target_zhenshi(6)];%目标真实到主站的速度向量
Vb = Vb*Vb'/Rb;%目标真实到主站的速度
VD =[target_zhenshi(4),target_zhenshi(5),target_zhenshi(6)];
Vv = zeros(BSN,1);
Vv(1)=VD*VD'/(sqrt((target_zhenshi(1)- BS(2,1))^2+(target_zhenshi(2) - BS(2,2))^2+(target_zhenshi(3) - BS(2,3))^2)+Noise(1));
Vv(2)=VD*VD'/(sqrt((target_zhenshi(1)- BS(3,1))^2+(target_zhenshi(2) - BS(3,2))^2+(target_zhenshi(3) - BS(3,3))^2)+Noise(2));
Vv(3)=VD*VD'/(sqrt((target_zhenshi(1)- BS(4,1))^2+(target_zhenshi(2) - BS(4,2))^2+(target_zhenshi(3) - BS(4,3))^2)+Noise(3));
% TDOA协方差矩阵Q
Q = zeros(6,6);  
for i = 1: 3
    Q(i,i)=1;
    Q(i+3,i+3)=0.1;
end
delta = [1 1 1];   
kk = 0;   
% Taylor级数展开法
while ((abs(delta(1)) + abs(delta(2))+abs(delta(3))) > 0.01)   
    R1 = sqrt((target_guji(1) - BS(1,1))^2 + (target_guji(2) - BS(1,2))^2+(target_guji(3) - BS(1,3))^2);  %目标估计值到主站的距离
    R = zeros(1,BSN);
    kk = kk+1;
    for i = 1: BSN 
        R(i) = sqrt((target_guji(1) - BS(i+1,1))^2 + (target_guji(2) - BS(i+1,2))^2+(target_guji(3) - BS(i+1,2))^2);%目标估计值到辅站的距离
    end
    V1= [target_guji(4),target_guji(5),target_guji(6)]; %目标估计值到主站的速度向量
    V1 = V1*V1'/R1;%目标真实到主站的速度
    V=zeros(BSN,1);
    VV = zeros(BSN,1);
    VH =[target_guji(4),target_guji(5),target_guji(6)];
    VV(1)=VH*VH'/(sqrt((target_guji(1)- BS(2,1))^2+(target_guji(2) - BS(2,2))^2+(target_guji(3) - BS(2,3))^2));
    VV(2)=VH*VH'/(sqrt((target_guji(1)- BS(3,1))^2+(target_guji(2) - BS(3,2))^2+(target_guji(3) - BS(3,3))^2));
    VV(3)=VH*VH'/(sqrt((target_guji(1)- BS(4,1))^2+(target_guji(2) - BS(4,2))^2+(target_guji(3) - BS(4,3))^2));
     % W
     W=zeros(6,1);
     for i = 1: 3
         W(i)=  (R(i) - R1)-RD(i) ;%距离参数值减去真实值
         W(i+3)=(VV(i) - V1)-(Vv(i)-Vb);%速度参数值减去真实值
     end
     %A
     A=zeros(6,6);
     for i = 1: 3
         A(i,1)=-((target_guji(1)-BS(1,1))/R1 - (target_guji(1)-BS(i+1,1))/R(i));
         A(i,2)=-((target_guji(2)-BS(1,2))/R1 - (target_guji(2)-BS(i+1,2))/R(i));
         A(i,3)=-((target_guji(3)-BS(1,3))/R1 - (target_guji(3)-BS(i+1,3))/R(i));
         A(i+3,4)=-((target_guji(1)-BS(1,1))/R1 - (target_guji(1)-BS(i+1,1))/R(i));
         A(i+3,5)=-((target_guji(2)-BS(1,2))/R1 - (target_guji(2)-BS(i+1,2))/R(i));
         A(i+3,6)=-((target_guji(3)-BS(1,3))/R1 - (target_guji(3)-BS(i+1,3))/R(i));
         A(i,4)=0;
         A(i,5)=0;
         A(i,6)=0;
         A(i+3,1)=(target_guji(1)/R(i))-((VV(i)/R(i)^2)*(target_guji(1)-BS(i+1,1)))-(target_guji(1)/R1-(V1/R1^2)*(target_guji(1)-BS(1,1)));
         A(i+3,2)=(target_guji(2)/R(i))-((VV(i)/R(i)^2)*(target_guji(2)-BS(i+1,2)))-(target_guji(2)/R1-(V1/R1^2)*(target_guji(2)-BS(1,2)));
         A(i+3,3)=(target_guji(3)/R(i))-((VV(i)/R(i)^2)*(target_guji(3)-BS(i+1,3)))-(target_guji(3)/R1-(V1/R1^2)*(target_guji(3)-BS(1,3)));
     end
      % delta
      delta = inv(A'*inv(Q)*A)*A'*inv(Q)*W;  
      if (abs(delta(1))+abs(delta(2))+abs(delta(3))) > 0.01   
        EP = target_guji + delta';   
        target_guji = EP;   % 更新迭代值
      end
end
z_out = target_guji   % 标签坐标估计值

您好，这是我编写的基于TDOA\FDOA的Taylor级数定位方法，具体公式：

你把代码文件和数据发给我