Basic GPS Concept - Fundamentals of Global Positioning System Receivers: A Software Approach - page 28

%p21.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

x 1 y 1 z 1

x 2 y 2 z 2

%sp= x 3 y 3 z 3

···
x nn

y nn z nn

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