GPS Software Receivers - Fundamentals of Global Positioning System Receivers: A Software Approach - page 218

%p97.m Userpos.m use pseudorange and satellite positions to
calculate user position
% JT 30 April 96
function[upos] = userpos(inp);

[mm nn]=size(inp);
nsat=nn;

xquess = 0; yquess = 0; zquess = 0; tc = 0;

sp = inp(1:3,1:nn);
pr = inp(4,:);
qu(1) = xquess; qu(2) = yquess; qu(3) = zquess;
for j = 1:nsat
rn(j)=((qu(1)-sp(1,j))^2+(qu(2)-sp(2,j))^2+(qu(3)-
sp(3,j))^2)^.5;

end

rn0 = rn;
h(:,4) = ones(nsat,1);

erro = 1;
while erro > 0.01;
for j = 1:nsat;
for k = 1:3;
h(j,k) = (qu(k)-sp(k,j))/(rn(j)); %** find h

end

end
dr = pr - (rn + ones(1,nsat)*tc); %** find del pr

dl = pinv(h)*dr';
tc = tc + dl(4);
for k = 1:3;
qu(k) = qu(k) + dl(k); %** find new position

end
erro = dl(1)^2 + dl(2)^2 + dl(3)^2;
for j = 1:nsat;
rn(j)=((qu(1)-sp(1,j))^2+(qu(2)-
sp(2,j))^2+(qu(3)-sp(3,j))^2)^.5; %**new pr
end
inp = sat corr(inp', qu)'; %Correct satellite position

sp = inp(1:3,1:nn);

end
xuser = qu(1); yuser = qu(2); zuser = qu(3); bias = tc;
format long
format short
rsp = (xuser^2+yuser^2+zuser^2)^.5;
Lc = atan(zuser/(xuser^2+yuser^2)^.5);
lsp = atan(yuser/xuser)*180/pi;