daima.zip_gps 卫星位置_卫星位置计算_用户位置

% p2 1.m % Userpos.m use pseudorange and satellite positions to calculate user position % JT 30 April 96 % ***** Input data ***** sp(1:3, 1:nsat); % satellite position which has the following format % sp =[?x1 y1 z1 x2 y2 z2 x3 y3 z3] xnn ynn znn pr(1:nsat); % is the measured pseudorange which has the format as % pr=[pr1 pr2 pr3 ... prnn]T; nn=nsat; % is the number of satellites % ***** Select initial guessed positions and clock bias ***** x guess = 0; y guess = 0; z guess = 0; bu = 0; gu(1) = x_guess; gu(2) = y_guess; gu(3) = z_guess; % Calculating rao the pseudorange as shown in Equation (2.1)the % clock bias is not included. for j = 1:nsat rao(j)=((gu(1)-sp(1,j))^2+(gu(2)-sp(2,j))^2+(gu(3)-sp(3,j))^2)^.5; end % generate the fourth column of the alpha matrix in Eq. 2.15 alpha(:,4) = ones(nsat,1); erro=1; while erro>.01 for j = 1:nsat for k = 1:3 alpha(j,k) = (gu(k)-sp(k,j))/(rao(j)); % find first %3 colums of alpha matrix end end drao = pr - (rao + ones(1,nsat)*bu);%** find delta rao % includes clock bias dl = pinv(alpha)*drao; % Equation (2.16) pinv (pseudoinverse); bu = bu + dl(4); % new clock bias REFERENCES 29 for k = 1:3 gu(k) = gu(k) + dl(k); %** find new position end erro=dl(1)^2+dl(2)^2+dl(3)^2; % find error for j = 1:nsat rao(j)=(gu(1)-sp(1,j))^2+(gu(2)-sp(2,j)^2+(gu(3)-sp(3,j))^2)^.5; % find new rao without clock bias end end % ***** Final result in spherical coordinate system ***** xuser = gu(1); yuser = gu(2); zuser = gu(3); bias = bu; rsp = (xuser^2+yuser^2+zuser^2)^.5; % radius of spherical earth % Eq 2.17 Lc = atan(zuser/(xuser^2+yuser^2)^.5); % latitude of sperical % earth Eq 2.18 lsp = atan(yuser/xuser)*180/pi; % longitude spherical and flat % earth Eq 2.19 % ***** Converting to practical earth shape ***** e=1/298.257223563; Ltemp=Lc; erro1=1; while erro1>1e-6 % calculating latitude by Eq. 2.51 L=Lc+e*sin(2*Ltemp); erro1=abs(Ltemp-L); Ltemp=L; end Lflp=L*180/pi; % latitude of flat earth re=6378137; h=rsp-re*(1-e*(sin(L)^2)); % altitude of flat earth % longitude of flat earth upos = [xuser yuser zuser bias rsp Lflp lsp h];