user_position.m

gps c/a码程序以及模拟卫星定位的源代码、地球模型

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