kopia lustrzana https://github.com/rs1729/RS
GPS ephemeris
rodzic
289b646802
commit
272e805638
|
@ -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
|
||||||
|
|
Plik diff jest za duży
Load Diff
|
@ -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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
}
|
||||||
|
|
Plik binarny nie jest wyświetlany.
Ładowanie…
Reference in New Issue