The GPS Easy Suite–Matlab code for the GPS newcomer

function[phi,lambda] = recpos23(filename) % RECPOS_923: Least-squares searching for receiver position. Given % 4 or more pseudoranges and data from ephemerides. % Idea to this script originates from Clyde C. Goad % % Made by Kai Borre 04-19-96 % Copyright (c) by Kai Borre % Revision 1.0 Date: 1997/09/24 % Modified by group 923 AAU, Date: 1998/11/25 % Improved by: 1) A better and faster intial guess taking % care of the problem near the Greenwich meridian % 2) A simpler data input format % 3) Using rotation outside loops % 4) Using a faster/converging grid + an optimized % interation bound % 5) Removing all sin/cos ops. from code, only those % which can be tabelized remain % 6) Moving asin ops. out of loops leaving only two % asins left % Using data: filename (ex. recpos_input_ohio.mat) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Load file for Space vehicle positions, pseudoranges and % the correction value tcorr %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% format compact str = 'load*'; % these four lines loads a file str = strcat(str,filename); str = strrep(str,'*',' '); eval(str); disp(ID); % display name of the datafile %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Define constants and initialize variables % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %Grid parameters iterations = 38; % iterations in main loop, signifies precision scalediv = 1.5; % scale division factor ndiv = 1; % no. of slices in inclination of spherical cap azimuth_angle = 60; % angle to slice spherical cap in azimuth last_angle = 300; % maximum angle in azimuth scale = 70; % initialize scale (deg from zenith to cap edge) % Universal and global constants dtr = pi/180; % conversion factor between degrees and radians vlight = 299792458; % vacuum speed of light in m/s Omegae_dot = 7.292115147e-5; % Earth rotation rate in rad/s semi_major_axis = 6378137; % Earth semi major axis flattening = 1/298.257223563; % Earth flattening esq=(2-flattening)*flattening; % Earth eccentricity squared (6.69437999014e-3) % Inverse constants, used to avoid division inv_scalediv = 1/scalediv; % inverse of scalediv (used only with psi LUT's) inv_ndiv = 1/ndiv; % inverse of ndiv (used only with psi LUT's) inv_vlight = 1/vlight; % inverse of vlight inv_dtr = 180/pi; % inverse of dtr %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Generate lookup tables for the sine and cosine of alpha and psi, respectively %%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% alpha_counter=0; for alpha = 0:azimuth_angle:last_angle, % generate cos/sin table for alpha (2*6 values) alpha_counter = alpha_counter+1; % inc alpha_counter cos_alpha(alpha_counter) = cos(alpha*dtr); sin_alpha(alpha_counter) = sin(alpha*dtr); end psi_counter=0; for iter=1:iterations, for b = 1:ndiv, % generate cos/sin table for psi (2*38 values) psi_counter=psi_counter+1; % inc psi_counter psi = b*scale*inv_ndiv; % angle to zenith in spherical cap cos_psi(psi_counter) = cos(psi*dtr); % compute sine and cosine of psi sin_psi(psi_counter) = sin(psi*dtr); % scale=scale*inv_scalediv; % reduce scale end; end; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Compute a start coordinate to get the algorithm going % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Space vehicle positions are known in the ECEF system in form of % Carthesian coordinates (X,Y,Z). The pseudoranges and tcorr are % known as well. % Initialize phi_old & lambda_old thus establishing the initial % receiver position estimate using the known Carthesian coordinates % (phi_old,lambda_old) signifies the initial point (zenith) of % the present spherical cap throughout the algorithm % compute mass mean of the SV positions and find the appropriate ECEF octant signs=sign(mean(XS')); % define a vector of signums signs(find(signs)==0)=1; % repair sign if input==0 % cos(phi) is constantly sqrt(1/2) and sin(phi) has same sign as Z % cos(lambda) has same sign as X and sin(lambda) has same sign as Y cos_phi_old = sqrt(1/2); if signs(3)==1, sin_phi_old = sqrt(1/2); else sin_phi_old = -sqrt(1/2); end; if signs(1)==1, cos_lambda_old = sqrt(1/2); else cos_lambda_old = -sqrt(1/2); end; if signs(2)==1, sin_lambda_old = sqrt(1/2); else sin_lambda_old = -sqrt(1/2); end; % Convert sine and cosine of initial position estimate to % Carthesian ECEFs. Beginning of simplified FRGEOD.M unfolded % (convert point on sphere to point on ellipsoid) % compute radius of curvature in prime vertical N_phi = semi_major_axis/sqrt(1-esq*sin_phi_old*sin_phi_old); P = N_phi*cos_phi_old; % P is distance from Z axis init_z = N_phi*(1-esq)*sin_phi_old; % initial estimate for z init_x = P*cos_lambda_old; % initial estimate for x init_y = P*sin_lambda_old; % initial estimate for y % End of simplified FRGEOD.M unfolded % Initialize Old_Sum as the squared residual for the initial receiver % position estimate m = length(pseudoranges); % find number of SVs for t = 1:m sat_clock(t) = tcorr(t)*vlight; % compute tcorrs influence on SV clocks cal_one_way(1,t) = norm(XS(:,t)-[init_x,init_y,init_z]'); % find distance to initial position one_way_res(1,t) = pseudoranges(t,2)-cal_one_way(1,t)+sat_clock(t); end; resid_t = one_way_res(1,:)-one_way_res(1,1); % compute residual for initial position Old_Sum = resid_t*resid_t'; % initialize Old_Sum for initial position %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Main part of code, with the Least-squares error search % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for iter = 1:iterations, % begining of the main loop sin_phi0 = sin_phi_old; % update sin_phi0 and cos_phi0 cos_phi0 = cos_phi_old; % for b = 1:ndiv, % begining of slicing loop (inclination) psi_index=iter*ndiv+b-1; % psi_index=iter when ndiv=1 for alpha_index = 1:alpha_counter, % begining of slicing loop (azimuth) % update grid point using spherical triangle sin_phi2 = sin_phi0*cos_psi(psi_index)+cos_phi0*sin_psi(psi_index)*cos_alpha(alpha_index); cos_phi2 = sqrt(1-sin_phi2*sin_phi2); % compute sin and cos for new possible phi if cos_phi2 == 0, % check to avoid divide by zero sin_dlambda = 0; % if cos_phi2 = 0 => sin_dlambda = 0 else % else sin_dlambda is computed like this sin_dlambda = sin_alpha(alpha_index)*sin_psi(psi_index)/cos_phi2; % a real division end; % end of divide by zero check % To avoid computation of dlambda using asin and thereafter updating lambda2 by adding % lambda_old and dlambda, we instead compute cos_dlambda by sqrt and update sin_lambda2 % using the fact that sin(A+B)=sin(A)*cos(B)+cos(A)*sin(B). The same is done for cos_lambda2 cos_dlambda=sqrt(1-sin_dlambda*sin_dlambda);