mirror
/
RS-tracker
kopia lustrzana https://github.com/rs1729/RS
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

GPS ephemeris

dump
Zilog80 2015-10-29 17:04:40 +01:00
rodzic 289b646802
commit 272e805638
5 zmienionych plików z 4692 dodań i 0 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

282
rs92/almanac.sem.week0843.061440.txt 100644
Wyświetl plik

@ -0,0 +1,282 @@
31 CURRENT.ALM
843 61440
1
63
0
4.76169586181641E-03 6.42013549804688E-03 -2.50292941927910E-09
5.15360498046875E+03 5.49559831619263E-01 1.56480073928833E-01
9.42617654800415E-01 2.86102294921875E-06 0.00000000000000E+00
0
11
2
61
0
1.48696899414062E-02 -3.68118286132812E-04 -2.56841303780675E-09
5.15358935546875E+03 5.36597847938538E-01 -7.07825660705566E-01
-8.72436165809631E-01 5.93185424804688E-04 0.00000000000000E+00
0
9
3
69
0
4.80175018310547E-04 5.43022155761719E-03 -2.52475729212165E-09
5.15352343750000E+03 8.80910396575928E-01 -8.89197349548340E-01
-3.48153710365295E-01 2.00271606445312E-05 0.00000000000000E+00
0
11
4
34
0
1.19705200195312E-02 -6.56127929687500E-04 -2.57932697422802E-09
5.15358593750000E+03 5.41403293609619E-01 3.54043602943420E-01
8.81271481513977E-01 -3.43322753906250E-05 -3.63797880709171E-12
0
9
5
50
0
4.47034835815430E-03 1.34086608886719E-03 -2.57205101661384E-09
5.15360156250000E+03 8.78067612648010E-01 1.41729474067688E-01
-9.47973728179932E-02 -1.84059143066406E-04 3.63797880709171E-12
0
10
6
67
0
2.11715698242188E-04 6.37626647949219E-03 -2.50292941927910E-09
5.15350830078125E+03 5.46863794326782E-01 8.81587386131287E-01
-2.91297674179077E-01 7.91549682617188E-05 7.27595761418343E-12
0
11
7
48
0
8.99744033813477E-03 8.29315185546875E-03 -2.53567122854292E-09
5.15364599609375E+03 -4.44634079933167E-01 -8.56053233146667E-01
7.37231254577637E-01 4.82559204101562E-04 0.00000000000000E+00
0
10
8
72
0
1.43575668334961E-03 5.85174560546875E-03 -2.50292941927910E-09
5.15354345703125E+03 2.12597131729126E-01 -5.56113839149475E-01
1.06339812278748E-01 -9.53674316406250E-06 0.00000000000000E+00
0
11
9
68
0
3.50475311279297E-04 4.50325012207031E-03 -2.49929144047201E-09
5.15366406250000E+03 -7.87639379501343E-01 7.68764019012451E-01
-5.35195589065552E-01 -9.53674316406250E-07 3.63797880709171E-12
0
11
11
46
0
1.63025856018066E-02 -1.53999328613281E-02 -2.70301825366914E-09
5.15356542968750E+03 4.34556365013123E-01 4.67948317527771E-01
7.64931321144104E-01 -6.11305236816406E-04 -3.63797880709171E-12
0
9
12
58
0
5.51271438598633E-03 1.52435302734375E-02 -2.49565346166492E-09
5.15354687500000E+03 -1.03542566299438E-01 2.05610990524292E-01
5.91576457023621E-01 3.37600708007812E-04 3.63797880709171E-12
0
10
13
43
0
4.65059280395508E-03 9.75990295410156E-03 -2.44472175836563E-09
5.15364160156250E+03 -7.50291109085083E-01 6.54097199440002E-01
-9.83810424804688E-01 -1.57356262207031E-04 -3.63797880709171E-12
0
9
14
41
0
8.43811035156250E-03 7.37762451171875E-03 -2.47018761001527E-09
5.15362451171875E+03 -7.61617422103882E-01 -6.20580315589905E-01
-3.76976013183594E-01 2.76565551757812E-05 -3.63797880709171E-12
0
9
15
55
0
7.60126113891602E-03 -3.18527221679688E-03 -2.56477505899966E-09
5.15367968750000E+03 -8.02642464637756E-01 1.32260441780090E-01
-5.98965883255005E-01 -2.77519226074219E-04 -3.63797880709171E-12
0
10
16
56
0
8.12625885009766E-03 1.54151916503906E-02 -2.49565346166492E-09
5.15359667968750E+03 -9.75867509841919E-02 1.01557016372681E-01
-5.11003732681274E-02 -7.53402709960938E-05 3.63797880709171E-12
0
9
17
53
0
1.05104446411133E-02 1.00307464599609E-02 -2.45563569478691E-09
5.15353906250000E+03 2.28366613388062E-01 -6.38240098953247E-01
-3.62008810043335E-01 -1.91688537597656E-04 0.00000000000000E+00
0
10
18
54
0
1.63583755493164E-02 -5.47409057617188E-03 -2.64117261394858E-09
5.15357177734375E+03 8.72890233993530E-01 -6.15308403968811E-01
1.28568768501282E-01 4.44412231445312E-04 3.63797880709171E-12
0
9
19
59
0
1.13387107849121E-02 9.08660888671875E-03 -2.45927367359400E-09
5.15510888671875E+03 2.44028687477112E-01 2.08910703659058E-01
-8.89738202095032E-01 -5.21659851074219E-04 0.00000000000000E+00
0
9
20
51
0
4.97055053710938E-03 -5.09643554687500E-03 -2.63389665633440E-09
5.15358886718750E+03 8.56302618980408E-01 3.97144556045532E-01
-5.79112887382507E-01 3.64303588867188E-04 3.63797880709171E-12
0
9
21
45
0
2.26144790649414E-02 -2.55584716796875E-03 -2.58660293184221E-09
5.15367919921875E+03 5.40921092033386E-01 -5.95261573791504E-01
4.29614663124084E-01 -4.87327575683594E-04 -3.63797880709171E-12
0
9
22
47
0
7.99179077148438E-03 -6.16455078125000E-03 -2.65208655036986E-09
5.15360986328125E+03 8.73178720474243E-01 -6.50720953941345E-01
-2.59807109832764E-02 4.02450561523438E-04 3.63797880709171E-12
0
9
23
60
0
1.03850364685059E-02 1.63459777832031E-03 -2.52111931331456E-09
5.15373535156250E+03 -7.85323858261108E-01 -8.35123538970947E-01
-7.43304252624512E-01 -1.39236450195312E-04 -3.63797880709171E-12
0
9
24
65
0
3.56006622314453E-03 2.92778015136719E-03 -2.59387888945639E-09
5.15360156250000E+03 -4.58439588546753E-01 9.36031341552734E-02
-9.51664447784424E-01 -6.67572021484375E-06 0.00000000000000E+00
0
11
25
62
0
4.66060638427734E-03 1.15451812744141E-02 -2.53567122854292E-09
5.15356640625000E+03 -1.18910312652588E-01 2.33322262763977E-01
3.87429952621460E-01 -5.43594360351562E-05 -3.63797880709171E-12
0
11
26
71
0
2.22206115722656E-04 5.83267211914062E-03 -2.59387888945639E-09
5.15360009765625E+03 -1.20532512664795E-01 -1.81727409362793E-02
2.22779273986816E-01 -8.86917114257812E-05 -1.45519152283669E-11
0
11
27
66
0
2.68411636352539E-03 8.42475891113281E-03 -2.47382558882236E-09
5.15366015625000E+03 2.12892651557922E-01 9.78533029556274E-02
-3.79243373870850E-01 2.86102294921875E-05 3.63797880709171E-12
0
11
28
44
0
1.98087692260742E-02 1.49860382080078E-02 -2.48473952524364E-09
5.15365576171875E+03 -9.59317684173584E-02 -5.26890754699707E-01
-1.09564542770386E-01 4.74929809570312E-04 3.63797880709171E-12
0
9
29
57
0
8.44955444335938E-04 1.03130340576172E-02 -2.45563569478691E-09
5.15363476562500E+03 2.31390595436096E-01 -1.60193920135498E-01
4.28920388221741E-01 6.39915466308594E-04 3.63797880709171E-12
0
10
30
64
0
1.69801712036133E-03 3.76701354980469E-03 -2.58660293184221E-09
5.15363330078125E+03 -4.29573178291321E-01 -9.96210932731628E-01
7.12476134300232E-01 4.29153442382812E-05 7.27595761418343E-12
0
11
31
52
0
8.19396972656250E-03 9.84001159667969E-03 -2.52111931331456E-09
5.15356835937500E+03 -4.41966295242310E-01 -1.63536071777344E-01
7.38614082336426E-01 2.98500061035156E-04 0.00000000000000E+00
0
10
32
23
0
1.13606452941895E-02 1.49917602539062E-03 -2.55386112257838E-09
5.15361035156250E+03 9.06374692916870E-01 4.80757951736450E-02
8.73380422592163E-01 -1.04904174804688E-05 1.09139364212751E-11
0
9

3376
rs92/brdc2910.15n 100644
Wyświetl plik

Plik diff jest za duży Load Diff

198
rs92/gps_navdata.c 100644
Wyświetl plik

@ -0,0 +1,198 @@
/*
(precise) ftp://ftp.nga.mil/pub2/gps/pedata/2015pe/
(broadcast) ftp://cddis.gsfc.nasa.gov/gnss/data/daily/2015/291/15n/
(almanac) https://celestrak.com/GPS/almanac/SEM/2015/
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
typedef unsigned char ui8_t;
typedef unsigned short ui16_t;
typedef unsigned int ui32_t;
#include "nav_gps.c"
int rollover = 0;
EPHEM_t alm[33];
EPHEM_t eph[33][24];
SAT_t sat_alm[33], sat_eph[33];
int calc_satpos_alm(EPHEM_t alm[], double t, SAT_t *sat) {
double X, Y, Z;
int j;
int week;
double cl_corr;
for (j = 1; j < 33; j++) {
if (alm[j].prn > 0) { // prn==j
// Woche hat 604800 sec
if (t-alm[j].toa > 604800/2) rollover = +1;
else if (t-alm[j].toa < -604800/2) rollover = -1;
else rollover = 0;
week = alm[j].week - rollover;
GPS_SatellitePosition_Ephem(
week, t, alm[j],
&cl_corr, &X, &Y, &Z
);
sat[alm[j].prn].X = X;
sat[alm[j].prn].Y = Y;
sat[alm[j].prn].Z = Z;
sat[alm[j].prn].clock_corr = cl_corr;
}
}
return 0;
}
int calc_satpos_rnx(EPHEM_t eph[][24], double t, SAT_t *sat) {
double X, Y, Z;
int j, i, ti;
int week;
double cl_corr;
double tdiff, td;
for (j = 1; j < 33; j++) {
// Woche hat 604800 sec
tdiff = 604800;
ti = 0;
for (i = 0; i < 24; i++) {
if (eph[j][i].prn > 0) {
if (t-eph[j][i].toe > 604800/2) rollover = +1;
else if (t-eph[j][i].toe < -604800/2) rollover = -1;
else rollover = 0;
td = t-eph[j][i].toe - rollover*604800;
if (td < 0) td *= -1;
if ( td < tdiff ) {
tdiff = td;
ti = i;
week = eph[j][ti].week - rollover;
}
}
}
//printf("prn %02d: ti=%02d (%.1f) td=%.0f\n", eph[j][ti].prn, ti, eph[j][ti].toe, tdiff);
GPS_SatellitePosition_Ephem(
week, t, eph[j][ti],
&cl_corr, &X, &Y, &Z
);
sat[eph[j][ti].prn].X = X;
sat[eph[j][ti].prn].Y = Y;
sat[eph[j][ti].prn].Z = Z;
sat[eph[j][ti].prn].clock_corr = cl_corr;
}
return 0;
}
// ftp://ftp.nga.mil/pub2/gps/pedata/2015pe/
// nga18670.Z
double precise201510180000[33][5] = {
{ 0, 0, 0, 0, 0 },
{ 1, -13396.503088, 16970.854595, 15216.330882, 2.582065 },
{ 2, 14263.732177, 7790.652882, -20685.856689, 593.070368 },
{ 3, -22205.915807, 13269.761877, -6063.419651, 19.656736 },
{ 4, -15544.326337, 7438.029201, 19792.978577, -33.725265 },
{ 5, 23627.784132, 106.951075, -12385.480993, -184.039639 },
{ 6, 8367.508834, 19482.783055, -16001.912082, 78.618819 },
{ 7, -5782.726114, 25630.989537, -1813.496023, 482.644186 },
{ 8, -19787.879341, 183.638881, 17754.688132, -9.097048 },
{ 9, -1225.647544, 17186.903817, -20217.354165, -1.371448 },
{ 10, 0.0 , 0.0 , 0.0 , 0.0 },
{ 11, -12853.596170, 12708.409923, 18888.524237, -610.793350 },
{ 12, 23820.802471, -10798.624959, -4014.469346, 337.713996 },
{ 13, 21434.480084, 10098.136853, 11762.516964, -157.494119 },
{ 14, -12538.553468, -21293.861893, 10189.913434, 27.512184 },
{ 15, 19603.561444, -2247.081455, 17828.390890, -277.380526 },
{ 16, -23375.226825, -1510.183240, -12951.652156, -75.480117 },
{ 17, 13408.271049, 20849.320095, 10129.984029, -191.301863 },
{ 18, 5861.005813, -18014.778495, 19020.343167, 443.988849 },
{ 19, -8773.575811, 12151.257210, 21698.223212, -521.650415 },
{ 20, 23127.215192, -12950.317089, 2099.622700, 363.931244 },
{ 21, 1813.458929, -26280.230144, 2143.997113, -487.414006 },
{ 22, -8635.568795, -13798.497369, 21236.345578, 401.853309 },
{ 23, -14247.163389, 7838.405009, -20901.923478, -139.410012 },
{ 24, 14174.752473, -14458.950599, 17069.494892, -6.485548 },
{ 25, 14660.477315, -16439.358633, -14817.632013, -53.973420 },
{ 26, -16096.651504, -8238.053093, -19457.243197, -87.913428 },
{ 27, -22869.968612, -10311.018093, 8885.997685, 28.370732 },
{ 28, 3626.507040, 14437.082373, 22625.094407, 475.137362 },
{ 29, 4371.948677, -15366.892825, -21211.068149, 640.063249 },
{ 30, 925.779039, 24924.887332, 9010.767707, 42.751461 },
{ 31, -8154.144932, -18631.120730, -16764.082274, 298.816917 },
{ 32, -25500.564581, 4246.936140, 4608.443236, -11.160977 }};
int main(int argc, char *argv[]) {
FILE *fp_sem, *fp_eph;
unsigned gpstime = 0;
double lat, lon, h;
int j;
gpstime = 0; // passend zu den precise-Daten
fp_sem = fopen("almanac.sem.week0843.061440.txt", "r");
//fp_eph = fopen("hour2910.15n", "r");
fp_eph = fopen("brdc2910.15n", "r");
read_SEMalmanac(fp_sem, alm);
calc_satpos_alm(alm, gpstime/1000.0, sat_alm);
read_RNXephemeris(fp_eph, eph);
calc_satpos_rnx(eph, gpstime/1000.0, sat_eph);
printf("gpstime/s: %.1f\n", gpstime/1000.0);
printf("\n");
for (j = 1; j < 33; j++) {
printf(" %2d: ", j);
printf("( %12.1f , %12.1f , %12.1f ) ", precise201510180000[j][1]*1000.0, precise201510180000[j][2]*1000.0, precise201510180000[j][3]*1000.0);
ecef2elli(precise201510180000[j][1]*1000.0, precise201510180000[j][2]*1000.0, precise201510180000[j][3]*1000.0, &lat, &lon, &h);
printf(" lat: %10.6f , lon: %11.6f , h: %11.2f\n", lat, lon, h);
printf(" %2d: ", j);
printf("( %12.1f , %12.1f , %12.1f ) ", sat_eph[j].X, sat_eph[j].Y, sat_eph[j].Z);
ecef2elli(sat_eph[j].X, sat_eph[j].Y, sat_eph[j].Z, &lat, &lon, &h);
printf(" lat: %10.6f , lon: %11.6f , h: %11.2f", lat, lon, h);
printf(" ; delta = %.1f\n", dist(sat_eph[j].X, sat_eph[j].Y, sat_eph[j].Z, precise201510180000[j][1]*1000.0, precise201510180000[j][2]*1000.0, precise201510180000[j][3]*1000.0));
printf(" %2d: ", j);
printf("( %12.1f , %12.1f , %12.1f ) ", sat_alm[j].X, sat_alm[j].Y, sat_alm[j].Z);
ecef2elli(sat_alm[j].X, sat_alm[j].Y, sat_alm[j].Z, &lat, &lon, &h);
printf(" lat: %10.6f , lon: %11.6f , h: %11.2f", lat, lon, h);
printf(" ; delta = %.1f\n", dist(sat_eph[j].X, sat_eph[j].Y, sat_eph[j].Z, sat_alm[j].X, sat_alm[j].Y, sat_alm[j].Z));
printf("\n");
}
fclose(fp_sem);
fclose(fp_eph);
return 0;
}

836
rs92/nav_gps.c 100644
Wyświetl plik

@ -0,0 +1,836 @@
/*
Quellen:
- IS-GPS-200H (bis C: ICD-GPS-200):
http://www.gps.gov/technical/icwg/
- Borre: http://kom.aau.dk/~borre
- Essential GNSS Project (hier und da etwas unklar)
*/
#define PI (3.1415926535897932384626433832795)
#define RELATIVISTIC_CLOCK_CORRECTION (-4.442807633e-10) // combined constant defined in IS-GPS-200 [s]/[sqrt(m)]
#define GRAVITY_CONSTANT (3.986005e14) // gravity constant defined on IS-GPS-200 [m^3/s^2]
#define EARTH_ROTATION_RATE (7.2921151467e-05) // IS-GPS-200 [rad/s]
#define SECONDS_IN_WEEK (604800.0) // [s]
#define LIGHTSPEED (299792458.0) // light speed constant defined in IS-GPS-200 [m/s]
#define RANGE_ESTIMATE_IN_SEC (0.072) // approx. GPS-Sat range 0.072s*299792458m/s=21585057m
#define EARTH_a (6378137.0)
#define EARTH_b (6356752.31424518)
#define EARTH_a2_b2 (EARTH_a*EARTH_a - EARTH_b*EARTH_b)
/* ---------------------------------------------------------------------------------------------------- */
void ecef2elli(double X, double Y, double Z, double *lat, double *lon, double *alt) {
double ea2 = EARTH_a2_b2 / (EARTH_a*EARTH_a),
eb2 = EARTH_a2_b2 / (EARTH_b*EARTH_b);
double phi, lam, R, p, t;
double sint, cost;
lam = atan2( Y , X );
p = sqrt( X*X + Y*Y );
t = atan2( Z*EARTH_a , p*EARTH_b );
sint = sin(t);
cost = cos(t);
phi = atan2( Z + eb2 * EARTH_b * sint*sint*sint ,
p - ea2 * EARTH_a * cost*cost*cost );
R = EARTH_a / sqrt( 1 - ea2*sin(phi)*sin(phi) );
*alt = p / cos(phi) - R;
*lat = phi*180.0/PI;
*lon = lam*180.0/PI;
}
double dist(double X1, double Y1, double Z1, double X2, double Y2, double Z2) {
return sqrt( (X2-X1)*(X2-X1) + (Y2-Y1)*(Y2-Y1) + (Z2-Z1)*(Z2-Z1) );
}
void rotZ(double x1, double y1, double z1, double angle, double *x2, double *y2, double *z2) {
double cosa = cos(angle);
double sina = sin(angle);
*x2 = cosa * x1 + sina * y1;
*y2 = -sina * x1 + cosa * y1;
*z2 = z1;
}
/* ---------------------------------------------------------------------------------------------------- */
typedef struct {
ui16_t prn;
ui16_t week;
ui32_t toa;
char epoch[15];
double toe;
double toc;
double e;
double delta_n;
double delta_i;
double i0;
double OmegaDot;
double sqrta;
double Omega0;
double w;
double M0;
double tgd;
double idot;
double cuc;
double cus;
double crc;
double crs;
double cic;
double cis;
double af0;
double af1;
double af2;
int gpsweek;
ui16_t svn;
ui8_t ura;
ui8_t health;
ui8_t conf;
} EPHEM_t;
typedef struct {
ui32_t t;
double pseudorange;
double clock_corr;
double X;
double Y;
double Z;
} SAT_t;
/* ---------------------------------------------------------------------------------------------------- */
int read_SEMalmanac(FILE *fp, EPHEM_t *alm) {
int l, j;
char buf[64];
unsigned n, week, toa, ui;
double dbl;
l = fscanf(fp, "%u", &n); if (l != 1) return -1;
l = fscanf(fp, "%s", buf); if (l != 1) return -1;
l = fscanf(fp, "%u", &week); if (l != 1) return -1;
l = fscanf(fp, "%u", &toa); if (l != 1) return -1;
for (j = 1; j <= n; j++) {
//memset(&ephem, 0, sizeof(ephem));
alm[j].week = (ui16_t)week;
alm[j].toa = (ui32_t)toa;
alm[j].toe = (double)toa;
alm[j].toc = alm[j].toe;
l = fscanf(fp, "%u", &ui); if (l != 1) return -1; alm[j].prn = (ui16_t)ui;
l = fscanf(fp, "%u", &ui); if (l != 1) return -2; alm[j].svn = (ui16_t)ui;
l = fscanf(fp, "%u", &ui); if (l != 1) return -3; alm[j].ura = (ui8_t)ui;
l = fscanf(fp, "%lf", &dbl); if (l != 1) return -4; alm[j].e = dbl;
l = fscanf(fp, "%lf", &dbl); if (l != 1) return -5; alm[j].delta_i = dbl;
alm[j].i0 = (0.30 + alm[j].delta_i) * PI;
l = fscanf(fp, "%lf", &dbl); if (l != 1) return -6; alm[j].OmegaDot = dbl * PI;
l = fscanf(fp, "%lf", &dbl); if (l != 1) return -7; alm[j].sqrta = dbl;
l = fscanf(fp, "%lf", &dbl); if (l != 1) return -6; alm[j].Omega0 = dbl * PI;
l = fscanf(fp, "%lf", &dbl); if (l != 1) return -8; alm[j].w = dbl * PI;
l = fscanf(fp, "%lf", &dbl); if (l != 1) return -9; alm[j].M0 = dbl * PI;
l = fscanf(fp, "%lf", &dbl); if (l != 1) return -10; alm[j].af0 = dbl;
l = fscanf(fp, "%lf", &dbl); if (l != 1) return -11; alm[j].af1 = dbl;
alm[j].af2 = 0;
alm[j].crc = 0;
alm[j].crs = 0;
alm[j].cuc = 0;
alm[j].cus = 0;
alm[j].cic = 0;
alm[j].cis = 0;
alm[j].tgd = 0;
alm[j].idot = 0;
alm[j].delta_n = 0;
l = fscanf(fp, "%u", &ui); if (l != 1) return -12; alm[j].health = (ui8_t)ui;
l = fscanf(fp, "%u", &ui); if (l != 1) return -13; alm[j].conf = (ui8_t)ui;
}
return 0;
}
int read_RNXephemeris(FILE *fp, EPHEM_t eph[][24]) {
int l, i;
char buf[64], str[20];
char buf_header[81];
//buf_data[80]; // 3 + 4*19 = 79
unsigned ui;
double dbl;
int c;
EPHEM_t ephem = {};
int hr = 0;
do {
l = fread(buf_header, 81, 1, fp); // besser fgets()? was wenn \r\n? fopen in textmode
buf_header[80] = '\0'; // \n durch \0 ueberschreiben
} while ( l == 1 && !strstr(buf_header, "END OF HEADER") );
if (l != 1) return -1;
//l = fread(buf_data, 80, 1, fp);
//buf_data[79] = '\0';
while (hr < 24) {
//memset(&ephem, 0, sizeof(ephem));
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; sscanf(buf, "%d", &ui);
ephem.prn = ui;
for (i = 0; i < 14; i++) ephem.epoch[i] = '0';
ephem.epoch[14] = '\0';
l = fread(buf, 19, 1, fp); if (l != 1) break; buf[19] = 0;
strncpy(ephem.epoch , "20", 2);
for (i = 0; i < 6; i++) {
c = buf[3*i ]; if (c == ' ') c = '0'; str[2*i ] = c;
c = buf[3*i+1]; if (c == ' ') c = '0'; str[2*i+1] = c;
}
str[12] = buf[17];
str[13] = buf[18];
str[14] = '\0';
strncpy(ephem.epoch, str, 15);
strncpy(str, buf+9, 2); str[2] = '\0';
hr = atoi(str);
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af0 = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af1 = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af2 = dbl;
if ((c=fgetc(fp)) == EOF) break;
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iode = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.crs = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.delta_n = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.M0 = dbl;
if ((c=fgetc(fp)) == EOF) break;
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cuc = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.e = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cus = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.sqrta = dbl;
if ((c=fgetc(fp)) == EOF) break;
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.toe = dbl;
ephem.toc = ephem.toe;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cic = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.Omega0 = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cis = dbl;
if ((c=fgetc(fp)) == EOF) break; //if (c != 0x0a) printf("%c", c);
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.i0 = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.crc = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.w = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.OmegaDot = dbl;
if ((c=fgetc(fp)) == EOF) break;
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.idot = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.codeL2 = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.gpsweek = (int)dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iodc = dbl;
if ((c=fgetc(fp)) == EOF) break;
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.sva = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.svh = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.tgd = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iodc = dbl;
if ((c=fgetc(fp)) == EOF) break;
l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.ttom = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.fit = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.spare1 = dbl;
l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.spare2 = dbl;
if ((c=fgetc(fp)) == EOF) break;
ephem.week = 1; // ephem.gpsweek
eph[ephem.prn][hr] = ephem;
}
return 0;
}
/* ---------------------------------------------------------------------------------------------------- */
void GPS_SatelliteClockCorrection(
const unsigned short transmission_gpsweek, // GPS week when signal was transmit (0-1024+) [weeks]
const double transmission_gpstow, // GPS time of week when signal was transmit [s]
const unsigned short ephem_week, // ephemeris: GPS week (0-1024+) [weeks]
const double toe, // ephemeris: time of week [s]
const double toc, // ephemeris: clock reference time of week [s]
const double af0, // ephemeris: polynomial clock correction coefficient [s],
const double af1, // ephemeris: polynomial clock correction coefficient [s/s],
const double af2, // ephemeris: polynomial clock correction coefficient [s/s^2]
const double ecc, // ephemeris: eccentricity of satellite orbit []
const double sqrta, // ephemeris: square root of the semi-major axis of orbit [m^(1/2)]
const double delta_n, // ephemeris: mean motion difference from computed value [rad]
const double m0, // ephemeris: mean anomaly at reference time [rad]
const double tgd, // ephemeris: group delay differential between L1 and L2 [s]
double* clock_correction ) // ephemeris: satellite clock correction [m]
{
int j;
double tot; // time of transmission (including gps week) [s]
double tk; // time from ephemeris reference epoch [s]
double tc; // time from clock reference epoch [s]
double d_tr; // relativistic correction term [s]
double d_tsv; // SV PRN code phase time offset [s]
double a; // semi-major axis of orbit [m]
double n; // corrected mean motion [rad/s]
double M; // mean anomaly, [rad]
double E; // eccentric anomaly [rad]
// compute the times from the reference epochs
tot = transmission_gpsweek*SECONDS_IN_WEEK + transmission_gpstow;
tk = tot - (ephem_week*SECONDS_IN_WEEK + toe);
tc = tot - (ephem_week*SECONDS_IN_WEEK + toc);
// compute the corrected mean motion term
a = sqrta*sqrta;
n = sqrt( GRAVITY_CONSTANT / (a*a*a) ); // mean motion
n += delta_n; // corrected mean motion
// Kepler-Gleichung M = E - e sin(E)
M = m0 + n*tk; // mean anomaly
E = M; // f(E) = M + e sin(E) hat Fixpunkt fuer e < 1,
for( j = 0; j < 7; j++ ) { // da |f'(E)|=|e cos(E)|<1.
E = M + ecc * sin(E); // waehle Startwert E_1 = M, E_{k+1} = f(E_k)
} // konvergiert langsam gegen E_0 = f(E_0)
// relativistic correction
d_tr = RELATIVISTIC_CLOCK_CORRECTION * ecc * sqrta * sin(E); // [s]
d_tr *= LIGHTSPEED;
// clock correcton
d_tsv = af0 + af1*tc + af2*tc*tc; // [s]
// L1 only
d_tsv -= tgd; // [s]
// clock correction
*clock_correction = d_tsv*LIGHTSPEED + d_tr; // [m]
}
void GPS_ComputeSatellitePosition(
const unsigned short transmission_gpsweek, // GPS week when signal was transmit (0-1024+) [weeks]
const double transmission_gpstow, // GPS time of week when signal was transmit [s]
const unsigned short ephem_week, // ephemeris: GPS week (0-1024+) [weeks]
const double toe, // ephemeris: time of week [s]
const double m0, // ephemeris: mean anomaly at reference time [rad]
const double delta_n, // ephemeris: mean motion difference from computed value [rad]
const double ecc, // ephemeris: eccentricity []
const double sqrta, // ephemeris: square root of the semi-major axis [m^(1/2)]
const double omega0, // ephemeris: longitude of ascending node of orbit plane at weekly epoch [rad]
const double i0, // ephemeris: inclination angle at reference time [rad]
const double w, // ephemeris: argument of perigee [rad]
const double omegadot, // ephemeris: rate of right ascension [rad/s]
const double idot, // ephemeris: rate of inclination angle [rad/s]
const double cuc, // ephemeris: cos harmonic correction term to the argument of latitude [rad]
const double cus, // ephemeris: sin harmonic correction term to the argument of latitude [rad]
const double crc, // ephemeris: cos harmonic correction term to the orbit radius [m]
const double crs, // ephemeris: sin harmonic correction term to the orbit radius [m]
const double cic, // ephemeris: cos harmonic correction term to the angle of inclination [rad]
const double cis, // ephemeris: sin harmonic correction term to the angle of inclination [rad]
double* x, // satellite x [m]
double* y, // satellite y [m]
double* z ) // satellite z [m]
{
int j;
double tot; // time of transmission (including gps week) [s]
double tk; // time from ephemeris reference epoch [s]
double a; // semi-major axis of orbit [m]
double n; // corrected mean motion [rad/s]
double M; // mean anomaly, [rad]
double E; // eccentric anomaly [rad]
double v; // true anomaly [rad]
double u; // argument of latitude, corrected [rad]
double r; // radius in the orbital plane [m]
double i; // orbital inclination [rad]
double cos2u; // cos(2*u) []
double sin2u; // sin(2*u) []
double d_u; // argument of latitude correction [rad]
double d_r; // radius correction [m]
double d_i; // inclination correction [rad]
double x_op; // x position in the orbital plane [m]
double y_op; // y position in the orbital plane [m]
double omegak; // corrected longitude of the ascending node [rad]
double cos_omegak; // cos(omegak)
double sin_omegak; // sin(omegak)
double cosu; // cos(u)
double sinu; // sin(u)
double cosi; // cos(i)
double sini; // sin(i)
double cosE; // cos(E)
double sinE; // sin(E)
// compute the times from the reference epochs
tot = transmission_gpsweek*SECONDS_IN_WEEK + transmission_gpstow;
tk = tot - (ephem_week*SECONDS_IN_WEEK + toe);
// compute the corrected mean motion term
a = sqrta*sqrta;
n = sqrt( GRAVITY_CONSTANT / (a*a*a) ); // computed mean motion
n += delta_n; // corrected mean motion
// Kepler-Gleichung M = E - e sin(E)
M = m0 + n*tk; // mean anomaly
E = M; // f(E) = M + e sin(E) hat Fixpunkt fuer e < 1,
for( j = 0; j < 7; j++ ) { // da |f'(E)|=|e cos(E)|<1.
E = M + ecc * sin(E); // waehle Startwert E_1 = M, E_{k+1} = f(E_k)
} // konvergiert langsam gegen E_0 = f(E_0)
cosE = cos(E);
sinE = sin(E);
// true anomaly
v = atan2( (sqrt(1.0 - ecc*ecc)*sinE), (cosE - ecc) );
// argument of latitude
u = v + w;
// radius in orbital plane
r = a * (1.0 - ecc * cos(E));
// orbital inclination
i = i0;
// second harmonic perturbations
//
cos2u = cos(2.0*u);
sin2u = sin(2.0*u);
// argument of latitude correction
d_u = cuc * cos2u + cus * sin2u;
// radius correction
d_r = crc * cos2u + crs * sin2u;
// correction to inclination
d_i = cic * cos2u + cis * sin2u;
// corrected argument of latitude
u += d_u;
// corrected radius
r += d_r;
// corrected inclination
i += d_i + idot * tk;
// positions in orbital plane
cosu = cos(u);
sinu = sin(u);
x_op = r * cosu;
y_op = r * sinu;
omegak = omega0 + omegadot*tk - EARTH_ROTATION_RATE*(tk + toe);
// fuer Bestimmung der Satellitenposition in ECEF, range=0;
// fuer GPS-Loesung (Sats in ECI) Erdrotation hinzuziehen:
// rotZ(EARTH_ROTATION_RATE*0.072), 0.072s*299792458m/s=21585057m
// compute the WGS84 ECEF coordinates,
// vector r with components x & y is now rotated using, R3(-omegak)*R1(-i)
cos_omegak = cos(omegak);
sin_omegak = sin(omegak);
cosi = cos(i);
sini = sin(i);
*x = x_op * cos_omegak - y_op * sin_omegak * cosi;
*y = x_op * sin_omegak + y_op * cos_omegak * cosi;
*z = y_op * sini;
}
void GPS_SatellitePosition_Ephem(
const unsigned short gpsweek, // gps week of signal transmission (0-1024+) [week]
const double gpstow, // time of week of signal transmission (gpstow-psr/c) [s]
EPHEM_t ephem,
double* clock_correction, // clock correction for this satellite for this epoch [m]
double* satX, // satellite X position WGS84 ECEF [m]
double* satY, // satellite Y position WGS84 ECEF [m]
double* satZ // satellite Z position WGS84 ECEF [m]
)
{
double tow; // user time of week adjusted with the clock corrections [s]
double x; // sat X position [m]
double y; // sat Y position [m]
double z; // sat Z position [m]
unsigned short week; // user week adjusted with the clock correction if needed [week]
x = y = z = 0.0;
GPS_SatelliteClockCorrection( gpsweek, gpstow,
ephem.week, ephem.toe, ephem.toc, ephem.af0,
ephem.af1, ephem.af2, ephem.e, ephem.sqrta,
ephem.delta_n, ephem.M0, ephem.tgd, clock_correction );
// adjust for week rollover
week = gpsweek;
tow = gpstow + (*clock_correction)/LIGHTSPEED;
if ( tow < 0.0 ) {
tow += SECONDS_IN_WEEK;
week--;
}
if ( tow > 604800.0 ) {
tow -= SECONDS_IN_WEEK;
week++;
}
//range = 0.072s*299792458m/s=21585057m
GPS_ComputeSatellitePosition( week, tow,
ephem.week, ephem.toe, ephem.M0, ephem.delta_n, ephem.e, ephem.sqrta,
ephem.Omega0, ephem.i0, ephem.w, ephem.OmegaDot, ephem.idot,
ephem.cuc, ephem.cus, ephem.crc, ephem.crs, ephem.cic, ephem.cis,
&x, &y, &z );
*satX = x;
*satY = y;
*satZ = z;
}
/* ---------------------------------------------------------------------------------------------------- */
int NAV_ClosedFormPositionSolution_FromPseudorange(
SAT_t sat1, SAT_t sat2, SAT_t sat3, SAT_t sat4,
double* latitude, // ellipsoid latitude [rad]
double* longitude, // ellipsoid longitude [rad]
double* height, // ellipsoid height [m]
double* rx_clock_bias, // receiver clock bias [m]
double pos_ecef[3] ) // ECEF coordinates [m]
{
double p1 = sat1.pseudorange + sat1.clock_corr; // pseudorange measurement + clock correction [m]
double p2 = sat2.pseudorange + sat2.clock_corr;
double p3 = sat3.pseudorange + sat3.clock_corr;
double p4 = sat4.pseudorange + sat4.clock_corr;
double x1, y1, z1; // Sat1 ECEF
double x2, y2, z2; // Sat2 ECEF
double x3, y3, z3; // Sat3 ECEF
double x4, y4, z4; // Sat4 ECEF
// Erdrotation ECEF-ECI, 0.070s*299792458m/s=20985472m, 0.072s*299792458m/s=21585057m
rotZ(sat1.X, sat1.Y, sat1.Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE_IN_SEC, &x1, &y1, &z1);
rotZ(sat2.X, sat2.Y, sat2.Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE_IN_SEC, &x2, &y2, &z2);
rotZ(sat3.X, sat3.Y, sat3.Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE_IN_SEC, &x3, &y3, &z3);
rotZ(sat4.X, sat4.Y, sat4.Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE_IN_SEC, &x4, &y4, &z4);
double x12, x13, x14; // 'xij' = 'xi' - 'xj' [m]
double y12, y13, y14;
double z12, z13, z14;
double p21, p31, p41; // 'pij' = 'pi' - 'pj' [m]
double k1, k2, k3; // coefficients
double c1, c2, c3;
double f1, f2, f3;
double m1, m2, m3;
double d1; // clock bias, solution 1 [m]
double d2; // clock bias, solution 2 [m]
double detA; // determinant of A
double tmp1;
double tmp2;
double tmp3;
double A[3][3];
double D[3][3]; // D is A^-1 * detA
typedef struct {
double x;
double y;
double z;
} struct_SOLN;
struct_SOLN s1;
struct_SOLN s2;
struct_SOLN stmp;
double dtmp;
double x0, y0, z0;
double latS, lonS, altS,
lat1, lon1, alt1,
lat2, lon2, alt2;
double d2_1, d2_2;
x12 = x1 - x2;
x13 = x1 - x3;
x14 = x1 - x4;
y12 = y1 - y2;
y13 = y1 - y3;
y14 = y1 - y4;
z12 = z1 - z2;
z13 = z1 - z3;
z14 = z1 - z4;
p21 = p2 - p1;
p31 = p3 - p1;
p41 = p4 - p1;
k1 = x12*x12 + y12*y12 + z12*z12 - p21*p21;
k2 = x13*x13 + y13*y13 + z13*z13 - p31*p31;
k3 = x14*x14 + y14*y14 + z14*z14 - p41*p41;
A[0][0] = 2.0*x12;
A[1][0] = 2.0*x13;
A[2][0] = 2.0*x14;
A[0][1] = 2.0*y12;
A[1][1] = 2.0*y13;
A[2][1] = 2.0*y14;
A[0][2] = 2.0*z12;
A[1][2] = 2.0*z13;
A[2][2] = 2.0*z14;
tmp1 = A[1][1]*A[2][2] - A[2][1]*A[1][2];
tmp2 = A[1][0]*A[2][2] - A[2][0]*A[1][2];
tmp3 = A[1][0]*A[2][1] - A[2][0]*A[1][1];
detA = A[0][0]*tmp1 - A[0][1]*tmp2 + A[0][2]*tmp3;
D[0][0] = tmp1;
D[1][0] = -tmp2;
D[2][0] = tmp3;
D[0][1] = -A[0][1]*A[2][2] + A[2][1]*A[0][2];
D[1][1] = A[0][0]*A[2][2] - A[2][0]*A[0][2];
D[2][1] = -A[0][0]*A[2][1] + A[2][0]*A[0][1];
D[0][2] = A[0][1]*A[1][2] - A[1][1]*A[0][2];
D[1][2] = -A[0][0]*A[1][2] + A[1][0]*A[0][2];
D[2][2] = A[0][0]*A[1][1] - A[1][0]*A[0][1];
c1 = (D[0][0]*p21 + D[0][1]*p31 + D[0][2]*p41) * 2.0 / detA;
c2 = (D[1][0]*p21 + D[1][1]*p31 + D[1][2]*p41) * 2.0 / detA;
c3 = (D[2][0]*p21 + D[2][1]*p31 + D[2][2]*p41) * 2.0 / detA;
f1 = (D[0][0]*k1 + D[0][1]*k2 + D[0][2]*k3) / detA;
f2 = (D[1][0]*k1 + D[1][1]*k2 + D[1][2]*k3) / detA;
f3 = (D[2][0]*k1 + D[2][1]*k2 + D[2][2]*k3) / detA;
m1 = c1*c1 + c2*c2 + c3*c3 - 1.0;
m2 = -2.0*( c1*f1 + c2*f2 + c3*f3 );
m3 = f1*f1 + f2*f2 + f3*f3;
tmp1 = m2*m2 - 4.0*m1*m3;
if ( tmp1 < 0 ) { // not good, there is no solution
return 0; //FALSE
}
d1 = ( -m2 - sqrt( tmp1 ) ) * 0.5 / m1; // +-Reihenfolge vertauscht
d2 = ( -m2 + sqrt( tmp1 ) ) * 0.5 / m1;
// two solutions
s1.x = c1*d1 - f1 + x1;
s1.y = c2*d1 - f2 + y1;
s1.z = c3*d1 - f3 + z1;
s2.x = c1*d2 - f1 + x1;
s2.y = c2*d2 - f2 + y1;
s2.z = c3*d2 - f3 + z1;
ecef2elli( s1.x, s1.y, s1.z, &lat1, &lon1, &alt1 );
*latitude = lat1;
*longitude = lon1;
*height = alt1;
tmp1 = sqrt( s1.x*s1.x + s1.y*s1.y + s1.z*s1.z );
tmp2 = sqrt( s2.x*s2.x + s2.y*s2.y + s2.z*s2.z );
// choose the correct solution based
// on whichever solution is closest to
// the Earth's surface
tmp1 = fabs( tmp1 - 6371000.0 );
tmp2 = fabs( tmp2 - 6371000.0 );
// geht manchmal schief
// wenn beide Werte nahe Erdoberflaeche
// ungefaehre Position kann aus den Positionen der empfangen Sats abgeleitet werden
if ( tmp2 < tmp1 /*alt2 < alt1 && alt2 > -1500*/ && tmp1 < 60000 )
{
x0 = (x1+x2+x3+x4)/4.0;
y0 = (y1+y2+y3+y4)/4.0;
z0 = (z1+z2+z3+z4)/4.0;
ecef2elli( x0, y0, z0, &latS, &lonS, &altS );
ecef2elli( s2.x, s2.y, s2.z, &lat2, &lon2, &alt2 );
d2_1 = sqrt( (latS-lat1)*(latS-lat1) + (lonS-lon1)*(lonS-lon1) );
d2_2 = sqrt( (latS-lat2)*(latS-lat2) + (lonS-lon2)*(lonS-lon2) );
if ( d2_2 < d2_1 ) {
stmp = s1; s1 = s2; s2 = stmp; // s1 = s2;
dtmp = d1; d1 = d2; d2 = dtmp; // d1 = d2;
*latitude = lat2;
*longitude = lon2;
*height = alt2;
}
}
*rx_clock_bias = d1;
pos_ecef[0] = s1.x;
pos_ecef[1] = s1.y;
pos_ecef[2] = s1.z;
if( *height < -1500.0 || *height > 50000.0 ) {
return 0; //FALSE
}
return 1; //TRUE
}
/* ---------------------------------------------------------------------------------------------------- */
int matrix_invert(double mat[4][4], double inv[4][4])
/* selected elements from 4x4 matrox inversion */
{
// Find all NECESSARY 2x2 subdeterminants
double Det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
double Det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0];
//double Det2_12_03 = mat[1][0]*mat[2][3] - mat[1][3]*mat[2][0];
double Det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
//double Det2_12_13 = mat[1][1]*mat[2][3] - mat[1][3]*mat[2][1];
//double Det2_12_23 = mat[1][2]*mat[2][3] - mat[1][3]*mat[2][2];
double Det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
//double Det2_13_02 = mat[1][0]*mat[3][2] - mat[1][2]*mat[3][0];
double Det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
//double Det2_13_12 = mat[1][1]*mat[3][2] - mat[1][2]*mat[3][1];
double Det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
//double Det2_13_23 = mat[1][2]*mat[3][3] - mat[1][3]*mat[3][2];
double Det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0];
double Det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0];
double Det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0];
double Det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1];
double Det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1];
double Det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2];
// Find all NECESSARY 3x3 subdeterminants
double Det3_012_012 = mat[0][0] * Det2_12_12 - mat[0][1] * Det2_12_02 + mat[0][2] * Det2_12_01;
//double Det3_012_013 = mat[0][0]*Det2_12_13 - mat[0][1]*Det2_12_03 + mat[0][3]*Det2_12_01;
//double Det3_012_023 = mat[0][0]*Det2_12_23 - mat[0][2]*Det2_12_03 + mat[0][3]*Det2_12_02;
//double Det3_012_123 = mat[0][1]*Det2_12_23 - mat[0][2]*Det2_12_13 + mat[0][3]*Det2_12_12;
//double Det3_013_012 = mat[0][0]*Det2_13_12 - mat[0][1]*Det2_13_02 + mat[0][2]*Det2_13_01;
double Det3_013_013 = mat[0][0] * Det2_13_13 - mat[0][1] * Det2_13_03 + mat[0][3] * Det2_13_01;
//double Det3_013_023 = mat[0][0]*Det2_13_23 - mat[0][2]*Det2_13_03 + mat[0][3]*Det2_13_02;
//double Det3_013_123 = mat[0][1]*Det2_13_23 - mat[0][2]*Det2_13_13 + mat[0][3]*Det2_13_12;
//double Det3_023_012 = mat[0][0]*Det2_23_12 - mat[0][1]*Det2_23_02 + mat[0][2]*Det2_23_01;
//double Det3_023_013 = mat[0][0]*Det2_23_13 - mat[0][1]*Det2_23_03 + mat[0][3]*Det2_23_01;
double Det3_023_023 = mat[0][0] * Det2_23_23 - mat[0][2] * Det2_23_03 + mat[0][3] * Det2_23_02;
//double Det3_023_123 = mat[0][1]*Det2_23_23 - mat[0][2]*Det2_23_13 + mat[0][3]*Det2_23_12;
double Det3_123_012 = mat[1][0] * Det2_23_12 - mat[1][1] * Det2_23_02 + mat[1][2] * Det2_23_01;
double Det3_123_013 = mat[1][0] * Det2_23_13 - mat[1][1] * Det2_23_03 + mat[1][3] * Det2_23_01;
double Det3_123_023 = mat[1][0] * Det2_23_23 - mat[1][2] * Det2_23_03 + mat[1][3] * Det2_23_02;
double Det3_123_123 = mat[1][1] * Det2_23_23 - mat[1][2] * Det2_23_13 + mat[1][3] * Det2_23_12;
// Find the 4x4 determinant
static double det;
det = mat[0][0] * Det3_123_123
- mat[0][1] * Det3_123_023
+ mat[0][2] * Det3_123_013
- mat[0][3] * Det3_123_012;
// Very small determinants probably reflect floating-point fuzz near zero
if (fabs(det) < 0.0001)
return -1;
inv[0][0] = Det3_123_123 / det;
//inv[0][1] = -Det3_023_123 / det;
//inv[0][2] = Det3_013_123 / det;
//inv[0][3] = -Det3_012_123 / det;
//inv[1][0] = -Det3_123_023 / det;
inv[1][1] = Det3_023_023 / det;
//inv[1][2] = -Det3_013_023 / det;
//inv[1][3] = Det3_012_023 / det;
//inv[2][0] = Det3_123_013 / det;
//inv[2][1] = -Det3_023_013 / det;
inv[2][2] = Det3_013_013 / det;
//inv[2][3] = -Det3_012_013 / det;
//inv[3][0] = -Det3_123_012 / det;
//inv[3][1] = Det3_023_012 / det;
//inv[3][2] = -Det3_013_012 / det;
inv[3][3] = Det3_012_012 / det;
return 0;
}
int calculate_DOP(SAT_t sat0, SAT_t sat1, SAT_t sat2, SAT_t sat3, double pos_ecef[3], double DOP[4][4]) {
int i, j, k;
double norm[4], satpos[4][3];
double SATS[4][4], AtA[4][4];
satpos[0][0] = sat0.X-pos_ecef[0];
satpos[0][1] = sat0.Y-pos_ecef[1];
satpos[0][2] = sat0.Z-pos_ecef[2];
satpos[1][0] = sat1.X-pos_ecef[0];
satpos[1][1] = sat1.Y-pos_ecef[1];
satpos[1][2] = sat1.Z-pos_ecef[2];
satpos[2][0] = sat2.X-pos_ecef[0];
satpos[2][1] = sat2.Y-pos_ecef[1];
satpos[2][2] = sat2.Z-pos_ecef[2];
satpos[3][0] = sat3.X-pos_ecef[0];
satpos[3][1] = sat3.Y-pos_ecef[1];
satpos[3][2] = sat3.Z-pos_ecef[2];
for (i = 0; i < 4; i++) {
norm[i] = sqrt( satpos[i][0]*satpos[i][0] + satpos[i][1]*satpos[i][1] + satpos[i][2]*satpos[i][2] );
for (j = 0; j < 3; j++) {
SATS[i][j] = satpos[i][j] / norm[i];
}
SATS[i][3] = 1;
}
for (i = 0; i < 4; i++) {
for (j = 0; j < 4; j++) {
AtA[i][j] = 0.0;
for (k = 0; k < 4; k++) {
AtA[i][j] += SATS[k][i]*SATS[k][j];
}
}
}
return matrix_invert(AtA, DOP);
}

BIN
rs92/nga18670.Z 100644
Wyświetl plik

Plik binarny nie jest wyświetlany.