/*Umgebungsvariable setzen*/
ratprint:false;

ratmx:true;

lmxchar:"["$
rmxchar:"]"$

/*3D-Grafik einbinden*/
load(draw);

/*Paket fuer die Newton-Naeherung laden*/
load(mnewton);

/*Konstanten*/
v_light:299792458;

/*Zuordnung PRN-Codes zu den Satelliten*/
SV_PRN:[[2,6],[3,7],[4,8],[5,9],[1,9],[2,10],[1,8],[2,9],[3,10],[2,3],
[3,4],[5,6],[6,7],[7,8],[8,9],[9,10],[1,4],[2,5],[3,6],[4,7],[5,8],
[6,9],[1,3],[4,6],[5,7],[6,8],[7,9],[8,10],[1,6],[2,7],[3,8],[4,9]];


/*Kapitel 2: Verwendung von Maxima*/

/*Bogenmaß berechnen*/
bogen(grad):=grad*%pi/180;

/*Gradmaß berechnen*/
grad(bogen):=bogen*180/%pi;

/*Hauptwert berechnen*/
hauptwert(X):=float(mod(X+2*%pi,2*%pi));

/*GPS-Zeit vor Über- und Unterlauf schützen*/
check_t(sek):=
if sek>302400 then sek-604800
elseif sek<-302400 then sek+604800
else sek;


/*Berechnung der Differenz-Norm*/
norm(V1,V2):=sqrt((V1[1]-V2[1])^2+(V1[2]-V2[2])^2+(V1[3]-V2[3])^2);

/*Entfernung zwischen zwei in Polarkoordinaten angegebenen Positionen*/
entfernung(pos1,pos2):=block(
[phi1,lambda1,r1,phi2,lambda2,r2,x1,y1,z1,x2,y2,z2,p1,p2],
[phi1,lambda1,r1]:pos1,
[phi2,lambda2,r2]:pos2,
z1:[(6378137+r1)*sin(bogen(phi1))],
x1:[(6378137+r1)*cos(bogen(phi1))*cos(bogen(lambda1))],
y1:[(6378137+r1)*cos(bogen(phi1))*sin(bogen(lambda1))],
p1:matrix(x1,y1,z1),
z2:[(6378137+r2)*sin(bogen(phi2))],
x2:[(6378137+r2)*cos(bogen(phi2))*cos(bogen(lambda2))],
y2:[(6378137+r2)*cos(bogen(phi2))*sin(bogen(lambda2))],
p2:matrix(x2,y2,z2),
norm(p1,p2));

/*URL für Google-Maps erstellen*/
make_url(pos):=block(
[url,nord,east],
url:"https://www.google.de/maps/place/",
nord:string(pos[1]),
east:string(pos[2]),
concat(url,nord,",",east));


/*Kapitel 3: NAVSTAR GPS*/


/*Kapitel 4: Zeitangaben im GPS-System*/

/*Julianisches Datum am letzten Februartag eines Jahres um 0:00 Uhr*/
jdy(year):=floor(365.25*(year-1900))+2415078.5;

/*Umorganisation der Monate*/
jdf(y,m):=if m<3 then [y-1,m+12] else [y,m];

/*Tagesanzahl der vollständigen Monate*/
jdm(m):=floor(30.61*(m+1))-122;

/*Julianisches Datum berechnen*/
julday(year,m,day,h,min,sec):=block(
[year,m]:jdf(year,m),
(jdy(year)+jdm(m)+day+h/24+min/1440+sec/86400));

/*GPS-Woche berechnen*/
gps_week(jd):=floor((jd-2444244.5)/7);

/*GPS-Wochentag berechnen*/
wotag(jd):=mod(floor(jd-2444244.5),7);

/*GPS-Wochensekunde berechnen*/
sow(jd):=block(
[tbt],
tbt:mod(jd+0.5,1),
round((tbt+wotag(jd))*86400) 
);

/*GPS-Zeit ermitteln*/
gps_time(jd):=[gps_week(jd),sow(jd)];


/*Kapitel 5: Datenübertragung und Entfernungsbestimmung*/

/*Zuordnung PRN-Codes zu den Satelliten*/
SV_PRN:[[2,6],[3,7],[4,8],[5,9],[1,9],[2,10],[1,8],[2,9],[3,10],[2,3],
[3,4],[5,6],[6,7],[7,8],[8,9],[9,10],[1,4],[2,5],[3,6],[4,7],[5,8],
[6,9],[1,3],[4,6],[5,7],[6,8],[7,9],[8,10],[1,6],[2,7],[3,8],[4,9]];

/*Schieben G1-Register*/
shift_G1():=block(
[input,erg],
input:mod(G1[3]+G1[10],2),
erg:G1[10],
push(input,G1),
G1:rest(G1,-1),
erg
);

/*Chipmuster des ersten Registers erzeugen*/
make_prn1():=block(
[prn1,chip],
prn1:[],
G1:[1,1,1,1,1,1,1,1,1,1],
for i:1 thru 1023 do
(
    chip:shift_G1(),
    prn1:endcons(chip,prn1)
),
prn1);

/*Schieben G2-Register*/
shift_G2(e1,e2):=block(
[input,erg],
input:mod(G2[2]+G2[3]+G2[6]+G2[8]+G2[9]+G2[10],2),
erg:mod(G2[e1]+G2[e2],2),
push(input,G2),
G2:rest(G2,-1),
erg);

/*Chipfolge des zweiten Registers erzeugen*/
make_prn2(SV):=block(
[e1,e2,prn2,chip],
[e1,e2]:SV_PRN[SV],
prn2:[],
G2:[1,1,1,1,1,1,1,1,1,1],
for i:1 thru 1023 do
(
    chip:shift_G2(e1,e2),
    prn2:endcons(chip,prn2)
),
prn2);

/*Die ersten 50 Chips des zweiten Registers erzeugen*/
make_prn2_50(SV):=block(
[e1,e2,prn2,chip],
[e1,e2]:SV_PRN[SV],
prn2:[],
G2:[1,1,1,1,1,1,1,1,1,1],
for i:1 thru 50 do
(
    chip:shift_G2(e1,e2),
    prn2:endcons(chip,prn2)
),
prn2);

/*Mustervergleich des zweiten Registers*/
check_prn2():=block(
for i:1 thru 32 do
(
    G2:[1,1,1,1,1,1,1,1,1,1],
    print(i,make_prn2_50(i))
));

/*Vollständige PRN-Sequenz erzeugen*/
make_prn(SV):=block(
[e1,e2,prn_liste],
[e1,e2]:SV_PRN[SV],
prn_liste:[],
G1:[1,1,1,1,1,1,1,1,1,1],
G2:[1,1,1,1,1,1,1,1,1,1],
for i:1 thru 1023 do
(
    chip:mod(shift_G1()+shift_G2(e1,e2),2),
    prn_liste:endcons(chip,prn_liste)
),
prn_liste
);

/*Oktale Darstellung der ersten 10 Chips*/
oktal(liste):=block(
[s1,s2,s3,s4],
s1:string(first(liste)),
s2:string(liste[2]*4+liste[3]*2+liste[4]),
s3:string(liste[5]*4+liste[6]*2+liste[7]),
s4:string(liste[8]*4+liste[9]*2+liste[10]),
concat(s1,s2,s3,s4));

/*Chipsumme erstellen*/
chipsumme(liste):=block(
[summe],
summe:0,
for i in liste do summe:summe+i,
summe);

/*Schieben B1-Register der Länge 5*/
shift_B1():=block(
[input,erg],
input:mod(B1[3]+B1[5],2),
erg:B1[5],
push(input,B1),
B1:rest(B1,-1),
erg);

/*Sequenz B1-Register mit 31 Chips erstellen*/
make_bsp1():=block(
[bsp1,chip],
bsp1:[],
B1:[1,1,1,1,1],
for i:1 thru 31 do
(
    chip:shift_B1(),
    bsp1:endcons(chip,bsp1)
),
bsp1);

/*Schieben B2-Register der Länge 5*/
shift_B2(e1,e2):=block(
[input,erg],
input:mod(B2[1]+B2[2]+B2[3]+B2[5],2),
erg:mod(B2[e1]+B2[e2],2),
push(input,B2),
B2:rest(B2,-1),
erg);

/*Sequenz B2-Register mit 31 Chips erstellen*/
make_bsp2(e1,e2):=block(
[bsp2,chip],
bsp2:[],
B2:[1,1,1,1,1],
for i:1 thru 31 do
(
    chip:shift_B2(e1,e2),
    bsp2:endcons(chip,bsp2)
),
bsp2);

/*Gold-Codes der Länge 31 erstellen*/
make_bsp(e1,e2):=block(
[bsp_liste],
bsp_liste:[],
B1:[1,1,1,1,1],
B2:[1,1,1,1,1],
for i:1 thru 31 do
(
    chip:mod(shift_B1()+shift_B2(e1,e2),2),
    bsp_liste:endcons(chip,bsp_liste)
),
bsp_liste
);

/*Liste um n Stellen zyklisch weiterschieben*/
shift(liste,n):=block(
for i:1 thru n do
(
    push(last(liste),liste),
    liste:rest(liste,-1)
),
liste
);

/*Anzahl der übereinstimmenden Eins-Werte in zwei Listen feststellen*/
korrelation(liste1,liste2):=block(
[summe],
summe:0,
for i:1 thru length(liste1) do
    summe:summe+liste1[i]*liste2[i],
summe
);

/*Autokorrelation zweier Listen feststellen*/
autokorr(recliste,satliste):=block(
[sum,sum_alt,merker],
sum:0,
sum_alt:0,
for i:1 thru length(recliste) do
(
    sum:korrelation(recliste,satliste),
    if sum>sum_alt then
    (
        sum_alt:sum,
        merker:i
    ),
    recliste:shift(recliste,1)
),
merker-1
);

/*Empfangssignal erzeugen*/
make_sum([satliste]):=block(
[summenliste],
summenliste:makelist(0,n,1,length(satliste[1])),
for i in satliste do summenliste:summenliste+i,
summenliste
);


/*Kapitel 6: GPS-Empfänger und Rohdaten*/

/*RINEX-Datei einlesen*/
read_rinex(pfad):=block(
[h,zeile,werte],
h:openr(pfad),
zeile:split(readline(h)),
unless member("END",zeile) and member("OF",zeile) and
        (member("HEADER",zeile) or member(concat("HEADER",ascii(13)),zeile)) 
        do zeile:split(readline(h)),
werte:read_nested_list(h),
close(h),
werte);


/*Kapitel 7: RINEX-Dateien einbinden*/

/*Ephemeridenmatrix erzeugen*/
make_navmatrix(rinex_navdata):=block(
[num_sv,eph_sv,svlist],
navmatrix:[],
svlist:[],
num_sv:length(rinex_navdata)/8,
for i:1 step 1 thru num_sv do
   (
   eph_sv:[],
   for k:1 thru 8 do
       (
        if k=1 then
            if not(numberp(rinex_navdata[k][1])) then
                (
                chr:string(first(rinex_navdata[k])),
                sv:eval_string(sremove("G",chr)),
                rinex_navdata[k][1]:sv
                ),
       eph_sv:append(eph_sv,pop(rinex_navdata))
       ),
   navmatrix:endcons(eph_sv,navmatrix),
   svlist:endcons(navmatrix[i][1],svlist)
   ),
print("Anzahl Ephemeriden:" ,num_sv,sort(svlist)),
navmatrix
);

/*Überblick über die Satelliten in der navmatrix*/
make_navsatlist():=block(
[navsatlist],
navsatlist:[],
for i:1 thru length(navmatrix) do navsatlist:endcons(navmatrix[i][1],navsatlist),
sort(navsatlist)
);

/*Zeiger auf den Eph-Datensatz eines Satelliten ermitteln*/
find_ephindex(sat_nr):=block(
[index],
index:0,
for i:1 thru length(navmatrix) do
    if navmatrix[i][1]=sat_nr then index:i,
index);

/*Satellitennummer auslesen*/
read_satnr(satliste,lfd_nr):=block(
[index,zeichenkette,satnr,z,e,navsatlist],
index:(lfd_nr-1)*3+1,
zeichenkette:string(satliste),
satnr:if charat(zeichenkette,index)="G" then
    (
    z:eval_string(charat(zeichenkette,index+1)),
    e:eval_string(charat(zeichenkette,index+2)),
    z*10+e
    )
    else 0,
navsatlist:make_navsatlist(),
if member(satnr,navsatlist) then satnr else 0
);

/*Pseudoentfernung auslesen*/
read_prange_rinex(rinex_obsdata,cluster,i):=block(
[prange],
prange:rinex_obsdata[cluster+i][2],
if prange<10 then prange:rinex_obsdata[cluster+i][3],
prange
);

/*Beobachtungszeit aus dem Header in GPS-Zeit umwandeln*/
read_obstime(cluster):=
gps_time(julday(cluster[1]+2000,cluster[2],cluster[3],
	cluster[4],cluster[5],cluster[6]));


/*Liste mit den Pseudoranges erstellen*/
make_obslist(rinex_obsdata,cluster):=block(
[header,num_sats,satlist,time,rangelist],
header:rinex_obsdata[cluster],
num_sats:header[8],
satlist:header[9],
time:read_obstime(header),
rangelist:[time,num_sats],
for i:1 thru num_sats do
    (
    if read_satnr(satlist,i)#0 then
        rangelist:endcons(
            [read_satnr(satlist,i),
            read_prange_rinex(rinex_obsdata,cluster,i)],
            rangelist)
    ),
rangelist[2]:length(rangelist)-2,
rangelist);

/*Matrix mit allen beobachteten Pseudoranges erstellen*/
make_obsmatrix(rinex_obsdata):=block(
[z_nr,zeilenzahl,ranges_at_time,num_sats,svlist],
obsmatrix:[],
z_nr:1,
zeilenzahl:length(rinex_obsdata),
while z_nr<zeilenzahl do
    (
    ranges_at_time:make_obslist(rinex_obsdata,z_nr),
    obsmatrix:endcons(ranges_at_time,obsmatrix),
    num_sats:rinex_obsdata[z_nr][8],
    z_nr:z_nr+num_sats+1
    ),
svlist:[],
print(length(obsmatrix),"Beobachtungen"),
for obs in obsmatrix do
(
	for i:3 thru length(obs) do
		svlist:endcons(obs[i][1],svlist),
	print(obs[1][2],sort(svlist)),
	svlist:[]
),
obsmatrix);

/*Dateinamen einlesen*/
read_name():=block(
[name,obsname,navname],
name:(read("Namensstamm der RINEX-Dateien:")),
navname:concat(name,"_nav.txt"),
obsname:concat(name,"_obs.txt"),
[navname,obsname]);

/*Pfade erstellen*/
make_path(name):=block(
pfad:"/users/emu/gps/",
navpfad:concat(pfad,name[1]),
obspfad:concat(pfad,name[2]),
[navpfad,obspfad]);

/*RINEX-Dateien einlesen und globale Datenstrukturen erstellen*/
einlesen():=block(
name:read_name(),
pfad:make_path(name),
rinex_nav:read_rinex(pfad[1]),
make_navmatrix(rinex_nav),
rinex_obs:read_rinex(pfad[2]),
make_obsmatrix(rinex_obs),
return(true));

/*Liste der Beobachtungszeiten erstellen*/
make_timelist():=block(
[timeline],
timeline:[],
for i:1 thru length(obsmatrix) do
    timeline:endcons(obsmatrix[i][1][2],timeline),
timeline);

/*Zeiger auf einen Beobachtungsdatensatz anhand der Zeit ermitteln*/
find_obsindex(time):=block(
[index],
index:0,
for i:1 thru length(obsmatrix) do
    if obsmatrix[i][1][2]=time then index:i,
index
);

/*Nummern derjenigen Satelliten ermitteln, von denen Beobachtungsdaten vorliegen*/
read_avail_sats(time):=block(
[obs_index,satlist],
obs_index:find_obsindex(time),
satlist:[],
for i:3 thru length(obsmatrix[obs_index]) do
     satlist:endcons(obsmatrix[obs_index][i][1],satlist),
sort(satlist));

/*Pseudorange eines Sat. zu einer best. Zeit auslesen*/
read_prange(sow,sv_nr):=block(
[obs_index],
obs_index:find_obsindex(sow),
for i:3 thru length(obsmatrix[obs_index]) do
    if obsmatrix[obs_index][i][1]=sv_nr then
         return(obsmatrix[obs_index][i][2]));


/*Kapitel 7 (Ergänzung): Routinen für durch RTKCONV generierte RINEX-Dateien*/

/*Beobachtungszeit erfassen*/
read_obstime8(zeile):=
gps_time(julday(zeile[2],zeile[3],zeile[4],
	zeile[5],zeile[6],zeile[7]));

/*Satellitennummer überprüfen*/
read_satnr8(eintrag):=block(
[zeichenkette,z,e,navsatlist],
zeichenkette:string(eintrag),
satnr:if charat(zeichenkette,1)="G" then
    (
    z:eval_string(charat(zeichenkette,2)),
    e:eval_string(charat(zeichenkette,3)),
    z*10+e
    )
    else 0,
navsatlist:make_navsatlist(),
if member(satnr,navsatlist) then satnr else 0);

/*Beobachtungsliste erstellen*/
make_obslist8(rinex_obsdata,z_nr):=block(
[satanzahl,beobachtungen,sat_nr],
satanzahl:rinex_obs[z_nr][9],
time:read_obstime8(rinex_obsdata[z_nr]),
beobachtungen:[time,satanzahl],
for i:z_nr+1 thru z_nr+satanzahl do
    (
    sat_nr:read_satnr8(rinex_obsdata[i][1]),
    prange:rinex_obsdata[i][2],
    if sat_nr#0 then
        beobachtungen:endcons([sat_nr,prange],beobachtungen)
    ),
beobachtungen
);

/*Beobachtungsmatrix erstellen*/
make_obsmatrix8(rinex_obsdata):=block(
[z_nr,ranges_at_time,num_sats,svlist],
obsmatrix:[],
z_nr:1,
while z_nr<length(rinex_obsdata) do
    (
    ranges_at_time:make_obslist8(rinex_obsdata,z_nr),
    obsmatrix:endcons(ranges_at_time,obsmatrix),
    num_sats:rinex_obsdata[z_nr][9],
    z_nr:z_nr+num_sats+1
    ),
svlist:[],
print(length(obsmatrix),"Beobachtungen"),
for obs in obsmatrix do
    (
    for i:3 thru length(obs) do
    svlist:endcons(obs[i][1],svlist),
    print(obs[1][2],sort(svlist)),
    svlist:[]
    ),
obsmatrix);

/*Angepasste Funktion zum Einlesen der Daten*/
einlesen8():=block(
name:read_name(),
pfad:make_path(name),
rinex_nav:read_rinex(pfad[1]),
make_navmatrix(rinex_nav),
rinex_obs:read_rinex(pfad[2]),
make_obsmatrix8(rinex_obs),
return(true));


/*Kapitel 8: Beschreibung von Satellitendaten*/

/*Ephemeridendaten darstellen*/
show_eph(sv):=block(
[k],
k:find_ephindex(sv),
if k=0 then (print("SV:",sv,"nicht vorhanden"),return()),
print("       SV: ",navmatrix[k][1]),
print("     Date: ",navmatrix[k][4],".",navmatrix[k][3],".",navmatrix[k][2]),
print("     Time: ",navmatrix[k][5],":",navmatrix[k][6],":",navmatrix[k][7]),
print("      af0: ",navmatrix[k][8]),
print("      af1: ",navmatrix[k][9]),
print("      af2: ",navmatrix[k][10]),
print("     IODE: ",navmatrix[k][11]),
print("      crs: ",navmatrix[k][12]),
print("  delta_n: ",navmatrix[k][13]),
print("       M0: ",navmatrix[k][14]),
print("      cuc: ",navmatrix[k][15]),
print("      ecc: ",navmatrix[k][16]),
print("      cus: ",navmatrix[k][17]),
print("   sqrt_A: ",navmatrix[k][18]),
print("      toe: ",navmatrix[k][19]),
print("      cic: ",navmatrix[k][20]),
print("  OMEGA_0: ",navmatrix[k][21]),
print("      cis: ",navmatrix[k][22]),
print("       i0: ",navmatrix[k][23]),
print("      crc: ",navmatrix[k][24]),
print("    omega: ",navmatrix[k][25]),
print("OMEGA_dot: ",navmatrix[k][26]),
print("    i_dot: ",navmatrix[k][27]),
print("    codes: ",navmatrix[k][28]),
print(" gps_week: ",navmatrix[k][29]),
print("  L2_flag: ",navmatrix[k][30]),
print(" sv_accur: ",navmatrix[k][31]),
print("sv_health: ",navmatrix[k][32]),
print("      tgd: ",navmatrix[k][33]),
print("     iodc: ",navmatrix[k][34]),
print("      tom: ",navmatrix[k][35]),
print(" interval: ",navmatrix[k][36]).
return(true));


/*Kapitel 9: Bestimmung der Satellitenposition*/

/*Ephemeriden in globale Variablen übertragen*/
set_eph(sv):=block(
[k],
k:find_ephindex(sv),
if k=0 then return(),
SV:navmatrix[k][1],
af0:navmatrix[k][8],
af1:navmatrix[k][9],
af2:navmatrix[k][10],
crs:navmatrix[k][12],
delta_n:navmatrix[k][13],
M0:navmatrix[k][14],
cuc:navmatrix[k][15],
ecc:navmatrix[k][16],
cus:navmatrix[k][17],
sqrt_A:navmatrix[k][18],
toe:navmatrix[k][19],
cic:navmatrix[k][20],
OMEGA_0:navmatrix[k][21],
cis:navmatrix[k][22],
i0:navmatrix[k][23],
crc:navmatrix[k][24],
omega:navmatrix[k][25],
OMEGA_dot:navmatrix[k][26],
i_dot:navmatrix[k][27],
return(true));

/*Bestimmung der Satellitenposition*/
satpos(sow,sv):=block(
[SV,af0,af1,af2,crs,delta_n,M0,cuc,ecc,cus,sqrt_A,toe,cic,OMEGA_0,cis,i0,crc,
omega,OMEGA_dot,i_dot,
GM,OMEGAe_dot,A,tk,n0,n,M,E,dE,E_old,v,Phi,u,r,i, x1,y1,OMEGA,sat_x,sat_y,sat_z],
GM:3.986005e14,
OMEGAe_dot:7.2921151467e-5,
set_eph(sv),
A:sqrt_A*sqrt_A,
tk:check_t(sow-toe),
n0:sqrt(GM/A^3),
n:n0+delta_n,
M:M0+n*tk,
M:hauptwert(M),
E:M,
dE:1,
for i:1 thru 10 unless abs(dE)<1*10^-12 do
    (
    E_old:E,
    E:M+ecc*sin(E),
    dE:(mod(E-E_old,2*%pi))
    ),
E:hauptwert(E),
v:atan2(sqrt(1-ecc^2)*sin(E), cos(E)-ecc),
Phi:v+omega,
Phi:hauptwert(Phi),
u:Phi             +cus*sin(2*Phi)+cuc*cos(2*Phi),
r:A*(1-ecc*cos(E))+crs*sin(2*Phi)+crc*cos(2*Phi),
i:i0+i_dot*tk     +cis*sin(2*Phi)+cic*cos(2*Phi),
x1:cos(u)*r,
y1:sin(u)*r,
OMEGA:OMEGA_0+(OMEGA_dot-OMEGAe_dot)*tk-OMEGAe_dot*toe,
OMEGA:hauptwert(OMEGA),
sat_x:x1*cos(OMEGA)-y1*cos(i)*sin(OMEGA),
sat_y:x1*sin(OMEGA)+y1*cos(i)*cos(OMEGA),
sat_z:y1*sin(i),
[sat_x,sat_y,sat_z]
)$

/*Bestimmung der Satellitenposition in Polarkoordinaten*/
satpos_polar(sow,sv):=block(
[ecef,polar],
ecef:satpos(sow,sv),
polar:ecef_polar_grad(ecef),
float([round(polar[1]*10)/10,round(polar[2]*10)/10])
);

/*Kapitel 10: Almanach*/

/*Almanach Textdatei einlesen*/
read_almanach(pfad):=block(
[h,almanach_txt],
h:openr(pfad),
almanach_txt:read_nested_list(h),
close(h),
almanach_txt
);

/*Matrix mit den Almanachdaten erstellen*/
make_almatrix(alm_txt):=block(
[sv,ecc,time,i0,OMEGA_dot,sqrt_A,OMEGA_0,omega,M0,
af0,af1,almlist,svlist],
almatrix:[],
svlist:[],
for i:1 thru length(alm_txt) step 15 do
(
    sv:last(alm_txt[i+1]),
    ecc:last(alm_txt[i+3]),
    time:last(alm_txt[i+4]),
    i0:last(alm_txt[i+5]),
    OMEGA_dot:last(alm_txt[i+6]),
    sqrt_A:last(alm_txt[i+7]),
    OMEGA_0:last(alm_txt[i+8]),
    omega:last(alm_txt[i+9]),
    M0:last(alm_txt[i+10]),
    af0:last(alm_txt[i+11]),
    af1:last(alm_txt[i+12]),
    almlist:[sv,ecc,time,i0,OMEGA_dot,sqrt_A,OMEGA_0,omega,M0,af0,af1],
    almatrix:endcons(almlist,almatrix),
    svlist:endcons(sv,svlist)
),
print("Im Almanach vertretene Satelliten:"),
print(sort(svlist)),
almatrix);

/*Almanach komfortabel einlesen*/
almanach_einlesen():=block(
[alm_name_list,alm_name,almanach_txt],
alm_name_list:[],
alm_name:read("Namensstamm der Almanach-Datei:"),
pfad:concat("/users/emu/gps/",alm_name,"_alm.txt"),
almanach_txt:read_almanach(pfad),
make_almatrix(almanach_txt),
return(true));

/*Almanachdaten eines bestimmten Satelliten finden*/
find_almanach(sv):=block(
[index],
index:0,
for i:1 thru length(almatrix) do
    if almatrix[i][1]=sv then index:i,
index);

/*Almanachdaten eines Satelliten darstellen*/
show_alm(sv):=block(
[index],
index:find_almanach(sv),
if index=0 then (print("SV:",sv,"nicht vorhanden"),return()),
print("SV:       ",almatrix[index][1]),
print("ecc:      ",almatrix[index][2]),
print("time:     ",almatrix[index][3]),
print("i0:       ",almatrix[index][4],"in Grad:",float(almatrix[index][4]*180/%pi)),
print("OMEGA_dot:",almatrix[index][5]),
print("sqrt_A:   ",almatrix[index][6],"quadriert:",almatrix[index][6]^2),
print("OMEGA_0:  ",almatrix[index][7],"in Grad:",float(almatrix[index][7]*180/%pi)),
print("omega:    ",almatrix[index][8]),
print("M0:       ",almatrix[index][9]),
return(true));

/*Satellitenposition aus den Almanachdaten berechnen*/
satpos_alm(sow,sv):=block(
[M0,ecc,sqrt_A,toa,OMEGA_0,i0,omega,OMEGA_dot,GM,OMEGAe_dot,A,tk,n0,
M,E,E_old,dE,v,Phi,u,r,i,OMEGA,x1,y1,sat_x,sat_y,sat_z],
GM:3.986005e14,
OMEGAe_dot:7.2921151467e-5,
index:find_almanach(sv),
M0:almatrix[index][9],
ecc:almatrix[index][2],
sqrt_A:almatrix[index][6],
toa:almatrix[index][3],
OMEGA_0:almatrix[index][7],
i0:almatrix[index][4],
omega:almatrix[index][8],
OMEGA_dot:almatrix[index][5],
A:sqrt_A*sqrt_A,
tk:check_t(sow-toa),
n0:sqrt(GM/A^3),
M:M0+n0*tk,
M:hauptwert(M),
E:M,
dE:1,
for i:1 thru 10 unless abs(dE)<1*10^-12 do
    (
    E_old:E,
    E:M+ecc*sin(E),
    dE:(mod(E-E_old,2*%pi))
    ),
E:hauptwert(E),
v:atan2(sqrt(1-ecc^2)*sin(E),cos(E)-ecc),
Phi:v+omega,
Phi:hauptwert(Phi),
u:Phi,
r:A*(1-ecc*cos(E)),
i:i0,
x1:cos(u)*r,
y1:sin(u)*r,
OMEGA:OMEGA_0+(OMEGA_dot-OMEGAe_dot)*tk-OMEGAe_dot*toa,
OMEGA:hauptwert(OMEGA),
sat_x:x1*cos(OMEGA)-y1*cos(i)*sin(OMEGA),
sat_y:x1*sin(OMEGA)+y1*cos(i)*cos(OMEGA),
sat_z:y1*sin(i),
[sat_x,sat_y,sat_z]);

/*Positionen aller Satelliten aus dem Almanach berechnen*/
positionen():=block(
[pos_list,time,pos],
pos_list:[],
time:almatrix[1][3],
for i:1 thru length(almatrix) do
    (
    pos:satpos_alm(time,almatrix[i][1]),
    pos_list:endcons(pos,pos_list)
    ),
pos_list
);

/*Umrechnung ECEF- in Dezimal-Polarkoordinaten*/
ecef_polar_grad(pos):=
[float(atan2(pos[3],sqrt(pos[1]^2+pos[2]^2))*180/%pi),
float(atan2(pos[2],pos[1])*180/%pi),
float(sqrt(pos[1]^2+pos[2]^2+pos[3]^2)-6378137)];

/*Liste mit Paaren [Länge,Breite] zum Plotten erstellen*/
make_polarlist(liste):=block(
[listeneu],
listeneu:[],
for i in liste do
    listeneu:endcons([i[2],i[1]],listeneu),
listeneu
);

/*Berechnung kartesischer Koordinaten aus Kugelkoordinaten*/
kugel_cart(phi,lambda):=block(
[Phi,r,x,y,z],
Phi:90-phi,
r:6370000,
x:r*sin(bogen(Phi))*cos(bogen(lambda)),
y:r*sin(bogen(Phi))*sin(bogen(lambda)),
z:r*cos(bogen(Phi)),
[x,y,z]
);

/*Parameter der Kreisgleichung für den Satellitenorbit berechnen*/
kreisparameter(Omega,i):=block(
[e10,e20,e1,e2],
e10:matrix([1],[0],[0]),
e20:matrix([0],[cos(i)],[sin(i)]),
D:matrix([cos(Omega),-sin(Omega),0],[sin(Omega),cos(Omega),0],[0,0,1]),
e1:D.e10,
e2:D.e20,
26000000*cos(alpha)*e1+26000000*sin(alpha)*e2
);

/*Liste aller Inklinationen erstellen*/
make_inklinationlist():=block(
[ilist],
ilist:[],
for i in almatrix do
    ilist:endcons(float(i[4]),ilist),
sort(ilist)
);

/*Liste mit den Längen des aufsteigenden Knotens erstellen*/
make_omegalist():=block(
[omegalist],
omegalist:[],
for i in almatrix do
    omegalist:endcons(float(i[7]),omegalist),
sort(omegalist)
);

/*Liste mit den Wurzeln der großen Halbachsen erstellen*/
make_sqrtalist():=block(
[sqrtalist],
sqrtalist:[],
for i in almatrix do
    sqrtalist:endcons(float(i[6]),sqrtalist),
sort(sqrtalist));

/*Gleichartige Werte clustern*/
clustern(liste):=block(
[cluster,summe,meanomega],
cluster:[],
meanomega:[],
for i:1 thru length(liste)-1 do
    if abs(liste[i]*180/%pi-liste[i+1]*180/%pi)>10
        then push(i,cluster),
push(length(liste),cluster),
cluster:sort(cluster),
start:1,
summe:0,
for i in cluster do
    (
    for k:start thru i do
        summe:summe+liste[k],
    print(i-start+1,float(summe/((i-start)+1)*180/%pi)),
    if i-start+1#1 then meanomega:endcons(float(summe/((i-start)+1)),meanomega),
    start:i+1,
    summe:0
    ),
meanomega);


/*Kapitel 11: Positionsbestimmung aus 4 Satelliten*/

/*Umrechnung Dezimalgrad in Grad, Minute und Sekunde*/
polar_gms(pos):=block(
[b,bg,bmr,bm,bs,l,lg,lmr,lm,ls,h],
[b,l,h]:pos,
bg:floor(b),
bmr:((b-bg)*60),
bm:floor(bmr),
bs:floor((bmr-bm)*60),
lg:floor(l),
lmr:((l-lg)*60),
lm:floor(lmr),
ls:floor((lmr-lm)*60),
[[bg,bm,bs],[lg,lm,ls],h]);

/*Bestimmung des Empfängerstandorts aus 4 Satellitenpositionen*/
recpos_4(sow):=block(
[estpos,L,svpos,obsindex,d,J,sv,pr,dist,A,pos],
estpos:[0.0,0.0,0.0],
L:matrix([1.0],[1.0],[1.0],[0.0]),
svpos:[],
obsindex:find_obsindex(sow),
unless abs(L[1][1])+abs(L[2][1])+abs(L[3][1])<0.1 do
    (
    d:matrix(),
    J:matrix(),
    for i:3 thru 6 do
        (
        sv:obsmatrix[obsindex][i][1],
        pr:read_prange(sow,sv),
        svpos:satpos(sow,sv),
        dist:pr-norm(svpos,estpos)-L[4][1],
        d:addrow(d,[dist]),
        A:[(estpos[1]-svpos[1])/norm(svpos,estpos),
           (estpos[2]-svpos[2])/norm(svpos,estpos),
           (estpos[3]-svpos[3])/norm(svpos,estpos),
            1],
        J:addrow(J,A)
        ),
    L:float(invert(J).d),
    estpos[1]:estpos[1]+L[1][1],
    estpos[2]:estpos[2]+L[2][1],
    estpos[3]:estpos[3]+L[3][1]
    ),
ecef_polar_grad(estpos));

/*Alle möglichen 4-Kombinationen empfangener Satelliten erzeugen*/
make_svmix(sow):=block(
[svnrlist,svnrset,ps],
svnrlist:read_avail_sats(sow),
svnrset:setify(svnrlist),
ps:powerset(svnrset,4),
print("Anzahl der Kombinationen:",cardinality(ps)),
full_listify(ps));

/*recpos_4() mit der Angabe der auszuwertenden Satelliten*/
recpos_vier(sow,svliste):=block(
[estpos,L,svpos,d,J,sv,pr,dist,A,pos],
estpos:[0.0,0.0,0.0],
L:matrix([1.0],[1.0],[1.0],[0.0]),
svpos:[],
unless abs(L[1][1])+abs(L[2][1])+abs(L[3][1])<0.1 do
    (
    d:matrix(),
    J:matrix(),
    for i:1 thru 4 do
        (
        sv:svliste[i],
        pr:read_prange(sow,sv),
        svpos:satpos(sow,sv),
        dist:pr-norm(svpos,estpos)-L[4][1],
        d:addrow(d,[dist]),
        A:[(estpos[1]-svpos[1])/norm(svpos,estpos),
           (estpos[2]-svpos[2])/norm(svpos,estpos),
           (estpos[3]-svpos[3])/norm(svpos,estpos),
            1],
        J:addrow(J,A)
        ),
    L:float(invert(J).d),
    estpos[1]:estpos[1]+L[1][1],
    estpos[2]:estpos[2]+L[2][1],
    estpos[3]:estpos[3]+L[3][1],
    if norm(estpos,[0,0,0])>10^10 then return(false)
    ),
ecef_polar_grad(estpos));

/*Mittelwert aller Satellitenquartette errechnen*/
mean_recpos(sow,mix):=block(
[posliste,x_wert,y_wert],
posliste:[],
x_wert:0,
y_wert:0,
for i:1 thru length(mix) do
    (
    pos:recpos_vier(sow,mix[i]),
    x_wert:x_wert+pos[1],
    y_wert:y_wert+pos[2]
    ),
anzahl:length(mix), 
[x_wert/anzahl,y_wert/anzahl]
);


/*Kapitel 12: Verbesserte Zeitbestimmung*/

/*Zeitkorrektur aufgrund der Laufzeit des Signals und der Ungenauigkeiten der Uhr*/
timecorrect(time,sv):=block(
[pseudorange,eph_list,signallaufzeit,tx_RAW,toe,dt,af0,af1,af2,tcorr,tx_GPS],
pseudorange:read_prange(time,sv),
eph_list:navmatrix[find_ephindex(sv)],
signallaufzeit:pseudorange/v_light,
tx_RAW:check_t(time-signallaufzeit),
toe:eph_list[19],
dt:check_t(tx_RAW-toe),
af0:eph_list[8],
af1:eph_list[9],
af2:eph_list[10],
tcorr:(af2*dt+af1)*dt+af0,
tx_GPS:check_t(tx_RAW-tcorr),
dt:check_t(tx_GPS-toe),
tcorr:(af2*dt+af1)*dt+af0,
tx_GPS:tx_RAW-tcorr,
[tx_GPS,tcorr]);

/*Vier beliebige Satelliten mit Zeitkorrektur*/
recpos_vier_t(sow,svliste):=block(
[estpos,L,svpos,d,J,sv,t_GPS,tcorr,pr,dist,A,pos],
estpos:[0.0,0.0,0.0],
L:matrix([1.0],[1.0],[1.0],[0.0]),
svpos:[],
unless abs(L[1][1])+abs(L[2][1])+abs(L[3][1])<0.1 do
    (
    d:matrix(),
    J:matrix(),
    for i:1 thru 4 do
        (
        sv:svliste[i],
        [t_GPS,tcorr]:timecorrect(sow,sv),
        pr:read_prange(sow,sv),
        svpos:satpos(t_GPS,sv),
        dist:pr-norm(svpos,estpos)-L[4][1]+v_light*tcorr,
        d:addrow(d,[dist]),
        A:[(estpos[1]-svpos[1])/norm(svpos,estpos),
           (estpos[2]-svpos[2])/norm(svpos,estpos),
           (estpos[3]-svpos[3])/norm(svpos,estpos),
            1],
        J:addrow(J,A)
        ),
    L:float(invert(J).d),
    estpos[1]:estpos[1]+L[1][1],
    estpos[2]:estpos[2]+L[2][1],
    estpos[3]:estpos[3]+L[3][1],
    if norm(estpos,[0,0,0])>10^10 then return(false)
    ),
ecef_polar_grad(estpos));


/*Kapitel 13: Methode der kleinsten Quadrate*/

/*Positionsbestimmung mit der lsf-Mthode*/
recpos_lsf(sow):=block(
[estpos,L,svpos,obsindex,d,J,sv,t_GPS,tcorr,pr,dist,A,pos],
estpos:[0.0,0.0,0.0],
L:matrix([1.0],[1.0],[1.0],[0.0]),
svpos:[],
obsindex:find_obsindex(sow),
unless abs(L[1][1])+abs(L[2][1])+abs(L[3][1])<0.1 do
    (
    d:matrix(),
    J:matrix(),
    for i:3 thru length(obsmatrix[obsindex]) do
        (
        sv:obsmatrix[obsindex][i][1],
        [t_GPS,tcorr]:timecorrect(sow,sv),
        pr:read_prange(sow,sv),
        svpos:satpos(t_GPS,sv),
        dist:pr-norm(svpos,estpos)-L[4][1]+v_light*tcorr,
        d:addrow(d,[dist]),
        A:[(estpos[1]-svpos[1])/norm(svpos,estpos),
           (estpos[2]-svpos[2])/norm(svpos,estpos),
           (estpos[3]-svpos[3])/norm(svpos,estpos),
            1],
        J:addrow(J,A)
        ),
    L:float(invert(transpose(J).J).transpose(J).d),
    estpos[1]:estpos[1]+L[1][1],
    estpos[2]:estpos[2]+L[2][1],
    estpos[3]:estpos[3]+L[3][1]
    ),
ecef_polar_grad(estpos)
);


/*Kapitel 14: Verbesserung des Erdmodells*/

/*Umrechnung von ECEF- in Polarkoordinaten im Bogenmaß*/
ecef_polar_bogen(pos):=
[float(atan(pos[3]/sqrt(pos[1]^2+pos[2]^2))),
float(atan(pos[2]/pos[1])),
float(sqrt(pos[1]^2+pos[2]^2+pos[3]^2)-6378137)];

/*Umrechnung im ellipsoidalen Modell in Polarkoordinaten*/
ecef_polar_ellipse(ecef):=block(
[a,eps,xE,yE,zE,phi,lambda,h,gl1,gl2,gl3,start,loes,erg],
a:6378137.0,
eps:0.08181919,
[xE,yE,zE]:ecef,
gl1:(a/sqrt(1-eps^2*(sin(phi))^2)+h)*cos(phi)*cos(lambda)-xE,
gl2:(a/sqrt(1-eps^2*(sin(phi))^2)+h)*cos(phi)*sin(lambda)-yE,
gl3:(a*(1-eps^2)/sqrt(1-eps^2*(sin(phi))^2)+h)*sin(phi)-zE,
start:ecef_polar_bogen(ecef),
loes:mnewton([gl1,gl2,gl3],[phi,lambda,h],start),
erg:matrixmap(rhs,loes)[1],
float([erg[1]*180/%pi,erg[2]*180/%pi,erg[3]]));

/*Positionsbestimmung mit dem ellipsoidalen Modell*/
recpos_ell(sow):=block(
[estpos,L,svpos,obsindex,d,J,sv,t_GPS,tcorr,pr,dist,A,pos],
estpos:[0.0,0.0,0.0],
L:matrix([1.0],[1.0],[1.0],[0.0]),
svpos:[],
A:[],
obsindex:find_obsindex(sow),
unless abs(L[1][1])+abs(L[2][1])+abs(L[3][1])<0.1 do
    (
    d:matrix(),
    J:matrix(),
    for i:3 thru length(obsmatrix[obsindex]) do
        (
        sv:obsmatrix[obsindex][i][1],
        [t_GPS,tcorr]:timecorrect(sow,sv), 
        pr:read_prange(sow,sv),
        svpos:satpos(t_GPS,sv),
        dist:pr-norm(svpos,estpos)-L[4][1]+v_light*tcorr,
        d:addrow(d,[dist]),
        A:[(estpos[1]-svpos[1])/norm(svpos,estpos),
           (estpos[2]-svpos[2])/norm(svpos,estpos),
           (estpos[3]-svpos[3])/norm(svpos,estpos),
            1],
        J:addrow(J,A)
        ),
    L:float(invert(transpose(J).J).transpose(J).d),
    estpos[1]:estpos[1]+L[1][1],
    estpos[2]:estpos[2]+L[2][1],
    estpos[3]:estpos[3]+L[3][1]
    ),
ecef_polar_ellipse(estpos));


/*Kapitel 15: Berücksichtigung der Erddrehung*/

/*Rotationskorrektur*/
rot_corr(satellitenpos,dt):=block(
[OMEGAe_dot,phi,satvektor,R3,new_pos],
OMEGAe_dot:7.292115147e-5,
phi:OMEGAe_dot*dt,
satvektor: matrix(
 [satellitenpos[1]],
 [satellitenpos[2]],
 [satellitenpos[3]]
),
R3:matrix(
 [cos(phi),sin(phi),0],
 [-sin(phi),cos(phi),0],
 [0,0,1]
),
new_pos:R3.satvektor,
[float(new_pos[1][1]),float(new_pos[2][1]),float(new_pos[3][1])]
);

/*Positionsbestimmung mit Korrektur der Erddrehung*/
recpos_rot(sow):=block(
[estpos,L,svpos,traveltime,svpos_rot,obsindex,d,J,sv,t_GPS,tcorr,pr,dist,A,pos],
estpos:[0.0,0.0,0.0],
L:matrix([1.0],[1.0],[1.0],[0.0]),
svpos:[],
obsindex:find_obsindex(sow),
unless abs(L[1][1])+abs(L[2][1])+abs(L[3][1])<0.1 do
    (
    d:matrix(),
    J:matrix(),
    for i:3 thru length(obsmatrix[obsindex]) do
        (
        sv:obsmatrix[obsindex][i][1],
        [t_GPS,tcorr]:timecorrect(sow,sv), 
        pr:read_prange(sow,sv),
        svpos:satpos(t_GPS,sv),
        traveltime:pr/v_light,
        svpos_rot:rot_corr(svpos,traveltime),
        dist:pr-norm(svpos_rot,estpos)-L[4][1]+v_light*tcorr,
        d:addrow(d,[dist]),
        A:[(estpos[1]-svpos_rot[1])/norm(svpos_rot,estpos),
           (estpos[2]-svpos_rot[2])/norm(svpos_rot,estpos),
           (estpos[3]-svpos_rot[3])/norm(svpos_rot,estpos),
            1],
        J:addrow(J,A)
        ),
    L:float(invert(transpose(J).J).transpose(J).d),
    estpos[1]:estpos[1]+L[1][1],
    estpos[2]:estpos[2]+L[2][1],
    estpos[3]:estpos[3]+L[3][1]
    ),
ecef_polar_ellipse(estpos));


/*Kapitel 16: Relativistische Korrektur*/

/*Berechnung der relativistischen Zeitkorrektur*/
dt_rel(sow,sv):=block(
[GM,F,k,delta_n,M0,ecc,sqrt_A,toe,A,tk,n0,n,M,E,dE,E_old],
GM:3.986005e14,
F:-4.442807633e-10,
k:find_ephindex(sv),
delta_n:navmatrix[k][13],
M0:navmatrix[k][14],
ecc:navmatrix[k][16],
sqrt_A:navmatrix[k][18],
toe:navmatrix[k][19],
A:sqrt_A*sqrt_A,
tk:check_t(sow-toe),
n0:sqrt(GM/A^3),
n:n0+delta_n,
M:M0+n*tk,
M:hauptwert(M),
E:M,
dE:1,
for i:1 thru 10 unless abs(dE)<1*10^-12 do
    (
    E_old:E,
    E:M+ecc*sin(E),
    dE:(mod(E-E_old,2*%pi))
    ),
E:hauptwert(E),
F*ecc*sqrt_A*sin(E)
);

/*Veränderte Funktion zur Zeitkorrektur*/
timecorrect2(time,sv):=block(
[pseudorange,eph_list,signallaufzeit,tx_RAW,toe,dt,af0,af1,af2,tcorr,tx_GPS],
pseudorange:read_prange(time,sv),
eph_list:navmatrix[find_ephindex(sv)],
signallaufzeit:pseudorange/v_light,
tx_RAW:check_t(time-signallaufzeit),
toe:eph_list[19],
dt:check_t(tx_RAW-toe),
af0:eph_list[8],
af1:eph_list[9],
af2:eph_list[10],
tcorr:(af2*dt+af1)*dt+af0+dt_rel(time,sv),
tx_GPS:check_t(tx_RAW-tcorr),
dt:check_t(tx_GPS-toe),
tcorr:(af2*dt+af1)*dt+af0+dt_rel(time,sv),
tx_GPS:tx_RAW-tcorr,
[tx_GPS,tcorr]);

/*Empfängerposition unter Berücksichtigung relativistischer Effekte*/
recpos_trel(sow):=block(
[estpos,L,svpos,traveltime,svpos_rot,obsindex,d,J,sv,t_GPS,tcorr,pr,dist,A,pos],
estpos:[0.0,0.0,0.0],
L:matrix([1.0],[1.0],[1.0],[0.0]),
svpos:[],
obsindex:find_obsindex(sow),
unless abs(L[1][1])+abs(L[2][1])+abs(L[3][1])<0.1 do
    (
    d:matrix(),
    J:matrix(),
    for i:3 thru length(obsmatrix[obsindex]) do
        (
        sv:obsmatrix[obsindex][i][1],
        [t_GPS,tcorr]:timecorrect2(sow,sv), 
        pr:read_prange(sow,sv),
        svpos:satpos(t_GPS,sv),
        traveltime:pr/v_light,
        svpos_rot:rot_corr(svpos,traveltime),
        dist:pr-norm(svpos_rot,estpos)-L[4][1]+v_light*tcorr,
        d:addrow(d,[dist]),
        A:[(estpos[1]-svpos_rot[1])/norm(svpos_rot,estpos),
           (estpos[2]-svpos_rot[2])/norm(svpos_rot,estpos),
           (estpos[3]-svpos_rot[3])/norm(svpos_rot,estpos),
            1],
        J:addrow(J,A)
        ),
    L:float(invert(transpose(J).J).transpose(J).d),
    estpos[1]:estpos[1]+L[1][1],
    estpos[2]:estpos[2]+L[2][1],
    estpos[3]:estpos[3]+L[3][1]
    ),
ecef_polar_ellipse(estpos));


/*Skyplot*/

/*Satellitenposition in ECEF-Koordinaten*/
recpos_trel_ecef(sow):=block(
[estpos,L,svpos,traveltime,svpos_rot,obsindex,d,J,sv,pr,t_GPS,tcorr,dist,A,pos],
estpos:[0.0,0.0,0.0],
L:matrix([1.0],[1.0],[1.0],[0.0]),
svpos:[],
obsindex:find_obsindex(sow),
unless abs(L[1][1])+abs(L[2][1])+abs(L[3][1])<0.1 do
    (
    d:matrix(),
    J:matrix(),
    for i:3 thru length(obsmatrix[obsindex]) do
        (
        sv:obsmatrix[obsindex][i][1],
        [t_GPS,tcorr]:timecorrect2(sow,sv), 
        pr:read_prange(sow,sv),
        svpos:satpos(t_GPS,sv),
        traveltime:pr/v_light,
        svpos_rot:rot_corr(svpos,traveltime),
        dist:pr-norm(svpos_rot,estpos)-L[4][1]+v_light*tcorr,
        d:addrow(d,[dist]),
        A:[(estpos[1]-svpos_rot[1])/norm(svpos_rot,estpos),
           (estpos[2]-svpos_rot[2])/norm(svpos_rot,estpos),
           (estpos[3]-svpos_rot[3])/norm(svpos_rot,estpos),
            1],
        J:addrow(J,A)
        ),
    L:float(invert(transpose(J).J).transpose(J).d),
    estpos[1]:estpos[1]+L[1][1],
    estpos[2]:estpos[2]+L[2][1],
    estpos[3]:estpos[3]+L[3][1]
    ),
estpos);

/*Elevation bestimmen*/
elevation(n,u):=block(
[elev],
elev:asin(abs(u.n)/(mat_norm(u,frobenius)*mat_norm(n,frobenius))),
if sign(n.u)=neg then -elev else elev);

/*Parallelprojektion in die Ebene*/
projektion(x,n,r):=x-(((x-r).n)/(n.n)).n;

/*Winkel zwischen zwei Geraden*/
azimut(u,v):=block(
acos((u.v)/(mat_norm(u,frobenius)*mat_norm(v,frobenius))));


/*Bestimmung von Azimut und Elevation eines Satelliten sv zu einem bestimmten Zeitpunkt sow*/
az_elev(sow,sv):=block(
[recpos_liste,recpos_v,satpos_liste,satpos_v,
satvect,elev,projektion_satvect,nordpol,
projektion_nordpol,nordrichtung,satrichtung,
nordwinkel,osten,projektion_osten,ostrichtung,
ostwinkel,az],
recpos_liste:recpos_trel_ecef(sow),
recpos_v:transpose(matrix(recpos_liste)),
satpos_liste:satpos(sow,sv),
satpos_v:transpose(matrix(satpos_liste)),
satvect:satpos_v-recpos_v,
elev:elevation(recpos_v,satvect),
projektion_satvect:projektion(satpos_v,recpos_v,recpos_v),
nordpol:matrix([0],[0],[3*637000]),
projektion_nordpol:projektion(nordpol,recpos_v,recpos_v),
nordrichtung:projektion_nordpol-recpos_v,
satrichtung:projektion_satvect-recpos_v,
nordwinkel:azimut(nordrichtung,satrichtung),
osten:matrix([0],[3*637000],[0]),
projektion_osten:projektion(osten,recpos_v,recpos_v),
ostrichtung:projektion_osten-recpos_v,
ostwinkel:azimut(ostrichtung,satrichtung),
if ostwinkel>%pi/2 then az:2*%pi-nordwinkel
				else az:nordwinkel,
[sv,float(grad(az)),float(grad(elev))]);

/*Azimut und Elevation im kartesischen System*/
az_elev_cart(plotlist):=block(
[sv,az,el,r,phi],
[sv,az,el]:plotlist,
r:abs(el/90-1),
phi:az,
x:float(r*cos(bogen(phi))),
y:float(r*sin(bogen(phi))),
[sv,y,x]);