?? calculateuserposition.m
字號:
function caluserposition=calculateuserposition(satelliteposition)
r1=6400;
c=300000;
deltaT=1e-3;
satelliteposnew=ones(1,3);
vissatnum=0;
calculateok=1;
for k=1:24
if (satelliteposition(k,4)==1)
vissatnum=vissatnum+1;
satelliteposnew=[satelliteposnew;satelliteposition(k,1:3)];
end
end
satelliteposnew(1,:)=[];
if (vissatnum<4)
calculateok=0;
caluserposition=[0 0 0 0];
return
end
prange=ones(1,vissatnum);
userpos=satelliteposition(25,1:3);
for n=1:vissatnum
prange(1,n)=sqrt((satelliteposnew(n,:)-userpos)...
*(satelliteposnew(n,:)-userpos)')+c*deltaT;
end
calculaterecord=[1 1 1];
xyz0=[0 0 0];
deltaT0=0;
wxyz=satelliteposnew;
error=1000;computetime=0;
while ((error>1)&(computetime<1000))
computetime=computetime+1;
r=ones(1,vissatnum);
for n=1:vissatnum
r(1,n)=sqrt((wxyz(n,:)-xyz0)*(wxyz(n,:)-xyz0)')+deltaT0*c;
end
deltap=r-prange;
a=ones(vissatnum,3);
for n=1:vissatnum
a(n,:)=(wxyz(n,:)-xyz0)./r(1,n);
end
h=[a ones(vissatnum,1)];
deltax=inv(h'*h)*h'*deltap';
tempdeltax=deltax(1:3);
error=max(abs(tempdeltax));
xyz0=xyz0+deltax(1:3,:)';
if (computetime<10)
calculaterecord=[calculaterecord;xyz0];
end
deltaT0=deltax(4,1)/(-c);
end
if (computetime==1000)
caluserposition=[0 0 0];
else
caluserposition=[xyz0;calculaterecord];
end
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -