x=60;y=70;z=75;r=65;
function [iEP] = TaylorAlgorithm(x,y,z,r)
BS=[80,80,-80,-80;80,-80,-80,80];
iEP=zeros(2,1);
MS=zeros(2,1);
toa=zeros(1,4);
toa(1)=x/(3*10^8);
toa(2)=y/(3*10^8);
toa(3)=z/(3*10^8);
toa(4)=r/(3*10^8);
if toa(1)==0
iEP=BS(:,1)
end
if toa(2)==0
iEP=BS(:,2)
end
if toa(3)==0
iEP=BS(:,3)
end
if toa(4)==0
iEP=BS(:,4)
end
if(toa(1)~=0&&toa(2)~=0&&toa(3)~=0&&toa(4)~=0)
MS=((BS(:,1)/toa(1)+BS(:,2)/toa(2)+BS(:,3)/toa(3)+BS(:,4)/toa(4))/(1/toa(1)+1/toa(2)+1/toa(3)+1/toa(4)))';
BSN=length(BS(1,:))-1;
Q = eye(BSN-1);
for i=1:BSN-1
h0(i)=toa(i+1)-toa(1);
end
delt=ones(2,1);
while((delt(1)^2+delt(2)^2)>0.0001)
R1 = sqrt((iEP(1)-BS(1,1))^2 +(iEP(2)-BS(2,1))^2);
for i =1: BSN-1,
R(i) = sqrt((iEP(1) - BS(1,i+1))^2 + (iEP(2) - BS(2,i+1))^2);
end
for i = 1: BSN-1,
h(i) = h0(i) - (R(i) - R1);
end
ht = h';
for i = 1: BSN-1,
Gt(i, 1) =(BS(1,1) -iEP(1))/R1 - (BS(1, i+1) - iEP(2))/R(i);
Gt(i, 2) =(BS(2,1)-iEP(2))/R1 - (BS(2, i+1) - iEP(2))/R(i);
end
delt = inv(Gt'*inv(Q)*Gt)*Gt'*inv(Q)*ht;
EP = iEP + delt';
iEP = EP;
end
iEP=iEP';
end
运行不起
|
