/* 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 (0.072) // approx. GPS-Sat range 0.072s*299792458m/s=21585057m #define RANGERATE_ESTIMATE (0.000) // #define EARTH_a (6378137.0) #define EARTH_b (6356752.31424518) #define EARTH_a2_b2 (EARTH_a*EARTH_a - EARTH_b*EARTH_b) /* ---------------------------------------------------------------------------------------------------- */ typedef struct { ui16_t prn; ui16_t week; ui32_t toa; char epoch[20]; 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 tow; double pseudorange; double pseudorate; double clock_corr; double clock_drift; double X; double Y; double Z; double vX; double vY; double vZ; int ephhr; double PR; double ephtime; int prn; ui8_t status; } SAT_t; typedef struct {double X; double Y; double Z;} LOC_t; typedef struct {double X; double Y; double Z; double vX; double vY; double vZ;} VEL_t; static void GPS_SatellitePositionVelocity_Ephem( const unsigned short , const double , EPHEM_t , double* , double* , double* , double* , double* , double* , double* , double* ); /* ---------------------------------------------------------------------------------------------------- */ static int 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; return 0; } static 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) ); } static 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; } /* ---------------------------------------------------------------------------------------------------- */ 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; } EPHEM_t *read_RNXpephs(FILE *fp) { int l, i, n; char buffer[83]; char buf[64], str[20]; char *pbuf; unsigned ui; double dbl; int c; EPHEM_t ephem = {}, *te = NULL; int count = 0; long fpos; do { // header-Zeilen: 80 Zeichen pbuf = fgets(buffer, 82, fp); // max 82-1 Zeichen + '\0' buffer[82] = '\0'; // doppelt haelt besser } while ( pbuf && !strstr(buffer, "END OF HEADER") ); if (pbuf == NULL) return NULL; fpos = ftell(fp); count = 0; while (count >= 0) { // data-Zeilen: 79 Zeichen pbuf = fgets(buffer, 82, fp); if (pbuf == 0) break; strncpy(str, buffer, 3); str[3] = '\0'; sscanf(str, "%d", &ui); if (ui < 33) count++; for (i = 0; i < 7; i++) { pbuf = fgets(buffer, 82, fp); if (pbuf == 0) break; } } //printf("Ephemerides: %d\n", count); fseek(fp, fpos, SEEK_SET); te = calloc( count+1, sizeof(ephem) ); // calloc( 1, sizeof(ephem) ); if (te == NULL) return NULL; n = 0; while (count > 0) { // brdc/hour-rinex sollte nur Daten von einem Tag enthalten //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 < 16; i++) ephem.epoch[i] = '0'; ephem.epoch[16] = '\0'; l = fread(buf, 19, 1, fp); if (l != 1) break; buf[19] = 0; 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 , "20", 2); // vorausgesetzt 21.Jhd; Datum steht auch im Header strncpy(ephem.epoch+2, str, 15); ephem.epoch[16] = '\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.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; 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; pbuf = fgets(buffer, 82, fp); /* // die letzten beiden Felder (spare) sind manchmal leer (statt 0.00); manchmal fehlt sogar das drittletzte Feld 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 te[n] = ephem; n += 1; //tmp = realloc( te, (count+1) * sizeof(ephem) ); //if (tmp == NULL) break; //te = tmp; if (pbuf == NULL) break; } te[n].prn = 0; return te; } /* ---------------------------------------------------------------------------------------------------- */ static int trace_invert(double mat[4][4], double trinv[4]) // trace-invert /* selected elements from 4x4 matrix 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; trinv[0] = Det3_123_123 / det; trinv[1] = Det3_023_023 / det; trinv[2] = Det3_013_013 / det; trinv[3] = Det3_012_012 / det; return 0; } int calc_DOPn(int n, SAT_t satss[], double pos_ecef[3], double DOP[4]) { int i, j, k; double norm[n], satpos[n][3]; double SATS[n][4], AtA[4][4]; for (i = 0; i < n; i++) { satpos[i][0] = satss[i].X-pos_ecef[0]; satpos[i][1] = satss[i].Y-pos_ecef[1]; satpos[i][2] = satss[i].Z-pos_ecef[2]; } for (i = 0; i < n; 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 < n; k++) { AtA[i][j] += SATS[k][i]*SATS[k][j]; } } } return trace_invert(AtA, DOP); } /* ---------------------------------------------------------------------------------------------------- */ static int rotE(SAT_t sat, double *x, double *y, double *z) { // Erdrotation ECEF-ECI, 0.070s*299792458m/s=20985472m, 0.072s*299792458m/s=21585057m double range = sat.PR/LIGHTSPEED; if (range < 19e6 || range > 30e6) range = 21e6; rotZ(sat.X, sat.Y, sat.Z, EARTH_ROTATION_RATE*(range/LIGHTSPEED), x, y, z); return 0; } static double lorentz(double a[4], double b[4]) { return a[0]*b[0] + a[1]*b[1] +a[2]*b[2] - a[3]*b[3]; } static int matrix_invert(double mat[4][4], double inv[4][4]) { // 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]; // 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; // 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 NAV_bancroft1(int N, SAT_t sats[], double pos_ecef[3], double *cc) { int i, j, k; double B[N][4], BtB[4][4], BBinv[4][4], BBB[4][N]; double a[N], Be[4], Ba[4]; double q0, q1, q2, p, q, sq, x1, x2; double Lsg1[4], Lsg2[4]; double tmp1, tmp2; double X, Y, Z; if (N < 4 || N > 12) return -1; for (i = 0; i < N; i++) { rotZ(sats[i].X, sats[i].Y, sats[i].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE, B[i], B[i]+1, B[i]+2); B[i][3] = sats[i].pseudorange + sats[i].clock_corr; } if (N == 4) { matrix_invert(B, BBB); } else { for (i = 0; i < 4; i++) { for (j = 0; j < 4; j++) { BtB[i][j] = 0.0; for (k = 0; k < N; k++) { BtB[i][j] += B[k][i]*B[k][j]; } } } matrix_invert(BtB, BBinv); for (i = 0; i < 4; i++) { for (j = 0; j < N; j++) { BBB[i][j] = 0.0; for (k = 0; k < 4; k++) { BBB[i][j] += BBinv[i][k]*B[j][k]; } } } } for (i = 0; i < 4; i++) { Be[i] = 0.0; for (k = 0; k < N; k++) { Be[i] += BBB[i][k]*1.0; } } for (i = 0; i < N; i++) { a[i] = 0.5 * lorentz(B[i], B[i]); } for (i = 0; i < 4; i++) { Ba[i] = 0.0; for (k = 0; k < N; k++) { Ba[i] += BBB[i][k]*a[k]; } } q2 = lorentz(Be, Be); q1 = lorentz(Ba, Be)-1; q0 = lorentz(Ba, Ba); if (q2 == 0) return -2; p = q1/q2; q = q0/q2; sq = p*p - q; if (sq < 0) return -2; x1 = -p + sqrt(sq); x2 = -p - sqrt(sq); for (i = 0; i < 4; i++) { Lsg1[i] = x1*Be[i]+Ba[i]; Lsg2[i] = x2*Be[i]+Ba[i]; } Lsg1[3] = -Lsg1[3]; Lsg2[3] = -Lsg2[3]; tmp1 = sqrt( Lsg1[0]*Lsg1[0] + Lsg1[1]*Lsg1[1] + Lsg1[2]*Lsg1[2] ); tmp2 = sqrt( Lsg2[0]*Lsg2[0] + Lsg2[1]*Lsg2[1] + Lsg2[2]*Lsg2[2] ); tmp1 = fabs( tmp1 - 6371000.0 ); tmp2 = fabs( tmp2 - 6371000.0 ); if (tmp1 < tmp2) { X = Lsg1[0]; Y = Lsg1[1]; Z = Lsg1[2]; *cc = Lsg1[3]; } else { X = Lsg2[0]; Y = Lsg2[1]; Z = Lsg2[2]; *cc = Lsg2[3]; } pos_ecef[0] = X; pos_ecef[1] = Y; pos_ecef[2] = Z; return 0; } int NAV_bancroft2(int N, SAT_t sats[], double pos_ecef[3], double *cc) { int i, j, k; double B[N][4], BtB[4][4], BBinv[4][4], BBB[4][N]; double a[N], Be[4], Ba[4]; double q0, q1, q2, p, q, sq, x1, x2; double Lsg1[4], Lsg2[4]; double tmp1, tmp2; double X, Y, Z; if (N < 4 || N > 12) return -1; for (i = 0; i < N; i++) { rotE(sats[i], B[i], B[i]+1, B[i]+2); B[i][3] = sats[i].PR; } if (N == 4) { matrix_invert(B, BBB); } else { for (i = 0; i < 4; i++) { for (j = 0; j < 4; j++) { BtB[i][j] = 0.0; for (k = 0; k < N; k++) { BtB[i][j] += B[k][i]*B[k][j]; } } } matrix_invert(BtB, BBinv); for (i = 0; i < 4; i++) { for (j = 0; j < N; j++) { BBB[i][j] = 0.0; for (k = 0; k < 4; k++) { BBB[i][j] += BBinv[i][k]*B[j][k]; } } } } for (i = 0; i < 4; i++) { Be[i] = 0.0; for (k = 0; k < N; k++) { Be[i] += BBB[i][k]*1.0; } } for (i = 0; i < N; i++) { a[i] = 0.5 * lorentz(B[i], B[i]); } for (i = 0; i < 4; i++) { Ba[i] = 0.0; for (k = 0; k < N; k++) { Ba[i] += BBB[i][k]*a[k]; } } q2 = lorentz(Be, Be); q1 = lorentz(Ba, Be)-1; q0 = lorentz(Ba, Ba); if (q2 == 0) return -2; p = q1/q2; q = q0/q2; sq = p*p - q; if (sq < 0) return -2; x1 = -p + sqrt(sq); x2 = -p - sqrt(sq); for (i = 0; i < 4; i++) { Lsg1[i] = x1*Be[i]+Ba[i]; Lsg2[i] = x2*Be[i]+Ba[i]; } Lsg1[3] = -Lsg1[3]; Lsg2[3] = -Lsg2[3]; tmp1 = sqrt( Lsg1[0]*Lsg1[0] + Lsg1[1]*Lsg1[1] + Lsg1[2]*Lsg1[2] ); tmp2 = sqrt( Lsg2[0]*Lsg2[0] + Lsg2[1]*Lsg2[1] + Lsg2[2]*Lsg2[2] ); tmp1 = fabs( tmp1 - 6371000.0 ); tmp2 = fabs( tmp2 - 6371000.0 ); if (tmp1 < tmp2) { X = Lsg1[0]; Y = Lsg1[1]; Z = Lsg1[2]; *cc = Lsg1[3]; } else { X = Lsg2[0]; Y = Lsg2[1]; Z = Lsg2[2]; *cc = Lsg2[3]; } pos_ecef[0] = X; pos_ecef[1] = Y; pos_ecef[2] = Z; return 0; } /* ---------------------------------------------------------------------------------------------------- */ int NAV_bancroft3(int N, SAT_t sats[], double pos_ecef1[3], double *cc1 , double pos_ecef2[3], double *cc2) { int i, j, k; double B[N][4], BtB[4][4], BBinv[4][4], BBB[4][N]; double a[N], Be[4], Ba[4]; double q0, q1, q2, p, q, sq, x1, x2; double Lsg1[4], Lsg2[4]; double tmp1, tmp2; double X1, Y1, Z1; double X2, Y2, Z2; if (N < 4 || N > 12) return -1; for (i = 0; i < N; i++) { // Test: nicht hier rotieren, sondern spaeter Lsg rotieren... rotZ(sats[i].X, sats[i].Y, sats[i].Z, 0.0, B[i], B[i]+1, B[i]+2); //B[i][3] = sats[i].PR; B[i][3] = sats[i].pseudorange + sats[i].clock_corr; } if (N == 4) { matrix_invert(B, BBB); } else { for (i = 0; i < 4; i++) { for (j = 0; j < 4; j++) { BtB[i][j] = 0.0; for (k = 0; k < N; k++) { BtB[i][j] += B[k][i]*B[k][j]; } } } matrix_invert(BtB, BBinv); for (i = 0; i < 4; i++) { for (j = 0; j < N; j++) { BBB[i][j] = 0.0; for (k = 0; k < 4; k++) { BBB[i][j] += BBinv[i][k]*B[j][k]; } } } } for (i = 0; i < 4; i++) { Be[i] = 0.0; for (k = 0; k < N; k++) { Be[i] += BBB[i][k]*1.0; } } for (i = 0; i < N; i++) { a[i] = 0.5 * lorentz(B[i], B[i]); } for (i = 0; i < 4; i++) { Ba[i] = 0.0; for (k = 0; k < N; k++) { Ba[i] += BBB[i][k]*a[k]; } } q2 = lorentz(Be, Be); q1 = lorentz(Ba, Be)-1; q0 = lorentz(Ba, Ba); if (q2 == 0) return -2; p = q1/q2; q = q0/q2; sq = p*p - q; if (sq < 0) return -2; x1 = -p + sqrt(sq); x2 = -p - sqrt(sq); for (i = 0; i < 4; i++) { Lsg1[i] = x1*Be[i]+Ba[i]; Lsg2[i] = x2*Be[i]+Ba[i]; } Lsg1[3] = -Lsg1[3]; Lsg2[3] = -Lsg2[3]; tmp1 = sqrt( Lsg1[0]*Lsg1[0] + Lsg1[1]*Lsg1[1] + Lsg1[2]*Lsg1[2] ); tmp2 = sqrt( Lsg2[0]*Lsg2[0] + Lsg2[1]*Lsg2[1] + Lsg2[2]*Lsg2[2] ); tmp1 = tmp1 - 6371000.0; tmp2 = tmp2 - 6371000.0; if ( fabs(tmp1) < fabs(tmp2) ) { X1 = Lsg1[0]; Y1 = Lsg1[1]; Z1 = Lsg1[2]; *cc1 = Lsg1[3]; X2 = Lsg2[0]; Y2 = Lsg2[1]; Z2 = Lsg2[2]; *cc2 = Lsg2[3]; } else { X1 = Lsg2[0]; Y1 = Lsg2[1]; Z1 = Lsg2[2]; *cc1 = Lsg2[3]; X2 = Lsg1[0]; Y2 = Lsg1[1]; Z2 = Lsg1[2]; *cc2 = Lsg1[3]; } rotZ(X1, Y1, Z1, EARTH_ROTATION_RATE*RANGE_ESTIMATE, pos_ecef1, pos_ecef1+1, pos_ecef1+2); rotZ(X2, Y2, Z2, EARTH_ROTATION_RATE*RANGE_ESTIMATE, pos_ecef2, pos_ecef2+1, pos_ecef2+2); return 0; } /* ---------------------------------------------------------------------------------------------------- */ static double NAV_relVel(LOC_t loc, VEL_t vel) { double d; double x,y,z; double norm; x = vel.X-loc.X; y = vel.Y-loc.Y; z = vel.Z-loc.Z; norm = sqrt(x*x+y*y+z*z); x /= norm; y /= norm; z /= norm; d = vel.vX*x + vel.vY*y + vel.vZ*z; return d; } int NAV_LinP(int N, SAT_t satv[], double pos_ecef[3], double dt, double dpos_ecef[3], double *cc) { int i, j, k; double B[N][4], Binv[4][N], BtB[4][4], BBinv[4][4]; double a[N], Ba[N]; double X, Y, Z; double norm[N]; double range, obs_range, prox_range; if (N < 4 || N > 12) return -1; for (i = 0; i < N; i++) { range = dist( pos_ecef[0], pos_ecef[1], pos_ecef[2], satv[i].X, satv[i].Y, satv[i].Z ); range /= LIGHTSPEED; if (range < 0.06 || range > 0.1) range = RANGE_ESTIMATE; rotZ(satv[i].X, satv[i].Y, satv[i].Z, EARTH_ROTATION_RATE*range, B[i], B[i]+1, B[i]+2); //rotZ(satv[i].X, satv[i].Y, satv[i].Z, 0.0, B[i], B[i]+1, B[i]+2); // est. RANGE_RATE = 0.0 X = B[i][0]-pos_ecef[0]; Y = B[i][1]-pos_ecef[1]; Z = B[i][2]-pos_ecef[2]; norm[i] = sqrt(X*X+Y*Y+Z*Z); B[i][0] = X/norm[i]; B[i][1] = Y/norm[i]; B[i][2] = Z/norm[i]; B[i][3] = 1; } if (N == 4) { matrix_invert(B, Binv); } else { for (i = 0; i < 4; i++) { for (j = 0; j < 4; j++) { BtB[i][j] = 0.0; for (k = 0; k < N; k++) { BtB[i][j] += B[k][i]*B[k][j]; } } } matrix_invert(BtB, BBinv); for (i = 0; i < 4; i++) { for (j = 0; j < N; j++) { Binv[i][j] = 0.0; for (k = 0; k < 4; k++) { Binv[i][j] += BBinv[i][k]*B[j][k]; } } } } for (i = 0; i < N; i++) { obs_range = satv[i].pseudorange + satv[i].clock_corr; //satv[i].PR; prox_range = norm[i] - dt; a[i] = prox_range - obs_range; } for (i = 0; i < 4; i++) { Ba[i] = 0.0; for (k = 0; k < N; k++) { Ba[i] += Binv[i][k]*a[k]; } } dpos_ecef[0] = Ba[0]; dpos_ecef[1] = Ba[1]; dpos_ecef[2] = Ba[2]; *cc = Ba[3]; return 0; } int NAV_LinV(int N, SAT_t satv[], double pos_ecef[3], double vel_ecef[3], double dt, double dvel_ecef[3], double *cc) { int i, j, k; double B[N][4], Binv[4][N], BtB[4][4], BBinv[4][4]; double a[N], Ba[N]; double X, Y, Z; double norm[N]; double v_proj; double obs_rate, prox_rate; LOC_t loc; VEL_t vel; if (N < 4 || N > 12) return -1; loc.X = pos_ecef[0]; loc.Y = pos_ecef[1]; loc.Z = pos_ecef[2]; if (N < 4 || N > 12) return -1; for (i = 0; i < N; i++) { rotZ(satv[i].X, satv[i].Y, satv[i].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE, B[i], B[i]+1, B[i]+2); //rotZ(satv[i].X, satv[i].Y, satv[i].Z, 0.0, B[i], B[i]+1, B[i]+2); // est. RANGE_RATE = 0.0 X = B[i][0]-pos_ecef[0]; Y = B[i][1]-pos_ecef[1]; Z = B[i][2]-pos_ecef[2]; norm[i] = sqrt(X*X+Y*Y+Z*Z); B[i][0] = X/norm[i]; B[i][1] = Y/norm[i]; B[i][2] = Z/norm[i]; // SatSpeed = sqrt( satv[i].vX*satv[i].vX + satv[i].vY*satv[i].vY + satv[i].vZ*satv[i].vZ ); B[i][3] = 1; } if (N == 4) { matrix_invert(B, Binv); } else { for (i = 0; i < 4; i++) { for (j = 0; j < 4; j++) { BtB[i][j] = 0.0; for (k = 0; k < N; k++) { BtB[i][j] += B[k][i]*B[k][j]; } } } matrix_invert(BtB, BBinv); for (i = 0; i < 4; i++) { for (j = 0; j < N; j++) { Binv[i][j] = 0.0; for (k = 0; k < 4; k++) { Binv[i][j] += BBinv[i][k]*B[j][k]; } } } } for (i = 0; i < N; i++) { obs_rate = satv[i].pseudorate; // + satv[i].clock_drift; vel.X = satv[i].X; vel.Y = satv[i].Y; vel.Z = satv[i].Z; vel.vX = satv[i].vX - vel_ecef[0]; vel.vY = satv[i].vY - vel_ecef[1]; vel.vZ = satv[i].vZ - vel_ecef[2]; v_proj = NAV_relVel(loc, vel); prox_rate = v_proj - dt; a[i] = prox_rate - obs_rate; } for (i = 0; i < 4; i++) { Ba[i] = 0.0; for (k = 0; k < N; k++) { Ba[i] += Binv[i][k]*a[k]; } } dvel_ecef[0] = Ba[0]; dvel_ecef[1] = Ba[1]; dvel_ecef[2] = Ba[2]; *cc = Ba[3]; return 0; } // ------------------------------------------------------------- #define GPS_WEEK1024 1 #define WEEKSEC 604800 int gps_satpos_alm(rs_data_t *rs_data, EPHEM_t alm[], double t, SAT_t *sat) { double X, Y, Z, vX, vY, vZ; int i, j; int week, rollover = 0; double cl_corr, cl_drift; for (i = 0; i < 12; i++) { if (sat[i].prn == 0) continue; for (j = 1; j < 33; j++) { if (alm[j].prn == sat[i].prn) break; } if (j == 33) { // Sat not found // fprintf(stderr, "[SEM: PRN %02d not found]\n", sat[i].prn); sat[i].prn = 0; } // Woche hat 604800 sec if (t-alm[j].toa > WEEKSEC/2) rollover = +1; else if (t-alm[j].toa < -WEEKSEC/2) rollover = -1; else rollover = 0; week = alm[j].week - rollover; /*if (j == 1)*/ (rs_data->GPS).week = week + GPS_WEEK1024*1024; GPS_SatellitePositionVelocity_Ephem( week, t, alm[j], &cl_corr, &cl_drift, &X, &Y, &Z, &vX, &vY, &vZ ); sat[i].clock_drift = cl_drift; sat[i].vX = vX; sat[i].vY = vY; sat[i].vZ = vZ; sat[i].X = X; sat[i].Y = Y; sat[i].Z = Z; sat[i].clock_corr = cl_corr; } return 0; } int gps_satpos_rnx(rs_data_t *rs_data, EPHEM_t *eph, double t, SAT_t *sat) { double X, Y, Z, vX, vY, vZ; int i; int week, rollover = 0; double cl_corr, cl_drift; double tdiff, td; int count, count0, satfound; for (i = 0; i < 12; i++) { if (sat[i].prn == 0) continue; count = count0 = 0; satfound = 0; // Woche hat 604800 sec tdiff = WEEKSEC; while (eph[count].prn > 0) { if (eph[count].prn == sat[i].prn) { satfound += 1; if (t - eph[count].toe > WEEKSEC/2) rollover = +1; else if (t - eph[count].toe < -WEEKSEC/2) rollover = -1; else rollover = 0; td = fabs( t - eph[count].toe - rollover*WEEKSEC); if ( td < tdiff ) { tdiff = td; week = eph[count].week - rollover; (rs_data->GPS).week = eph[count].gpsweek - rollover; count0 = count; } } count += 1; } if ( satfound ) { GPS_SatellitePositionVelocity_Ephem( week, t, eph[count0], &cl_corr, &cl_drift, &X, &Y, &Z, &vX, &vY, &vZ ); sat[i].clock_drift = cl_drift; sat[i].vX = vX; sat[i].vY = vY; sat[i].vZ = vZ; sat[i].X = X; sat[i].Y = Y; sat[i].Z = Z; sat[i].clock_corr = cl_corr; sat[i].ephtime = eph[count0].toe; } // test: rnx_data(PRN) expired (> 4 hrs) else { // Sat not found // fprintf(stdout, "[RNX: PRN %02d not found]\n", sat[i].prn); sat[i].prn = 0; } } return 0; } /* ---------------------------------------------------------------------------------------------------- */ // - 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) // ------------------------------------------------------------- // // Satellite Position and Velocity // static void GPS_SatelliteClockDriftCorrection( 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] double* clock_drift ) // ephemeris: satellite clock drift correction [m/s] { 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 correction 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] // clock drift *clock_drift = (af1 + 2.0*af2*tc) * LIGHTSPEED; // [m/s] } static void GPS_ComputeSatellitePositionVelocity( 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] double* vx, // satellite velocity x [m/s] double* vy, // satellite velocity y [m/s] double* vz ) // satellite velocity z [m/s] { 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; // Satellite Velocity Computations are below // see Reference // Remodi, B. M (2004). GPS Tool Box: Computing satellite velocities using the broadcast ephemeris. // GPS Solutions. Volume 8(3), 2004. pp. 181-183 // // example source code was available at [http://www.ngs.noaa.gov/gps-toolbox/bc_velo/bc_velo.c] // recomputed the cos and sin of the corrected argument of latitude double omegadotk; // corrected rate of right ascension [rad/s] double edot; // edot = n/(1.0 - ecc*cos(E)), [rad/s] double vdot; // d/dt of true anomaly [rad/s] double udot; // d/dt of argument of latitude [rad/s] double idotdot; // d/dt of the rate of the inclination angle [rad/s^2] double rdot; // d/dt of the radius in the orbital plane [m/s] double tmpa; // temp double tmpb; // temp double vx_op; // x velocity in the orbital plane [m/s] double vy_op; // y velocity in the orbital plane [m/s] cos2u = cos(2.0*u); sin2u = sin(2.0*u); edot = n / (1.0 - ecc*cosE); vdot = sinE*edot*(1.0 + ecc*cos(v)) / ( sin(v)*(1.0-ecc*cosE) ); udot = vdot + 2.0*(cus*cos2u - cuc*sin2u)*vdot; rdot = a*ecc*sinE*n/(1.0-ecc*cosE) + 2.0*(crs*cos2u - crc*sin2u)*vdot; idotdot = idot + (cis*cos2u - cic*sin2u)*2.0*vdot; vx_op = rdot*cosu - y_op*udot; vy_op = rdot*sinu + x_op*udot; // corrected rate of right ascension including similarily as above, // for omegak, compensation for the Sagnac effect omegadotk = omegadot - EARTH_ROTATION_RATE /* - EARTH_ROTATION_RATE*RANGERATE_ESTIMATE/LIGHTSPEED */ ; tmpa = vx_op - y_op*cosi*omegadotk; tmpb = x_op*omegadotk + vy_op*cosi - y_op*sini*idotdot; *vx = tmpa * cos_omegak - tmpb * sin_omegak; *vy = tmpa * sin_omegak + tmpb * cos_omegak; *vz = vy_op*sini + y_op*cosi*idotdot; } static void GPS_SatellitePositionVelocity_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* clock_drift, // 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* satvX, // satellite X velocity WGS84 ECEF [m] double* satvY, // satellite Y velocity WGS84 ECEF [m] double* satvZ // satellite Z velocity 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] double vx; // sat vX velocity [m] double vy; // sat VY velocity [m] double vz; // sat VZ velocity [m] unsigned short week; // user week adjusted with the clock correction if needed [week] x = y = z = 0.0; GPS_SatelliteClockDriftCorrection( 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, clock_drift ); // adjust for week rollover week = gpsweek; tow = gpstow + (*clock_correction)/LIGHTSPEED; if ( tow < 0.0 ) { tow += SECONDS_IN_WEEK; week--; } if ( tow > SECONDS_IN_WEEK ) { tow -= SECONDS_IN_WEEK; week++; } //range = 0.072s*299792458m/s=21585057m GPS_ComputeSatellitePositionVelocity( 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, &vx, &vy, &vz ); *satX = x; *satY = y; *satZ = z; *satvX = vx; *satvY = vy; *satvZ = vz; }