spplab.m

这是国外关于卫星导航方面一书的源代码

%
% ber. range je Satellit fuer beide Punkte
rs1=sqrt((s1x(t,isat)-xs1)^2+(s1y(t,isat)-ys1)^2+(s1z(t,isat)-zs1)^2);
% Winkel aus Erdrotation
dos1=rs1*omega_e/c0;
% um Erdrot. und um Antennenh鰄e korr. Empf.koord
xs10=xs1*cos(dos1)-ys1*sin(dos1)+ants1dx;
ys10=ys1*cos(dos1)+xs1*sin(dos1)+ants1dy;
zs10=zs1+ants1dz;

% Korrektur wg. Erdrot. 
xerks1(t,isat)=xs10-xs1;
yerks1(t,isat)=ys10-ys1;

% Ausgleichungsansatz:
% Verbesserungsgleichung (Zeile A-Matrix & Absolutglied): Ss2,w,A*x (t,isat)
% (nur w und A bzgl. Sat indizieren, Rest wird nicht wieder gebraucht, allerdings je s2 und s1)
ss1=sqrt((s1x(t,isat)-xs10)^2+(s1y(t,isat)-ys10)^2+(s1z(t,isat)-zs10)^2);
as1(isat,1)=-(s1x(t,isat)-xs10)/ss1;
as1(isat,2)=-(s1y(t,isat)-ys10)/ss1;
as1(isat,3)=-(s1z(t,isat)-zs10)/ss1;
as1(isat,4)=-1;                                  % eigtl. -c0  so erh鋖t man dt_e auf [m] statt [s]; notwendig zu akzeptabler Konditionierung des NGlSys
ls1z(isat)=s1c1(t,isat+versatz)-ss1+s1dts(t,isat)*c0;

end      % Ende der Schleife ueber alle Satelliten
%------------------------------------------------------------------------------------------------

ls1=ls1z';                                      % Zeilen- in Spaltenvektor

% Ausgleichung: dXe,dYe,dZe,dte,s0,sigmas(=sqrt(diag(s0*inv(a'pa)))),GDOP(=sqrt(trace(inv(a'pa))))
% F黵 S1

ns1=(as1'*as1)\eye(4);             		% Normalgleichungsmatrix
gdops1(t)=sqrt(trace(ns1));			% GDOP
condngls1(t)=rcond(ns1);			% Konditionierung der Normalgleichungsmatrix
xxs1=ns1*as1'*ls1;				% Unbekannte

% Korrektur wegen Antennenh鰄e (also Red. von Antennenphasenzentrum auf Punktmarke)
dxs1(t)=xxs1(1)-ants1dx;
dys1(t)=xxs1(2)-ants1dy;
dzs1(t)=xxs1(3)-ants1dz;
dts1(t)=xxs1(4);

if satmask1(t)>4      % Nur wenn Redundanz vorhanden, Genauigkeitsma遝 berechnen 
vvs1=as1*xxs1-ls1;				% Residuen
sigs1=vvs1'*vvs1/(size(ls1,1)-size(xxs1,1));	% (sigma_0)^2
kxxs1=sigs1*ns1;				% Kovarianzmatrix der Unbekannten
sigxxs1=sqrt(diag(kxxs1));			% Standardabweichungen der Unbekannten
sigmas1(t)=sqrt(sigs1);				% sigma_0
sxs1(t)=sigxxs1(1);sys1(t)=sigxxs1(2);szs1(t)=sigxxs1(3);sts1(t)=sigxxs1(4);

elseif satmask1(t)==4   % keine Redundanz
sxs1(t)=0;sys1(t)=0;szs1(t)=0;sts1(t)=0;sigmas1(t)=0;

else
       errordlg(['error in SPPLAB.M whilke calculating the positions.',...
             ' It seems that less than 4 satellites are observed or a program error appeared.' ...
             ' Check S1EPO in comparison to S1C1 and check the program.'], ...   
       'GPSLab: break -  1998 zeb');
       return;
end   % Ende if-Block Redundanz

% Geoz.kart. Koord. und H鰄en

s1_x(t)=xs1+dxs1(t);
s1_y(t)=ys1+dys1(t);
s1_z(t)=zs1+dzs1(t);
[dumb,duml,hoe_s1(t)]=xyz2blh(s1_x(t),s1_y(t),s1_z(t));

% wir erhalten: gdop,sigma,condngl,dx,dy,dz,dt,sx,sy,sz,st

%------------------------------------------------------------------------------------------------
waitbar(t/nobs1);   
end      % Ende der Schleife ueber alle Beobachtungen nobs1
%------------------------------------------------------------------------------------------------
close(h);



% ACHTUNG f黵 S2 (ROV)
% Berechnung der Position aller Sat. fuer 2 Stationen und alle Epochen  ------------------&
% Berechnung der Orbithoehen ueber Grund aller Sat. fuer 2 Stat. fuer alle Epochen  -------
% Korrektion der Empfaengerkoordinaten wegen Erdrotation  ---------------------------------
% Berechnung der Empfaengerpositionen fuer 2 Stationen ueber alle Epochen  ----------------
% ---> wie gross ist die Variation  gg.ueber den Naeherungskoord. (RINEX Header)
% Berechnung der Koord.unterschiede ueber alle Epochen  -----------------------------------
% ---> wie gross ist die Variation gg.ueber Naeherungskoord.differenz (RINEX-Header)

h=waitbar(0,'calculating the positions of the rover station');

for t=1:nobs2          % Schleife 黚er alle Beobachtungen
   
clear as2 ls2z ls2 vvs2;
   
versatz=0;   
for isat=1:satmask2(t) % Schleife 黚er alle beobachteten Satelliten
while s2c1(isat+versatz)==0, 
   versatz=versatz+1;         % fehlende Beobachtungen innerhalb einer Zeile 黚erspringen
end

ephsatz=1;
while s2eph(ephsatz)~=s2epo(t,isat+1+versatz)
   ephsatz=ephsatz+24;        % PRN raussuchen (einfach die erste passende)
end

[s2x(t,isat),s2y(t,isat),s2z(t,isat),s2dts(t,isat),s2rel(t,isat)]=eph2xyzn(s2epo(t,1),s2c1(t,isat+versatz),s2eph(ephsatz:(ephsatz+23)));
[s2b(t,isat),s2l(t,isat),s2h(t,isat)]=xyz2blh(s2x(t,isat),s2y(t,isat),s2z(t,isat));
% Erdrot.korr. Empf.koord: Ss20,dOmega_e,Xe0,Ye0,Ze0 je (t,isat)(nicht indizieren, aber je s2 und s1)
%
% ber. range je Satellit fuer beide Punkte
rs2=sqrt((s2x(t,isat)-xs2)^2+(s2y(t,isat)-ys2)^2+(s2z(t,isat)-zs2)^2); % Sat.koord-Rec.koord
% Winkel aus Erdrotation
dos2=rs2*omega_e/c0;                                                   % [m*rad/s/(m/s)=rad]
% um Erdrot. und um Antennenh鰄e korr. Empf.koord
xs20=xs2*cos(dos2)-ys2*sin(dos2)+ants2dx;
ys20=ys2*cos(dos2)+xs2*sin(dos2)+ants2dy;
zs20=zs2+ants2dz;

% Korrektur wg. Erdrot. 
xerks2(t,isat)=xs20-xs2;
yerks2(t,isat)=ys20-ys2;

% Ausgleichungsansatz:
% Verbesserungsgleichung (Zeile A-Matrix & Absolutglied): Ss2,w,A*x (t,isat)
% (nur w und A bzgl. Sat indizieren, Rest wird nicht wieder gebraucht, allerdings je s2 und s1)
ss2=sqrt((s2x(t,isat)-xs20)^2+(s2y(t,isat)-ys20)^2+(s2z(t,isat)-zs20)^2); % Sat.koord-Rec.koord
as2(isat,1)=-(s2x(t,isat)-xs20)/ss2;             % A-Matrix
as2(isat,2)=-(s2y(t,isat)-ys20)/ss2;
as2(isat,3)=-(s2z(t,isat)-zs20)/ss2;
as2(isat,4)=-1;                                  % eigtl. -c0; so erh鋖t man dt_e auf [m] statt [s]; notwendig zu akzeptabler Konditionierung des NGlSys
ls2z(isat)=s2c1(t,isat+versatz)-ss2+s2dts(t,isat)*c0;    % Widerspruchsvektor

end      % Ende der Schleife ueber alle Satelliten
%------------------------------------------------------------------------------------------------

ls2=ls2z';                                      % Zeilen- in Spaltenvektor

% Ausgleichung: dXe,dYe,dZe,dte,s0,sigmas(=sqrt(diag(s0*inv(a'pa)))),GDOP(=sqrt(trace(inv(a'pa))))
% F黵 s2

ns2=(as2'*as2)\eye(4);                     		% Normalgleichungsmatrix
gdops2(t)=sqrt(trace(ns2));			% GDOP
condngls2(t)=rcond(ns2);			% Konditionierung der Normalgleichungsmatrix
xxs2=ns2*as2'*ls2;				% Unbekannte

% Korrektur wegen Antennenh鰄e (also Red. von Antennenphasenzentrum auf Punktmarke)
dxs2(t)=xxs2(1)-ants2dx;
dys2(t)=xxs2(2)-ants2dy;
dzs2(t)=xxs2(3)-ants2dz;
dts2(t)=xxs2(4);

if satmask2(t)>4     % Nur wenn Redundanz vorhanden, Genauigkeitsma遝 berechnen 
vvs2=as2*xxs2-ls2;				% Residuen
sigs2=vvs2'*vvs2/(size(ls2,1)-size(xxs2,1));	% (sigma_0)^2
kxxs2=sigs2*ns2;				% Kovarianzmatrix der Unbekannten
sigxxs2=sqrt(diag(kxxs2));			% Standardabweichungen der Unbekannten
sigmas2(t)=sqrt(sigs2);				% sigma_0
sxs2(t)=sigxxs2(1);sys2(t)=sigxxs2(2);szs2(t)=sigxxs2(3);sts2(t)=sigxxs2(4);

elseif satmask2(t)==4   % keine Redundanz

sxs2(t)=0;sys2(t)=0;szs2(t)=0;sts2(t)=0;sigmas2(t)=0;

else
       errordlg(['error in SPPLAB.M whilke calculating the positions.',...
             ' It seems that less than 4 satellites are observed or a program error appeared.' ...
             ' Check S2EPO in comparison to S2C1 and check the program.'], ...   
       'GPSLab: break -  1998 zeb');
       return;
end   % Ende if-Block Redundanz

% Geoz.kart. Koord. und H鰄en

s2_x(t)=xs2+dxs2(t);
s2_y(t)=ys2+dys2(t);
s2_z(t)=zs2+dzs2(t);
[dumb,duml,hoe_s2(t)]=xyz2blh(s2_x(t),s2_y(t),s2_z(t));

% wir erhalten: gdop,sigma,condngl,dx,dy,dz,dt,sx,sy,sz,st

%------------------------------------------------------------------------------------------------
waitbar(t/nobs2);   
end      % Ende der Schleife ueber alle Beobachtungen nobs2
%------------------------------------------------------------------------------------------------
close(h);

% Koordinaten-DGPS
% ----------------

% --- Runden der Epochen auf n鋍hste Integer, Warnung, wenn diff>10 msec !!
%     Verdacht auf unganzzahlige Epochen wie halbe Sekunden o.