dgpslab.m

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

h=waitbar(0,'Processing the vectors Reference->Rover');

for t=1:nobs1          % Schleife 黚er alle Beobachtungen ###############################
   
clear as2 ls2z ls2 vvs2;
   
versatz=0;   

isat=0;             % number of double diff.: simultan auf beiden Stationen beob. Satelliten

used_obs2=0;        % Beob.z鋒ler f黵 die Anzahl von L2-Beob. w鋒rend 1 Epoche


for ksat=1:satmask1(t) % Schleife 黚er alle beobachteten Satelliten #####################
   
while s1c1(ksat+versatz)==0,
   versatz=versatz+1;         % fehlende Beobachtungen innerhalb einer Zeile 黚erspringen
end

% 蹷ERPR蹻EN, OB AUF S1 EMPF. SAT AUCH AUF S2 DA:

% Zu Beob. in S1C1 passende Beob. in S2C1:
rovmask=(s2epo(t,:)==s1epo(t,ksat+1+versatz));  % Maske auf Zeile in Rover (dieselbe epoche): Suche nach demselben Sat. dort

if sum(rovmask)>0         % ->>> nur wenn Sat auch auf Rover (C/A) vorhanden, weitermachen ...
   % Zur Erkl鋜ung: Von einem Satelliten wird als erstes C/A getracked, daher
   % erscheint die Satellitennummer in S1EPO und in S2EPO, sobald C/A-Beob. zu dem Sat. vorhanden!
   
rovindx=find(rovmask)-1; % an welcher Stelle in der Zeile bei S1C2 sich dieselbe Sat.nr. befindet

if mit_p2==0 | (mit_p2==1 & s2p2(t,rovindx)~=0 & s1p2(t,ksat+versatz)~=0)  
                          % ->>> nur wenn C/A-L鰏ung oder auch P2-Beob auf Ref und Rov vorhanden, weitermachen ...

isat=isat+1;       % number of double diff.: simultan auf beiden Stationen beob. Satelliten

% richtige PRN aus Eph raussuchen (einfach die erste passende aus S1EPH)
ephsatz=1;           
while s1eph(ephsatz)~=s1epo(t,ksat+1+versatz)
   ephsatz=ephsatz+24;        
end

% Koordinaten auch f黵 S2, d.h. mit denselben eph, aber mit s2c1 !!!
[s1x(t,isat),s1y(t,isat),s1z(t,isat),s1dts(t,isat),s1rel(t,isat)]=eph2xyzn(s1epoms(t,1),s1c1(t,ksat+versatz),s1eph(ephsatz:(ephsatz+23)));
[s2x(t,isat),s2y(t,isat),s2z(t,isat),s2dts(t,isat),s2rel(t,isat)]=eph2xyzn(s2epoms(t,1),s2c1(t,rovindx),s1eph(ephsatz:(ephsatz+23)));

[s1b(t,isat),s1l(t,isat),s1h(t,isat)]=xyz2blh(s1x(t,isat),s1y(t,isat),s1z(t,isat));
[s2b(t,isat),s2l(t,isat),s2h(t,isat)]=xyz2blh(s2x(t,isat),s2y(t,isat),s2z(t,isat));

% Erdrot.korr. Empf.koord: sS1_0,dOmega_e,Xe0,Ye0,Ze0 je (t,isat)(nicht indizieren, aber je s2 und s1)
%
% 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;

% Erdrot.korr. Empf.koord: sS2_0,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;

% 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);
ss2=sqrt((s2x(t,isat)-xs20)^2+(s2y(t,isat)-ys20)^2+(s2z(t,isat)-zs20)^2); % Sat.koord-Rec.koord

% Richtungskosinusse
rcs1(isat,1)=(s1x(t,isat)-xs10)/ss1;            
rcs1(isat,2)=(s1y(t,isat)-ys10)/ss1;
rcs1(isat,3)=(s1z(t,isat)-zs10)/ss1;
rcs2(isat,1)=(s2x(t,isat)-xs20)/ss2;
rcs2(isat,2)=(s2y(t,isat)-ys20)/ss2;
rcs2(isat,3)=(s2z(t,isat)-zs20)/ss2;

lss1(isat)=ss1;
lss2(isat)=ss2;         

% ---------------------A-Matrix & L-Vektor-----------------------------------------------

% Referenzsatellit: Hier m黶ste theoretisch der Auswahlalgorithmus stehen
% wenn isat=1, Bildung von REFSAT-Variablen, wenn isat>1 Bildung von A-Matrix und l-Vektor

if isat==1
refs=lss1(1)-lss2(1);	
refx=rcs2(1,1);
refy=rcs2(1,2);
refz=rcs2(1,3);
refs1indx=ksat+versatz;        % Wo Refsat in s1c1 steht
refs2indx=rovindx;             % Wo Refsat in s2c1 steht
end     % Ende des IF-Blocks [if isat==1]

% Die 黚rigen Satelliten ...

if isat>1
as2(isat-1,1)=-rcs2(isat,1)+refx;
as2(isat-1,2)=-rcs2(isat,2)+refy;
as2(isat-1,3)=-rcs2(isat,3)+refz;

if mit_p2==0         % Einfrequenzl鰏ung (C/A)
ls2z(isat-1)=lss1(isat)-lss2(isat)-refs+(s2c1(t,rovindx)-s2c1(t,refs2indx)-s1c1(t,ksat+versatz)+s1c1(t,refs1indx));
used_obs2=used_obs2+1;

elseif mit_p2==1     % ionosph鋜enfreie L鰏ung (C/A+P2 bzw. P1+P2)
   
ls2z(isat-1)=lss1(isat)-lss2(isat)-refs+ ...
   ((f2pr1*s2c1(t,rovindx)     -f2pr2*s2p2(t,rovindx))- ...
    (f2pr1*s2c1(t,refs2indx)   -f2pr2*s2p2(t,refs2indx))- ...
    (f2pr1*s1c1(t,ksat+versatz)-f2pr2*s1p2(t,ksat+versatz))+ ...
    (f2pr1*s1c1(t,refs1indx)   -f2pr2*s1p2(t,refs1indx)));
used_obs2=used_obs2+1;

else
     warndlg(['Break in program DGPSLAB.M during the check of the variable "mit_p2".',...
             ' It can only get the values "0" or "1" !' ...
             ' Wrong option setting. Please check.'], ...   
       'GPSLab: Break -  1999 zeb');
    return;

end      % Ende des IF-Blocks [mit_p2==0]


end      % Ende des IF-Blocks [if isat>1]

end      % Ende if-Block [ if mit_p2==0 | (mit_p2==1 & s2p2(t,rovindx)~=0 & s1p2(t,ksat+versatz)~=0) ]
end		% Ende if-Block [ if sum(rovmask)>0 ], also ob Sat auf beiden Stat. gemessen.
end      % Ende der Schleife ueber alle Satelliten
%------------------------------------------------------------------------------------------------

% Sind zu dieser Epoche t mindestens 4 Sat. simultan auf beiden Stationen beob. worden?
if used_obs2>2

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黵 Differenz S2 gegen黚er S1; d.h. Vektor S1->S2

ns2=(as2'*as2)\eye(3);              % invertierte Normalgleichungsmatrix
rdops2(t)=sqrt(trace(ns2));         % RDOP
condngls2(t)=rcond(ns2);            % Konditionierung der Normalgleichungsmatrix
xxs2=ns2*as2'*ls2;                  % Unbekannte

% Ausgleichungszuschl鋑e in Vektor(t) 黚ernehmen
dxs2(t)=xxs2(1);
dys2(t)=xxs2(2);
dzs2(t)=xxs2(3);


if used_obs2>3       				% 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);

elseif used_obs2==3     			% keine Redundanz

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

else
       errordlg(['Error in the program DGPSLAB.M during the calculation of the difference vectors.',...
             ' There seems to be observed less than 4 satellites or the program failed.' ...
             ' 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 f黵 S2

s2_x(t)=xs2+dxs2(t);
s2_y(t)=ys2+dys2(t);
s2_z(t)=zs2+dzs2(t);
[b_s2(t),l_s2(t),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
obs_is_used(t)=1;

else     % else zu [if used_obs2>2], d.h. wenn used_obs2<=2 :
   
obs_is_used(t)=0;
% ... nur diese eine Variable, um andere per Maske reduzieren zu k鰊nen (s.u.)

end      % Ende des IF-Blocks [if used_obs2>2]

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

% Wo gibt's eine L鰏ung ??? Also wo mehr als 3 Sat. simultan ? (isat>3)
dgpsmask=(obs_is_used~=0);

% --- Reduzierung der Felder auf die Elemente mit berechneten L鰏ungen -----------------
s1c1=s1c1(logical(dgpsmask),:);   % ...durch Herausstreichen der anderen Beobachtungen
s1epo=s1epo(logical(dgpsmask),:); % -"-
s2c1=s2c1(logical(dgpsmask),:);   % -"-
s2epo=s2epo(logical(dgpsmask),:); % -"-

s1epoms=s1epoms(logical(dgpsmask),:); % -"-
s2epoms=s2epoms(logical(dgpsmask),:); % -"-

% bisher nicht, bei Gl鋞tung n鰐ig: s1l1,s2l1
b_s2=b_s2(logical(dgpsmask));  
l_s2=l_s2(logical(dgpsmask));  
hoe_s2=hoe_s2(logical(dgpsmask));  
s2_x=s2_x(logical(dgpsmask));
s2_y=s2_y(logical(dgpsmask));
s2_z=s2_z(logical(dgpsmask));
sigmas2=sigmas2(logical(dgpsmask));
sxs2=sxs2(logical(dgpsmask));
sys2=sys2(logical(dgpsmask));
szs2=szs2(logical(dgpsmask));
dxs2=dxs2(logical(dgpsmask));
dys2=dys2(logical(dgpsmask));
dzs2=dzs2(logical(dgpsmask));
condngls2=condngls2(logical(dgpsmask));
rdops2=rdops2(logical(dgpsmask));


satmask1=satmask1(logical(dgpsmask),:);
satmask2=satmask2(logical(dgpsmask),:);


% Berechnung der Koordinatendifferenzen und der Baseline-L鋘ge

dapp_h=app2_h-app1_h;  % H鰄endifferenz aus N鋒erungswerten
strdapph=num2str(dapp_h); % H鰄endifferenz aus N鋒erungswerten (String)
app_bsl=sqrt((xs2-xs1)^2+(ys2-ys1)^2+(zs2-zs1)^2); % Bsl-L鋘ge aus N鋒erungswerten
strappbsl=num2str(app_bsl); % Bsl-L鋘ge aus N鋒erungswerten (String)

delta_h=hoe_s2-app1_h;          % H鰄endifferenzen

delta_x=dxs2;              % Diff. in X
delta_y=dys2;              % Diff. in Y
delta_z=dzs2;              % Diff. in Z
 
bsl=sqrt((s2_x-xs1).^2+(s2_y-ys1).^2+(s2_z-zs1).^2); % Zeitreihe Baseline-L鋘ge

% Mittel/Ergebnisse, Beobachtungsstatistik

satnrs=s2epo(:,2:size(s2epo,2)); % ohne 1. Spalte (Epochen), nur Sat.nummern
satnrs=satnrs(:); % Vektor aus Matrix 
satnrs=satnrs(logical(satnrs>0)); % Nullen eliminieren (Verk黵zung des Vektors)

xxs2=median(delta_x)+xs2;
yys2=median(delta_y)+ys2;
zzs2=median(delta_z)+zs2;
[bbs2,lls2,hhs2]=xyz2blh(xxs2,yys2,zzs2);

% Berechnung der Mittelwerte 黚er die Zeitreihe

m_bsl=sqrt((xxs2-xs1)^2+(yys2-ys1)^2+(zzs2-zs1)^2);
strmbsl=num2str(m_bsl); % String f黵 Plotbeschriftung (Mittel/Median der Bsl-L鋘ge)

m_delta_h=hhs2-app1_h;
strmdh=num2str(m_delta_h); % String f黵 Plotbeschriftung (Mittel/Median der H鰄endiff.)

% Statistik

sxxs2=median(sxs2);
syys2=median(sys2);
szzs2=median(szs2);

[sbbs2,slls2,shhs2,dums,dumA,dumz]=dxyz2neu(sxxs2,syys2,szzs2,bbs2,lls2);
sbbs2=abs(sbbs2);
slls2=abs(slls2);
shhs2=abs(shhs2);

sig0=median(sigmas2);

rdmin=min(rdops2);
rdmax=max(rdops2);
rdmit=mean(rdops2);

[haufig2,n2]=hist(satnrs,1:32);
n2=n2(logical(haufig2>0));
haufig2=haufig2(logical(haufig2>0));

% Flag, da?Berechnung duchgef黨rt wurde und entsprechende Variable vorliegen

dgps_is_calc=1;

% Ende  ==================================================================================
% (c) iapg 1998 Zebhauser