#include #include "NavEquations.h" #include "Navigation.h" #include "gps_ip_defines.h" #include "gps.h" #include #include #include "defines.h" /* Function: double computeRange(range_t* rho, ephem_t eph, ionoutc_t* ionoutc, gpstime_t g, double xyz[], t_temp_pos* tempPos) Computes range between a satellite and the receiver Parameters: eph - [in] ephemeris parameters ionoutc - ionospheric coefficients xyz - position of the receiver Returns: rho - [out] computed range ( pseudo range) */ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_ephemeris_t* eph, ionoutc_t* ionoutc, double* xyz, t_temp_pos* tempPos, double* clockCorr, mat_type *satellitePositions, mat_type * velocities){ double pos[3], vel[3], clk[2]; double los[3]; double tau; double range = 0; // rate = 0; double xrot, yrot; unsigned i; //gpstime_t * gpstime = (gpstime_t*) malloc(sizeof(gpstime_t)); //gpstime->sec = eph->toe.sec; //gpstime->week = eph->toe.week; //CPP'ye GOREydi, KAPATILDI// double abs_sample[SVN] = { 66564551., 66525117.,66589459.,66666672.,66633917.,66700000. }; //CPP'ye GOREydi, KAPATILDI// int codePhase = 2772; // TODO should be given by tracker. Şu anda matlab'dan alindi.. // SV position at time of the pseudorange observation. //int code_phase = eph->codephase; satpos(eph, pos, vel, clk, clockCorr); for (i = 0; i < 3; i++) { satellitePositions->matrix[sat_no][i] = pos[i]; } /*for (i = 0; i < 3; i++) tempPos->sv_po_at_pseudo_obs[i] = pos[i]; //memset(los, '0', sizeof(double)); for (i = 0; i < 3; i++) los[i] = 0; // Receiver to satellite vector and light-time. subVect(los, pos, xyz); tau = normVect(los) / SPEED_OF_LIGHT; // Extrapolate the satellite position backwards to the transmission time. pos[0] -= vel[0] * tau; pos[1] -= vel[1] * tau; pos[2] -= vel[2] * tau; for (i = 0; i < 3; i++) tempPos->extrapolated_sat_pos_tx_time[i] = pos[i]; // Earth rotation correction. The change in velocity can be neglected. xrot = pos[0] + pos[1] * OMEGA_EARTH * tau; yrot = pos[1] - pos[0] * OMEGA_EARTH * tau; pos[0] = xrot; pos[1] = yrot; for (i = 0; i < 3; i++) { tempPos->earth_rotation_correction[i] = pos[i]; satellitePositions->matrix[eph->SVID][i] = pos[i]; velocities->matrix[eph->SVID][i] = vel[i]; } #ifdef COUT_DEBUG #ifdef VECTOR_OPS print_mat(satellitePositions); #endif #endif // New observer to satellite vector and satellite range. subVect(los, pos, xyz); range = normVect(los); rho->d = range; tempPos->dRange = range;*/ double txmitTime = (eph->abs_sample) / (SAMPLES_PER_CODE); #ifdef COUT_DEBUG print_mat(satellitePositions); printf("absolute sample : %10.16lf \t SVN : %d \n", eph->abs_sample, eph->SVID); printf("travelTime : %10.16lf \t SVN : %d \n", txmitTime , eph->SVID); printf("dRange : %10.16lf \t SVN : %d \n", tempPos->dRange, eph->SVID); #endif //return txmitTime; *psTVLTime = txmitTime; // Ionospheric correction is applied in leastSquaresSTD(), where the // receiver position and per-satellite azimuth/elevation are available. /* tempPos->dpseudoRange = rho->range; // Relative velocity of SV and receiver. rate = dotProd(vel, los) / range; tempPos->relativeVelocity = rate; // Pseudorange rate. rho->rate = rate; // - SPEED_OF_LIGHT*clk[1]; tempPos->d1_pseudoRange = rate; // Time of application. rho->g = g; tempPos->gpsDate = g; // Azimuth and elevation angles. xyz2llh(xyz, llh); for (i = 0; i < 3; i++) tempPos->llh[i] = llh[i]; ltcmat(llh, tmat); ecef2neu(los, tmat, neu); neu2azel(rho->azel, neu); // Add ionospheric delay rho->iono_delay = ionosphericDelay(ionoutc, g, llh, rho->azel); rho->range += rho->iono_delay; tempPos->d2_range = rho->range; tempPos->ionDelay = rho->iono_delay; */ } /* brief Compute Norm of Vector * param[in] x Input vector * returns Length (Norm) of the input vector */ double normVect(const double* x) { return sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]); } /* Function: double subVect(double* y, const double* x1, const double* x2) Subtract two vectors of double Parameters: x1 - [in] Minuend of subtracion x2 - [in] Subtrahend of subtracion Returns: y - [out] y Result of subtraction */ void subVect(double* y, const double* x1, const double* x2) { y[0] = x1[0] - x2[0]; y[1] = x1[1] - x2[1]; y[2] = x1[2] - x2[2]; return; } /* Function: double check_t(long double tkl) - evaluates the seconds in week, iaw seconds in half week. Parameters: tkl - [in/out] seconds in week */ double check_t(long double tkl) { if (tkl > SECONDS_IN_HALF_WEEK) tkl -= SECONDS_IN_WEEK; else if (tkl < -SECONDS_IN_HALF_WEEK) tkl += SECONDS_IN_WEEK; return tkl; } /* Function: double NavEquations::satpos(ephem_t eph, gpstime_t g, double* pos, double* vel, double* clk, double code_phase) Compute range between a satellite and the receiver Parameters: eph - [in] ephemeris parameters g - [in]gps time. vel - [in] velocity of gps receiver code phase - [in] receiver/satellite synchronization clock delay.. clock - [out] satellite clock correction vector */ void satpos(gps_ephemeris_t* eph, double* pos, double* vel, double* clk, double* clockCorr) { // Computing Satellite Velocity using the Broadcast Ephemeris // http://www.ngs.noaa.gov/gps-toolbox/bc_velo.htm double tk; double mk; double ek; double ekdot; double cek, sek; double pk; double c2pk, s2pk; double uk; double ukdot; double cuk, suk; double ok; double sok, cok; double ik; double ikdot; double sik, cik; double rk; double rkdot; double xpk, ypk; double xpkdot, ypkdot; int cnt = 0; double relativistic, tmp, transmitTime = (double)((double)(eph->tow.sec) + POSTPONE_TOW); //- 18;//eph->toe.sec - codephase; GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_NAV, "tow.sec used in calculation = %.3f\r\n", eph->tow.sec); double clock_tk = check_t(transmitTime - eph->toc.sec); double timeCorrect = (eph->af2 * clock_tk + eph->af1) * clock_tk + eph->af0 - eph->tgd; double time = transmitTime - timeCorrect; tk = check_t (time - eph->toe.sec); double n0 = sqrt(GM_EARTH / (eph->A * eph->A * eph->A)); double n = n0 + eph->deltan; mk = eph->m0 + n * tk; double mkdot = n; ek = mk; double temp, dE; // = ek; for (cnt = 0; cnt <10; cnt++) { temp = ek; ek = mk + eph->ecc*sin(ek); dE = remainder(ek-temp,2*PI); if (fabs(dE) < 1.0E-14) break; } sek = sin(ek); cek = cos(ek); ekdot = mkdot/( 1 - eph->ecc*cek ); double tak = atan2(sqrt(1.0 - eph->ecc * eph->ecc) * sek, cek - eph->ecc); double takdot = sek * ekdot * (1.0 + eph->ecc * cos(tak)) / (sin(tak) * (1.0 - eph->ecc* cek)); relativistic = -4.442807633E-10 * eph->ecc * eph->sqrta * sek; double phi = tak + eph->omg0; phi = remainder(phi, 2 * PI); double u = phi + eph->cus * sin(2.0 * phi) + eph->cuc * cos(2.0 * phi); double r = eph->A * (1.0 - eph->ecc * cos(ek)) + eph->crc * cos(2 * phi) + eph->crs * sin(2 * phi); double i = eph->inc0 + eph->idot * tk + eph->cic * cos(2 * phi) + eph->cis * sin(2 * phi); double Omega = eph->omgdot + (eph->omgkdot - OMEGA_EARTH) * tk - OMEGA_EARTH * eph->toe.sec; Omega = remainder(Omega + 2 * PI, 2 * PI); pos[0] = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); pos[1] = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); pos[2] = sin(u) * r * sin(i); // Satellite clock correction // relativistic correction //*clockCorr = eph->af0 + tk * (eph->af1 + tk * eph->af2) + relativistic - eph->tgd; //7.23706041149e-05 *clockCorr = (eph->af2 * clock_tk + eph->af1) * clock_tk + eph->af0 - eph->tgd + relativistic; /*if (clk[0] != SECONDS_IN_HALF_WEEK) { *clockCorr = clk[0];//eph->af0 + tk * (eph->af1 + tk * eph->af2) + relativistic - eph->tgd; } else { *clockCorr = eph->af0 + tk * (eph->af1 + tk * eph->af2) + relativistic - eph->tgd; }*/ } void minimum_element(double* minimum, const double * vec, const int ActiveSatellitesNumber) { double min = vec[0]; for (int i =1; i < ActiveSatellitesNumber; i++) { if ((vec[i]) < min) min = vec[i]; } *minimum = min; } static bool least_squares_ura_sigma_m(unsigned char ura_index, double* sigma_m) { static const double ura_sigma_table_m[15] = { 2.4, 3.4, 4.85, 6.85, 9.65, 13.65, 24.0, 48.0, 96.0, 192.0, 384.0, 768.0, 1536.0, 3072.0, 6144.0 }; if((sigma_m == NULL) || (ura_index >= 15u)) { return false; } *sigma_m = ura_sigma_table_m[ura_index]; return true; } static double least_squares_measurement_weight(unsigned char ura_index, double elevation_deg, bool elevation_valid) { const double dtr = 0.01745329251994329576923690768489; const double min_elevation_deg = 10.0; const double elevation_sigma_zenith_m = 3.0; double ura_sigma_m; if(!least_squares_ura_sigma_m(ura_index, &ura_sigma_m)) { return 0.0; } double variance_m2 = ura_sigma_m * ura_sigma_m; if(elevation_valid) { double bounded_elevation_deg = elevation_deg; if(!isfinite(bounded_elevation_deg) || (bounded_elevation_deg < min_elevation_deg)) { bounded_elevation_deg = min_elevation_deg; } if(bounded_elevation_deg > 90.0) { bounded_elevation_deg = 90.0; } double sin_el = sin(bounded_elevation_deg * dtr); double elevation_sigma_m = elevation_sigma_zenith_m / sin_el; variance_m2 += elevation_sigma_m * elevation_sigma_m; } return 1.0 / variance_m2; } static bool least_squares_normalize_weights(double* weights, int count) { if((weights == NULL) || (count <= 0)) { return false; } double max_weight = 0.0; for(int i = 0 ; i < count ; i++) { if(!isfinite(weights[i]) || (weights[i] <= 0.0)) { return false; } if(weights[i] > max_weight) { max_weight = weights[i]; } } if(max_weight <= 0.0) { return false; } for(int i = 0 ; i < count ; i++) { weights[i] /= max_weight; } return true; } static mat_type* least_squares_weighted_normal_matrix(const mat_type* A, const double* weights) { if((A == NULL) || (A->matrix == NULL) || (weights == NULL)) { return NULL; } mat_type* normal = create_matrix(A->columns, A->columns, "weightedNormal"); if(normal == NULL) { return NULL; } initialize_all(0.0, normal); for(int row = 0 ; row < A->rows ; row++) { double w = weights[row]; for(int r = 0 ; r < A->columns ; r++) { for(int c = 0 ; c < A->columns ; c++) { normal->matrix[r][c] += w * A->matrix[row][r] * A->matrix[row][c]; } } } return normal; } static double covariance_along_axis(const mat_type* covariance, const double axis[3]) { double value = 0.0; for(int row = 0 ; row < 3 ; row++) { for(int col = 0 ; col < 3 ; col++) { value += axis[row] * covariance->matrix[row][col] * axis[col]; } } return value; } static double covariance_between_axes(const mat_type* covariance, const double first_axis[3], const double second_axis[3]) { double value = 0.0; for(int row = 0 ; row < 3 ; row++) { for(int col = 0 ; col < 3 ; col++) { value += first_axis[row] * covariance->matrix[row][col] * second_axis[col]; } } return value; } static double least_squares_chi_square_threshold_99(unsigned int degrees_of_freedom) { /* 99th percentile for chi-square distributions with 1..5 DOF. The * current hardware has at most nine GPS channels and four WLS states. */ static const double threshold[6] = { 0.0, 6.634896601, 9.210340372, 11.34486673, 13.27670414, 15.08627247 }; return (degrees_of_freedom < 6u) ? threshold[degrees_of_freedom] : 0.0; } static bool least_squares_solve_weighted_update(const mat_type* A, const mat_type* omc, const double* weights, double xVec[4]) { if((A == NULL) || (omc == NULL) || (weights == NULL) || (A->matrix == NULL) || (omc->matrix == NULL) || (A->columns > 4) || (omc->columns < A->rows)) { return false; } double rhs[4] = {0.0, 0.0, 0.0, 0.0}; for(int col = 0 ; col < A->columns ; col++) { for(int row = 0 ; row < A->rows ; row++) { if(!isfinite(A->matrix[row][col]) || !isfinite(weights[row]) || (weights[row] <= 0.0) || !isfinite(omc->matrix[0][row])) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "weighted solve rejected row=%d col=%d A=%e weight=%e omc=%e\r\n", row, col, A->matrix[row][col], weights[row], omc->matrix[0][row]); return false; } rhs[col] += A->matrix[row][col] * weights[row] * omc->matrix[0][row]; } if(!isfinite(rhs[col])) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "weighted solve produced non-finite rhs col=%d\r\n", col); return false; } } mat_type* normal = least_squares_weighted_normal_matrix(A, weights); mat_type* inv_normal = (normal != NULL) ? calculate_matrix_inverse(normal) : NULL; if((normal == NULL) || (inv_normal == NULL)) { free_matrix(normal); free_matrix(inv_normal); return false; } for(int row = 0 ; row < A->columns ; row++) { double sum = 0.0; for(int col = 0 ; col < A->columns ; col++) { sum += inv_normal->matrix[row][col] * rhs[col]; } xVec[row] = sum; if(!isfinite(xVec[row])) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "weighted solve produced non-finite update col=%d\r\n", row); free_matrix(normal); free_matrix(inv_normal); return false; } } free_matrix(normal); free_matrix(inv_normal); return true; } //#define COUT_DEBUG static double klobuchar_delay_meters(const ionoutc_t* ionoutc, double gps_tow, double latitude_deg, double longitude_deg, double azimuth_deg, double elevation_deg) { if((ionoutc == NULL) || !ionoutc->enable || !ionoutc->vflg || !isfinite(gps_tow) || !isfinite(latitude_deg) || !isfinite(longitude_deg) || !isfinite(azimuth_deg) || !isfinite(elevation_deg) || (elevation_deg <= 0.0)) { return 0.0; } double elevation_sc = elevation_deg / 180.0; double latitude_sc = latitude_deg / 180.0; double longitude_sc = longitude_deg / 180.0; double azimuth_rad = azimuth_deg * (PI / 180.0); double psi = 0.0137 / (elevation_sc + 0.11) - 0.022; double subion_lat = latitude_sc + psi * cos(azimuth_rad); if(subion_lat > 0.416) subion_lat = 0.416; if(subion_lat < -0.416) subion_lat = -0.416; double subion_lon = longitude_sc + psi * sin(azimuth_rad) / cos(subion_lat * PI); double geomag_lat = subion_lat + 0.064 * cos((subion_lon - 1.617) * PI); double local_time = fmod(43200.0 * subion_lon + gps_tow, 86400.0); if(local_time < 0.0) local_time += 86400.0; double amplitude = ionoutc->alpha0 + geomag_lat * (ionoutc->alpha1 + geomag_lat * (ionoutc->alpha2 + geomag_lat * ionoutc->alpha3)); if(amplitude < 0.0) amplitude = 0.0; double period = ionoutc->beta0 + geomag_lat * (ionoutc->beta1 + geomag_lat * (ionoutc->beta2 + geomag_lat * ionoutc->beta3)); if(period < 72000.0) period = 72000.0; double phase = 2.0 * PI * (local_time - 50400.0) / period; double obliquity = 1.0 + 16.0 * pow(0.53 - elevation_sc, 3.0); double delay_seconds = 5.0e-9; if(fabs(phase) < 1.57) { double phase2 = phase * phase; delay_seconds += amplitude * (1.0 - phase2 * 0.5 + phase2 * phase2 / 24.0); } return obliquity * delay_seconds * SPEED_OF_LIGHT; } static bool least_squares_build_postfit(const mat_type* satpos, const mat_type* obs, int satellite_count, const unsigned char* ura_indices, const ionoutc_t* ionoutc, double gps_tow, const double state[4], mat_type* design, mat_type* az, mat_type* el, double* residuals_m, double* absolute_weights) { if((satpos == NULL) || (obs == NULL) || (ura_indices == NULL) || (state == NULL) || (design == NULL) || (az == NULL) || (el == NULL) || (residuals_m == NULL) || (absolute_weights == NULL)) { return false; } const double dtr = 0.01745329251994329576923690768489; for(int i = 0 ; i < satellite_count ; i++) { double dx = satpos->matrix[i][0] - state[0]; double dy = satpos->matrix[i][1] - state[1]; double dz = satpos->matrix[i][2] - state[2]; double approximate_range = sqrt(dx*dx + dy*dy + dz*dz); if(!isfinite(approximate_range) || (approximate_range <= 1.0)) { return false; } double travel_time = approximate_range / SPEED_OF_LIGHT; double omega_tau = OMEGA_EARTH * travel_time; double cos_omega_tau = cos(omega_tau); double sin_omega_tau = sin(omega_tau); double rotated_satellite[3] = { cos_omega_tau * satpos->matrix[i][0] + sin_omega_tau * satpos->matrix[i][1], -sin_omega_tau * satpos->matrix[i][0] + cos_omega_tau * satpos->matrix[i][1], satpos->matrix[i][2] }; double difference[3] = { rotated_satellite[0] - state[0], rotated_satellite[1] - state[1], rotated_satellite[2] - state[2] }; double geometric_range = normVect(difference); if(!isfinite(geometric_range) || (geometric_range <= 1.0)) { return false; } t_phi_Lambda_h geodetic; t_topocentData topocentric; memset(&geodetic, 0, sizeof(geodetic)); memset(&topocentric, 0, sizeof(topocentric)); topocent(state, difference, 3, &geodetic, &topocentric); az->matrix[0][i] = topocentric.azim; el->matrix[0][i] = topocentric.elv; double weight = least_squares_measurement_weight(ura_indices[i], topocentric.elv, true); if(!isfinite(weight) || (weight <= 0.0)) { return false; } absolute_weights[i] = weight; double iono = klobuchar_delay_meters(ionoutc, gps_tow, geodetic.phi, geodetic.lambda, topocentric.azim, topocentric.elv); double trop = 0.0; if((topocentric.elv > 0.0) && (topocentric.height >= -1000.0) && (topocentric.height <= 15000.0)) { trop = tropo(sin(topocentric.elv * dtr), topocentric.height / 1000.0); if(!isfinite(trop) || (trop < 0.0)) { trop = 0.0; } } residuals_m[i] = obs->matrix[0][i] - geometric_range - state[3] - trop - iono; if(!isfinite(residuals_m[i])) { return false; } for(int component = 0 ; component < 3 ; component++) { design->matrix[i][component] = -difference[component] / geometric_range; } design->matrix[i][3] = 1.0; } return true; } static bool gps_wls_last_solution_valid = false; static double gps_wls_last_solution[4] = {0.0, 0.0, 0.0, 0.0}; // Function leastSquaresSTD // Definition : initialize all matc cpp class variables. // Arguments : // satpos - type mat_type*, matc class type replacement // obs - type mat_type*, matc class type replacement static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber, mat_type * az, mat_type * el, const unsigned char* ura_indices, const ionoutc_t* ionoutc, double gps_tow, const double seed[4], bool seed_valid, gps_nav_solution_t* solution_out) { //TODO computeRange i kapatmıştım, onu tekrar açmalıyım. if((satpos == NULL) || (obs == NULL) || (az == NULL) || (el == NULL) || (ura_indices == NULL) || (solution_out == NULL) || (satpos->matrix == NULL) || (obs->matrix == NULL) || (ActiveSatellitesNumber < MIN_NEEDED_SATELLITE) || ((unsigned int)ActiveSatellitesNumber > GPS_TRCK_IP_COUNT)) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "least squares skipped invalid inputs, sats=%d\r\n", ActiveSatellitesNumber); return false; } memset(solution_out, 0, sizeof(*solution_out)); uint64_t candidate_start_cycles = gps_read_cycle_counter(); //BURASI MATLAB GIRDILERININ KULLANILARAK, LEAST SQUARE IN, CROSS-CHECK ETMEK ICIN KULLANILDIGI YER. KALDIRILABILIR //***************************************************************************************************************** //printf("satpos C orig\n"); //print_mat(satpos); //printf("obs C orig\n"); //print_mat(obs); //DEBUG FOR MATLAB POS COMPARISON // double a [3] = {16689001.25160041078925132751, 12364950.10702963732182979584, 16709744.22839225456118583679 }; // double b [3] = {19239427.55783585086464881897, 3478161.40170052554458379745, 17944189.25546477362513542175 }; // double c [3] = {12427590.54291367903351783752, -9150939.29886376298964023590, 21714248.40279350802302360535 }; // double d [3] = { 2602525.01520504569634795189, 15281176.28262036666274070740, 21684297.87035004049539566040 }; // double e [3] = { 4761827.58405028749257326126, 17381963.84944353252649307251, 19210756.66427277028560638428 }; // double f [3] = {26186949.41875257715582847595, 2485255.36278775660321116447, -3937462.08371024020016193390 }; // for(int col = 0 ; col < 3 ; col++) // { // satpos->matrix[0][col] = a[col]; // satpos->matrix[1][col] = b[col]; // satpos->matrix[2][col] = c[col]; // satpos->matrix[3][col] = d[col]; // satpos->matrix[4][col] = e[col]; // satpos->matrix[5][col] = f[col]; //} // obs->matrix[0][0] = 21472208.01115246862173080444; // obs->matrix[0][1] = 20751765.87690072879195213318; // obs->matrix[0][2] = 21900655.31012692302465438843; // obs->matrix[0][3] = 23253973.47619381174445152283; // obs->matrix[0][4] = 22892911.31948769092559814453; // obs->matrix[0][5] = 23940020.98931504786014556885; //// Double Precision Loss Check Test //double result = a[0] * a[1]; //printf("number1 : %0.20lf \nnumber2 : %.20lf \nmultiply: %.20lf\n",a[0], a[1], result); //result = a[0] / a[1]; //printf("divide : %.20lf\n", result); //printf("satpos initialized by Matlab\n"); //print_mat(satpos); //printf("obs initialized by Matlab\n"); //print_mat(obs); //print_mat(satpos); //print_mat(obs); /******************************************************************************************************************/ t_phi_Lambda_h * phi_lambda_h = (t_phi_Lambda_h*) malloc(sizeof(t_phi_Lambda_h)); t_topocentData * topcent = (t_topocentData*) malloc(sizeof(t_topocentData));; //TODO: re-create by malloc if((phi_lambda_h == NULL) || (topcent == NULL)) { GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_MEM, "least squares state allocation failed\r\n"); free(phi_lambda_h); free(topcent); return false; } topcent->azim = 0.0; topcent->elv = 0.0; topcent->height = 0.0; topcent->tpcent = 0.0; double dtr = 0.01745329251994329576923690768489; unsigned int nonzero_sat_rows = number_of_nonzero_rows(satpos); if(nonzero_sat_rows != (unsigned int)ActiveSatellitesNumber) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "least squares skipped inconsistent sat rows, active=%d rows=%u\r\n", ActiveSatellitesNumber, nonzero_sat_rows); free(phi_lambda_h); free(topcent); return false; } unsigned int nmbOfSatellites = nonzero_sat_rows - 1u; // get satellite number as number of nonzero rows. #ifdef COUT_DEBUG if(nmbOfSatellites != ActiveSatellitesNumber) printf("SVN number mismatch, ActiveSatellitesNumber does not equal to number of SVNs in satpos array\n"); #endif //=============== Initialization before Iterations ================================================================= //mat_type * posEq = create_matrix(1, MAX_SAT, "posEq"); //1x12 matrix for DEBUG //initialize_all(0.0, posEq); mat_type* posVec = create_matrix(1, 4, "posVec"); //1x12 matrix for DEBUG //mat_type* pos = create_matrix(1, MAX_SAT, "pos"); //1x12 matrix for DEBUG //initialize_all(0.0, pos); // initialize all to the zero //mat_type * precisePseudoRanges = create_matrix(1, ActiveSatellitesNumber, "precisePseudoRanges"); //1x6 matrix for DEBUG //initialize_vec(&obs->matrix[0][0], precisePseudoRanges); //TODO:Burası DEBUG da kontrol edilecek YUSUF, belki de hiç precisePseudoRanges olmamalı mat_type* omc = create_matrix(1, ActiveSatellitesNumber, "omc"); //1x6 matrix for DEBUG mat_type* A = create_matrix(ActiveSatellitesNumber, 4, "A"); //6x4 matrix for DEBUG double* absoluteWeights = (double*)malloc(ActiveSatellitesNumber * sizeof(double)); if((posVec == NULL) || (omc == NULL) || (A == NULL) || (absoluteWeights == NULL)) { GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_MEM, "least squares matrix allocation failed\r\n"); free_matrix(posVec); free_matrix(omc); free_matrix(A); free(absoluteWeights); free(phi_lambda_h); free(topcent); return false; } initialize_all(0.0, posVec); // initialize all to the zero initialize_all(0.0, omc); initialize_all(0.0, A); for(int i = 0 ; i < ActiveSatellitesNumber ; i++) { absoluteWeights[i] = least_squares_measurement_weight(ura_indices[i], 0.0, false); } bool seeded_from_last_solution = seed_valid && (seed != NULL); for(int i = 0 ; seeded_from_last_solution && (i <= POSITION_VECTOR_COL) ; i++) { if(!isfinite(seed[i])) { seeded_from_last_solution = false; GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "discarded non-finite WLS seed\r\n"); } } if(seeded_from_last_solution) { for(int i = 0 ; i <= POSITION_VECTOR_COL ; i++) { posVec->matrix[0][i] = seed[i]; } } unsigned int iteration_count = 0u; //=============== Iteratively Find receiver position ================================================================= for (unsigned iteration = 0; iteration < NUMBER_OF_ITERATIONS; iteration++) { iteration_count = iteration + 1u; mat_type* Rot_X = create_matrix(1, 3, "Rot_X"); //1x3 matrix for DEBUG if(Rot_X == NULL) { GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_MEM, "least squares Rot_X allocation failed\r\n"); goto least_squares_fail; } initialize_all(0.0, Rot_X); for (unsigned i = 0; i <= nmbOfSatellites; i++) { double iono = 0.0; double trop = 2.0; #ifdef COUT_DEBUG printf("\n\nITERATION: %d\t\tSATELLITE: %d\n",iteration,i); //YUSUF for DEBUG #endif if((iteration == 0) && !seeded_from_last_solution) { for(int k = 0 ; k < 3 ; k++) Rot_X->matrix[0][k] = satpos->matrix[i][k]; //TODO: DEBUG for rotation #ifdef COUT_DEBUG printf("Rot X : %.20lf %.20lf %.20lf\n",Rot_X->matrix[0][0],Rot_X->matrix[0][1],Rot_X->matrix[0][2]); //TODO: for debug #endif } else { // update equations double dx = satpos->matrix[i][0] - posVec->matrix[0][0]; double dy = satpos->matrix[i][1] - posVec->matrix[0][1]; double dz = satpos->matrix[i][2] - posVec->matrix[0][2]; double rho2 = dx*dx+dy*dy+dz*dz; double traveltime = sqrt(rho2) / SPEED_OF_LIGHT; #ifdef COUT_DEBUG printf("i = %d dx = %.10lf dy = %.10lf dz = %.10lf rho2 = %.10lf traveltime = %.10lf\n", i, dx, dy, dz, rho2, traveltime); #endif mat_type* temp = create_matrix(1, 3, "temp"); if(temp == NULL) { free_matrix(Rot_X); GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_MEM, "least squares temp allocation failed\r\n"); goto least_squares_fail; } for(int j = 0 ; j < 3 ; j++) temp->matrix[0][j] = satpos->matrix[i][j]; rotateSatelliteSTD(traveltime, temp, Rot_X); #ifdef COUT_DEBUG print_mat(Rot_X); //TODO: for debug #endif free_matrix(temp); mat_type* difference = create_matrix(1, 3, "difference"); //1x3 matrix for DEBUG if(difference == NULL) { free_matrix(Rot_X); GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_MEM, "least squares difference allocation failed\r\n"); goto least_squares_fail; } initialize_all(0.0, difference); for(int k = 0 ; k < 3 ; k++) difference->matrix[0][k] = Rot_X->matrix[0][k] - posVec->matrix[0][k]; #ifdef COUT_DEBUG print_mat(difference); //TODO: for debug #endif //topocent(&*posVec.begin(), &*diff.begin(), diff.size()); ( ORIGINAL ) topocent(posVec->matrix[0], difference->matrix[0], difference->rows*difference->columns, phi_lambda_h, topcent); // arguments : initial element of individual vector elements. free_matrix(difference); //TODO: topocent e giren matrix lerin elemanları doğru yükleniyor mu diye bakılacak. !!!!!!!!!!!!!!!!!1 az->matrix[0][i] = topcent->azim; el->matrix[0][i] = topcent->elv; absoluteWeights[i] = least_squares_measurement_weight(ura_indices[i], topcent->elv, true); iono = klobuchar_delay_meters(ionoutc, gps_tow, phi_lambda_h->phi, phi_lambda_h->lambda, topcent->azim, topcent->elv); if((ionoutc != NULL) && (iteration == (NUMBER_OF_ITERATIONS - 1u))) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "iono sat_index=%u delay=%.3f m az=%.2f el=%.2f\r\n", i, iono, topcent->azim, topcent->elv); } trop = 0.0; if((topcent->elv > 0.0) && (topcent->height >= -1000.0) && (topcent->height <= 15000.0)) { /* topocent height is metres; the atmospheric model uses kilometres. */ trop = tropo(sin(topcent->elv * dtr), topcent->height / 1000.0); if(!isfinite(trop) || (trop < 0.0)) { trop = 0.0; } } }// iteration > 0 ends // ----------- apply the corrections------------------------------------------------ double diff[4]; for(int k = 0 ; k < 3 ; k++) diff[k] = (Rot_X->matrix[0][k] - posVec->matrix[0][k]); double probeniusNorm = normVect(diff); omc->matrix[0][i] = (obs->matrix[0][i] - probeniusNorm - posVec->matrix[0][3] - trop - iono); #ifdef COUT_DEBUG printf("obs->matrix[0][i]: %.10lf\t\tprobeniusNorm: %.10lf\t\tposVec->matrix[0][3]: %.10lf\t\ttrop: %.10lf\n",obs->matrix[0][i], probeniusNorm, posVec->matrix[0][3], trop); #endif // ------------------------construct ( use ) the A matrix ------------------------------------------------ for(unsigned int k = 0 ; k < 3 ; k++) { double denom = (probeniusNorm > 1.0) ? probeniusNorm : obs->matrix[0][i]; double val = (-(Rot_X->matrix[0][k] - posVec->matrix[0][k])) / denom; // Rot_X and posEq are vectors ( 1xN matrix ) A->matrix[i][k] = val; } A->matrix[i][3] = 1.0; }// Satellite_loop_end free_matrix(Rot_X); //double x[] = {0.0, 0.0, 0.0, 0.0}; // DEBUG ICIN COMMAND OUT EDILDI, TEKRAR AÇILACAK. MATRIX PROCESS EDILEBILIR MI KONTROLU ICIN //unsigned int uu = matrix_rank(A); //if(uu != 4) //{ // for(int n = 0 ; n < posVec->rows*posVec->columns ; n++) // posVec->matrix[0][n] = 0; // TODO rank 4'ten kucukse denklem cozulemez, dolayısıyla bır ust sevıyeye donulmesi gerekir.. //} double solveWeights[GPS_TRCK_IP_COUNT]; for(int weight_index = 0 ; weight_index < ActiveSatellitesNumber ; weight_index++) { solveWeights[weight_index] = absoluteWeights[weight_index]; } if(!least_squares_normalize_weights(solveWeights, ActiveSatellitesNumber)) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "weighted least squares rejected invalid URA/elevation weights\r\n"); goto least_squares_fail; } double xVec[4] = {0.0, 0.0, 0.0, 0.0}; bool weighted_ok = least_squares_solve_weighted_update(A, omc, solveWeights, xVec); if(!weighted_ok) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "weighted least squares skipped singular epoch, sats=%d rows=%u\r\n", ActiveSatellitesNumber, nonzero_sat_rows); goto least_squares_fail; } #ifdef COUT_DEBUG print_mat(A); #endif for(int ix = 0 ; ix <= POSITION_VECTOR_COL ; ++ix) { posVec->matrix[0][ix] = posVec->matrix[0][ix] + xVec[ix]; if(!isfinite(posVec->matrix[0][ix])) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "least squares rejected non-finite position iteration=%u col=%d\r\n", iteration, ix); goto least_squares_fail; } } #ifdef COUT_DEBUG printf(" posVec[0]: %.12lf \n posVec[1]: %.12lf \n posVec[2]: %.12lf \n posVec[3]: %.12lf \n",posVec->matrix[0][0],posVec->matrix[0][1],posVec->matrix[0][2],posVec->matrix[0][3] ); #endif double update_norm = sqrt((xVec[0] * xVec[0]) + (xVec[1] * xVec[1]) + (xVec[2] * xVec[2]) + (xVec[3] * xVec[3])); /* Position and receiver clock bias are all WLS states. Ignoring the * clock term can stop with an unconverged common-mode residual. */ if(update_norm < 1e-4) { break; } }// iteration_loop_end free_matrix(omc); //=============================================================================================== double X1 = posVec->matrix[0][0]; double Y1 = posVec->matrix[0][1]; double Z1 = posVec->matrix[0][2]; double ecef_radius = normVect(posVec->matrix[0]); if(!isfinite(ecef_radius) || (ecef_radius < 1000000.0) || (ecef_radius > 100000000.0)) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "position rejected invalid ECEF radius=%e x=%e y=%e z=%e\r\n", ecef_radius, X1, Y1, Z1); free_matrix(A); free_matrix(posVec); free(absoluteWeights); free(phi_lambda_h); free(topcent); return false; } cart2geo(X1, Y1, Z1, 5, phi_lambda_h); if(!isfinite(phi_lambda_h->phi) || !isfinite(phi_lambda_h->lambda) || !isfinite(phi_lambda_h->height)) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "position rejected invalid geodetic conversion lat=%e lon=%e height=%e\r\n", phi_lambda_h->phi, phi_lambda_h->lambda, phi_lambda_h->height); free_matrix(A); free_matrix(posVec); free(absoluteWeights); free(phi_lambda_h); free(topcent); return false; } double residuals_m[GPS_TRCK_IP_COUNT]; if(!least_squares_build_postfit(satpos, obs, ActiveSatellitesNumber, ura_indices, ionoutc, gps_tow, posVec->matrix[0], A, az, el, residuals_m, absoluteWeights)) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "post-fit residual construction failed\r\n"); free_matrix(A); free_matrix(posVec); free(absoluteWeights); free(phi_lambda_h); free(topcent); return false; } double residual_square_sum = 0.0; double absolute_weight_sum = 0.0; double weighted_residual_sum = 0.0; for(int i = 0 ; i < ActiveSatellitesNumber ; i++) { residual_square_sum += residuals_m[i] * residuals_m[i]; absolute_weight_sum += absoluteWeights[i]; weighted_residual_sum += absoluteWeights[i] * residuals_m[i] * residuals_m[i]; } if(!isfinite(residual_square_sum) || !isfinite(absolute_weight_sum) || !isfinite(weighted_residual_sum) || (absolute_weight_sum <= 0.0)) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "post-fit residual statistics invalid\r\n"); free_matrix(A); free_matrix(posVec); free(absoluteWeights); free(phi_lambda_h); free(topcent); return false; } unsigned int degrees_of_freedom = (unsigned int)ActiveSatellitesNumber - 4u; double chi_square_threshold = least_squares_chi_square_threshold_99(degrees_of_freedom); bool residual_test_valid = (degrees_of_freedom > 0u) && (chi_square_threshold > 0.0); double chi_square_ratio = residual_test_valid ? (weighted_residual_sum / chi_square_threshold) : INFINITY; bool chi_square_pass = residual_test_valid && (weighted_residual_sum <= chi_square_threshold); /* Absolute weights retain the physical 1/m^2 scale needed by covariance. * The normalized weights above are used only by the numerical WLS solve. */ mat_type* covariance_normal = least_squares_weighted_normal_matrix(A, absoluteWeights); mat_type* position_covariance = (covariance_normal != NULL) ? calculate_matrix_inverse(covariance_normal) : NULL; free_matrix(covariance_normal); if(position_covariance == NULL) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "position covariance skipped singular geometry\r\n"); free_matrix(A); free_matrix(posVec); free(absoluteWeights); free(phi_lambda_h); free(topcent); return false; } double postfit_rhs[4] = {0.0, 0.0, 0.0, 0.0}; for(int col = 0 ; col < 4 ; col++) { for(int row = 0 ; row < ActiveSatellitesNumber ; row++) { postfit_rhs[col] += A->matrix[row][col] * absoluteWeights[row] * residuals_m[row]; } } double postfit_correction[4] = {0.0, 0.0, 0.0, 0.0}; for(int row = 0 ; row < 4 ; row++) { for(int col = 0 ; col < 4 ; col++) { postfit_correction[row] += position_covariance->matrix[row][col] * postfit_rhs[col]; } } solution_out->postfit_position_correction_m = normVect(postfit_correction); solution_out->postfit_clock_correction_m = postfit_correction[3]; /* DOP describes geometry only, so it uses an unweighted normal matrix. */ double dop_weights[GPS_TRCK_IP_COUNT]; for(int i = 0 ; i < ActiveSatellitesNumber ; i++) { dop_weights[i] = 1.0; } mat_type* dop_normal = least_squares_weighted_normal_matrix(A, dop_weights); mat_type* dop_covariance = (dop_normal != NULL) ? calculate_matrix_inverse(dop_normal) : NULL; free_matrix(dop_normal); if(dop_covariance == NULL) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "DOP skipped singular geometry\r\n"); free_matrix(position_covariance); free_matrix(A); free_matrix(posVec); free(absoluteWeights); free(phi_lambda_h); free(topcent); return false; } double lat = phi_lambda_h->phi * (PI / 180.0); double lon = phi_lambda_h->lambda * (PI / 180.0); double sin_lat = sin(lat); double cos_lat = cos(lat); double sin_lon = sin(lon); double cos_lon = cos(lon); double east[3] = {-sin_lon, cos_lon, 0.0}; double north[3] = {-sin_lat * cos_lon, -sin_lat * sin_lon, cos_lat}; double up[3] = { cos_lat * cos_lon, cos_lat * sin_lon, sin_lat}; double q_east = covariance_along_axis(dop_covariance, east); double q_north = covariance_along_axis(dop_covariance, north); double q_up = covariance_along_axis(dop_covariance, up); double q_time = dop_covariance->matrix[3][3]; free_matrix(dop_covariance); double hdop2 = q_east + q_north; double vdop2 = q_up; double pdop2 = hdop2 + vdop2; double tdop2 = q_time; double gdop2 = pdop2 + tdop2; if(!isfinite(hdop2) || !isfinite(vdop2) || !isfinite(pdop2) || !isfinite(tdop2) || !isfinite(gdop2) || (hdop2 < 0.0) || (vdop2 < 0.0) || (pdop2 < 0.0) || (tdop2 < 0.0) || (gdop2 < 0.0)) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "DOP rejected invalid ENU geometry h2=%e v2=%e p2=%e t2=%e g2=%e\r\n", hdop2, vdop2, pdop2, tdop2, gdop2); free_matrix(position_covariance); free_matrix(A); free_matrix(posVec); free(absoluteWeights); free(phi_lambda_h); free(topcent); return false; } double gdop = sqrt(gdop2); double pdop = sqrt(pdop2); double hdop = sqrt(hdop2); double vdop = sqrt(vdop2); double tdop = sqrt(tdop2); const double* enu_axes[3] = {east, north, up}; for(int row = 0 ; row < 3 ; row++) { for(int col = 0 ; col < 3 ; col++) { solution_out->covariance_enu_m2[row][col] = covariance_between_axes(position_covariance, enu_axes[row], enu_axes[col]); } } free_matrix(position_covariance); free_matrix(A); double horizontal_variance = solution_out->covariance_enu_m2[0][0] + solution_out->covariance_enu_m2[1][1]; double vertical_variance = solution_out->covariance_enu_m2[2][2]; if(!isfinite(horizontal_variance) || !isfinite(vertical_variance) || (horizontal_variance < 0.0) || (vertical_variance < 0.0)) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "position covariance rejected hvar=%e vvar=%e\r\n", horizontal_variance, vertical_variance); free_matrix(posVec); free(absoluteWeights); free(phi_lambda_h); free(topcent); return false; } for(int component = 0 ; component < 3 ; component++) { solution_out->ecef_m[component] = posVec->matrix[0][component]; } solution_out->clock_bias_m = posVec->matrix[0][3]; solution_out->latitude_deg = phi_lambda_h->phi; solution_out->longitude_deg = phi_lambda_h->lambda; solution_out->height_m = phi_lambda_h->height; solution_out->hacc_m = sqrt(horizontal_variance); solution_out->vacc_m = sqrt(vertical_variance); solution_out->gdop = gdop; solution_out->pdop = pdop; solution_out->hdop = hdop; solution_out->vdop = vdop; solution_out->tdop = tdop; solution_out->residual_rms_m = sqrt(residual_square_sum / (double)ActiveSatellitesNumber); for(int i = 0 ; i < ActiveSatellitesNumber ; i++) { solution_out->postfit_residual_m[i] = residuals_m[i]; solution_out->elevation_deg[i] = el->matrix[0][i]; solution_out->measurement_sigma_m[i] = 1.0 / sqrt(absoluteWeights[i]); if(fabs(residuals_m[i]) > solution_out->max_abs_residual_m) { solution_out->max_abs_residual_m = fabs(residuals_m[i]); } } solution_out->weighted_residual_rms_m = sqrt(weighted_residual_sum / absolute_weight_sum); solution_out->weighted_residual_sum = weighted_residual_sum; solution_out->chi_square_threshold = chi_square_threshold; solution_out->chi_square_ratio = chi_square_ratio; solution_out->quality_score = residual_test_valid ? (100.0 / (1.0 + chi_square_ratio)) : 0.0; solution_out->available_satellite_count = (unsigned int)ActiveSatellitesNumber; solution_out->used_satellite_count = (unsigned int)ActiveSatellitesNumber; solution_out->degrees_of_freedom = degrees_of_freedom; solution_out->used_row_mask = (1u << (unsigned int)ActiveSatellitesNumber) - 1u; solution_out->excluded_row_mask = 0u; solution_out->candidate_valid = true; solution_out->residual_test_valid = residual_test_valid; solution_out->chi_square_pass = chi_square_pass; solution_out->fde_applied = false; solution_out->solution_valid = chi_square_pass; solution_out->iteration_count = iteration_count; solution_out->candidate_cycles = gps_read_cycle_counter() - candidate_start_cycles; //free(phi_lambda_h, sizeof(t_phi_Lambda_h)); //free(topcent, sizeof(t_topocentData));; //TODO: re-create by malloc free(phi_lambda_h); free(topcent); free_matrix(posVec); free(absoluteWeights); //=============================================================================================== return true; least_squares_fail: free_matrix(A); free_matrix(omc); free_matrix(posVec); free(absoluteWeights); free(phi_lambda_h); free(topcent); return false; } bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber, mat_type * az, mat_type * el, const unsigned char* ura_indices, const ionoutc_t* ionoutc, double gps_tow, gps_nav_solution_t* solution_out) { if(solution_out == NULL) { return false; } memset(solution_out, 0, sizeof(*solution_out)); uint64_t total_wls_start_cycles = gps_read_cycle_counter(); gps_nav_solution_t full_solution; if(!least_squares_candidate(satpos, obs, ActiveSatellitesNumber, az, el, ura_indices, ionoutc, gps_tow, gps_wls_last_solution, gps_wls_last_solution_valid, &full_solution)) { return false; } gps_nav_solution_t selected_solution = full_solution; bool passing_candidate_found = full_solution.chi_square_pass; uint32_t full_row_mask = (1u << (unsigned int)ActiveSatellitesNumber) - 1u; uint64_t fde_start_cycles = 0u; unsigned int fde_trial_count = 0u; /* Six available measurements leave only five after exclusion (one residual * DOF). In that geometry several leave-one-out candidates can pass while * producing widely separated positions. Keep the legacy full-WLS position * as an explicitly invalid best effort until at least two residual DOF remain * after exclusion (seven available measurements). */ if((GPS_NAV_ENABLE_FDE != 0u) && !full_solution.chi_square_pass && (ActiveSatellitesNumber >= 7)) { fde_start_cycles = gps_read_cycle_counter(); for(int excluded_row = 0 ; excluded_row < ActiveSatellitesNumber ; excluded_row++) { fde_trial_count++; int subset_count = ActiveSatellitesNumber - 1; mat_type* subset_satpos = create_matrix(subset_count, 3, "fdeSatpos"); mat_type* subset_obs = create_matrix(1, subset_count, "fdeObs"); mat_type* subset_az = create_matrix(1, subset_count, "fdeAz"); mat_type* subset_el = create_matrix(1, subset_count, "fdeEl"); unsigned char subset_ura[GPS_TRCK_IP_COUNT]; if((subset_satpos == NULL) || (subset_obs == NULL) || (subset_az == NULL) || (subset_el == NULL)) { free_matrix(subset_satpos); free_matrix(subset_obs); free_matrix(subset_az); free_matrix(subset_el); GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_MEM, "FDE subset allocation failed\r\n"); break; } initialize_all(0.0, subset_satpos); initialize_all(0.0, subset_obs); initialize_all(0.0, subset_az); initialize_all(0.0, subset_el); int subset_row = 0; for(int original_row = 0 ; original_row < ActiveSatellitesNumber ; original_row++) { if(original_row == excluded_row) { continue; } for(int component = 0 ; component < 3 ; component++) { subset_satpos->matrix[subset_row][component] = satpos->matrix[original_row][component]; } subset_obs->matrix[0][subset_row] = obs->matrix[0][original_row]; subset_ura[subset_row] = ura_indices[original_row]; subset_row++; } double full_seed[4] = { full_solution.ecef_m[0], full_solution.ecef_m[1], full_solution.ecef_m[2], full_solution.clock_bias_m }; gps_nav_solution_t candidate; bool candidate_ok = least_squares_candidate(subset_satpos, subset_obs, subset_count, subset_az, subset_el, subset_ura, ionoutc, gps_tow, full_seed, true, &candidate); free_matrix(subset_satpos); free_matrix(subset_obs); free_matrix(subset_az); free_matrix(subset_el); if(!candidate_ok) { continue; } candidate.available_satellite_count = (unsigned int)ActiveSatellitesNumber; candidate.used_row_mask = full_row_mask & ~(1u << (unsigned int)excluded_row); candidate.excluded_row_mask = 1u << (unsigned int)excluded_row; candidate.fde_applied = true; double mapped_residuals[GPS_TRCK_IP_COUNT] = {0.0}; double mapped_elevations[GPS_TRCK_IP_COUNT] = {0.0}; double mapped_sigmas[GPS_TRCK_IP_COUNT] = {0.0}; subset_row = 0; for(int original_row = 0 ; original_row < ActiveSatellitesNumber ; original_row++) { if(original_row != excluded_row) { mapped_residuals[original_row] = candidate.postfit_residual_m[subset_row]; mapped_elevations[original_row] = candidate.elevation_deg[subset_row]; mapped_sigmas[original_row] = candidate.measurement_sigma_m[subset_row]; subset_row++; } } memcpy(candidate.postfit_residual_m, mapped_residuals, sizeof(mapped_residuals)); memcpy(candidate.elevation_deg, mapped_elevations, sizeof(mapped_elevations)); memcpy(candidate.measurement_sigma_m, mapped_sigmas, sizeof(mapped_sigmas)); bool choose_candidate = false; if(candidate.chi_square_pass) { choose_candidate = !passing_candidate_found || (candidate.chi_square_ratio < selected_solution.chi_square_ratio); } else if(!passing_candidate_found && (candidate.chi_square_ratio < selected_solution.chi_square_ratio)) { /* Preserve the best statistical improvement as an explicitly invalid * best-effort navigation result. */ choose_candidate = true; } if(choose_candidate) { selected_solution = candidate; } if(candidate.chi_square_pass) { passing_candidate_found = true; } } } selected_solution.available_satellite_count = (unsigned int)ActiveSatellitesNumber; selected_solution.full_chi_square_ratio = full_solution.chi_square_ratio; selected_solution.full_postfit_position_correction_m = full_solution.postfit_position_correction_m; selected_solution.full_postfit_clock_correction_m = full_solution.postfit_clock_correction_m; memcpy(selected_solution.full_postfit_residual_m, full_solution.postfit_residual_m, sizeof(selected_solution.full_postfit_residual_m)); memcpy(selected_solution.full_elevation_deg, full_solution.elevation_deg, sizeof(selected_solution.full_elevation_deg)); memcpy(selected_solution.full_measurement_sigma_m, full_solution.measurement_sigma_m, sizeof(selected_solution.full_measurement_sigma_m)); selected_solution.full_solution_cycles = full_solution.candidate_cycles; selected_solution.fde_trial_count = fde_trial_count; selected_solution.fde_cycles = (fde_start_cycles != 0u) ? (gps_read_cycle_counter() - fde_start_cycles) : 0u; selected_solution.total_wls_cycles = gps_read_cycle_counter() - total_wls_start_cycles; selected_solution.solution_valid = selected_solution.candidate_valid && selected_solution.residual_test_valid && selected_solution.chi_square_pass; if(selected_solution.solution_valid) { gps_wls_last_solution[0] = selected_solution.ecef_m[0]; gps_wls_last_solution[1] = selected_solution.ecef_m[1]; gps_wls_last_solution[2] = selected_solution.ecef_m[2]; gps_wls_last_solution[3] = selected_solution.clock_bias_m; gps_wls_last_solution_valid = true; } *solution_out = selected_solution; GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV, "GDOP=%.10lf\r\n", solution_out->gdop); GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV, "position lat=%.20lf lon=%.20lf height=%.3lf\r\n", solution_out->latitude_deg, solution_out->longitude_deg, solution_out->height_m); GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV, "NAVSOL valid=%u integrity=%u fde=%u sats=%u/%u excluded=0x%03x " "residual_rms=%.3lf max_residual=%.3lf weighted_rms=%.3lf " "chi2=%.3lf dof=%u threshold=%.3lf ratio=%.3lf quality=%.1lf " "hacc=%.3lf vacc=%.3lf\r\n", solution_out->solution_valid ? 1u : 0u, solution_out->chi_square_pass ? 1u : 0u, solution_out->fde_applied ? 1u : 0u, solution_out->used_satellite_count, solution_out->available_satellite_count, solution_out->excluded_row_mask, solution_out->residual_rms_m, solution_out->max_abs_residual_m, solution_out->weighted_residual_rms_m, solution_out->weighted_residual_sum, solution_out->degrees_of_freedom, solution_out->chi_square_threshold, solution_out->chi_square_ratio, solution_out->quality_score, solution_out->hacc_m, solution_out->vacc_m); for(int row = 0 ; row < ActiveSatellitesNumber ; row++) { if((solution_out->used_row_mask & (1u << (unsigned int)row)) != 0u) { GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "postfit row=%d residual=%.3lf m\r\n", row, solution_out->postfit_residual_m[row]); } } return true; } void rotateSatelliteSTD(double const traveltime, const mat_type* X_sat, mat_type* X_sat_rot) // x_sat -> vector type { double omegatau = OMEGA_EARTH * traveltime; double cosomegatau = cos(omegatau); //TODO: C++ ile tutmuyor, kontrol et değerleri double sinomegatau = sin(omegatau); X_sat_rot->matrix[0][0] = cosomegatau * X_sat->matrix[0][0] + sinomegatau * X_sat->matrix[0][1]; X_sat_rot->matrix[0][1] = ((-1) * sinomegatau * X_sat->matrix[0][0]) + cosomegatau * X_sat->matrix[0][1]; X_sat_rot->matrix[0][2] = X_sat->matrix[0][2]; } void topocent(const double* XPtr, const double* dxPtr, int size, t_phi_Lambda_h* phi_Lambda_h, t_topocentData* topcent) { double dtr = 0.01745329251994329576923690768489; double mX[3], dx[3]; for(int i = 0 ; i < 3 ; i++) { mX[i] = *XPtr++; dx[i] = *dxPtr++; } #ifdef COUT_DEBUG printf("mX[0]: %.10lf\t\tmX[1]: %.10lf\t\tmX[2]: %.10lf\t\t\n",mX[0],mX[1],mX[2]); //TODO:yusuf for DEBUG printf("dX[0]: %.10lf\t\tdX[1]: %.10lf\t\tdX[2]: %.10lf\t\t\n",dx[0],dx[1],dx[2]); //TODO:yusuf for DEBUG #endif togeod( 6378137, 298.257223563, mX[0], mX[1], mX[2] , phi_Lambda_h, topcent) ; double cl = cos(phi_Lambda_h->lambda * dtr); double sl = sin(phi_Lambda_h->lambda * dtr); double cb = cos(phi_Lambda_h->phi * dtr); double sb = sin(phi_Lambda_h->phi * dtr); double Az, El; double sum = 0; double Ftransposed[3][3] = {{-sl, cl, 0 }, {-sb*cl, -sb*sl, cb}, {cb*cl, cb*sl, sb}}; //printf("FTrnsp: %.20lf\t\t%.20lf\t\t%.20lf\n %.20lf\t\t%.20lf\t\t%.20lf\n %.20lf\t\t%.20lf\t\t%.20lf\n",Ftransposed[1][1],Ftransposed[1][2],Ftransposed[1][3],Ftransposed[2][1],Ftransposed[2][2],Ftransposed[2][3],Ftransposed[3][1],Ftransposed[3][2],Ftransposed[3][3]); //TODO:yusuf for DEBUG double local_vector[3]; for(int i = 0 ; i < 3 ; i++) { for(int j = 0 ; j < 3 ; j++) { sum = sum + Ftransposed[i][j] * dx[j]; } #ifdef COUT_DEBUG printf("sum+FTrp= %.20lf \n",sum); //TODO: for debug #endif local_vector[i] = sum; sum = 0; } double E = local_vector[0]; double N = local_vector[1]; double U = local_vector[2]; double hor_dis = sqrt(E * E + N * N); #ifdef COUT_DEBUG printf("hor_dis= %.20lf \n",hor_dis); //TODO: for debug #endif if (hor_dis < 1.e-20) { Az = 0; El = 90; } else { Az = atan2(E, N) / dtr; El = atan2(U, hor_dis) / dtr; } if (Az < 0) Az = Az + 360; topcent->azim = Az; topcent->elv = El; topcent->tpcent = sqrt(dx[0] * dx[0] + dx[1] * dx[1] + dx[2] * dx[2]); #ifdef COUT_DEBUG printf("topcent.azim: %.10lf\t\ttopcent.elv: %.10lf\t\ttopcent.tpcent: %.10lf\t\ttopcent.height: %.10lf\n",topcent->azim,topcent->elv,topcent->tpcent,topcent->height); #endif } void togeod(double a, double finv, double X, double Y, double Z, t_phi_Lambda_h * phi_Lambda_h, t_topocentData* topcent) { double h = 0; double tolsq = 1.e-10; double maxit = 10; double esq; // compute radians - to - degree factor double rtd = 180 / PI; // compute square of eccentricity if (finv < 1.e-20) esq = 0; else esq = (2 - 1 / finv) / finv; double oneesq = 1 - esq; // first guess // P is distance from spin axis double P = sqrt(X*X + Y*Y); double dlambda; // direct calculation of longitude if (P > 1.e-20) dlambda = atan2(Y, X) * rtd; else dlambda = 0; if (dlambda < 0) dlambda = dlambda + 360; // r is distance from origin(0, 0, 0) double r = sqrt(P*P + Z*Z); double sinphi; if (r > 1.e-20) sinphi = Z / r; else sinphi = 0; double dphi = asin(sinphi); // initial value of height = distance from origin minus // approximate distance from origin to surface of ellipsoid if (r < 1.e-20) { phi_Lambda_h->phi = dphi; phi_Lambda_h->lambda = dlambda; phi_Lambda_h->height = 0; return; // &phi_Lambda_h; } h = r - a * (1 - sinphi * sinphi / finv); // iterate for (int i = 0; i < maxit; i++) { sinphi = sin(dphi); double cosphi = cos(dphi); // compute radius of curvature in prime vertical direction double N_phi = a / sqrt(1 - esq * sinphi * sinphi); // compute residuals in Pand Z double dP = P - (N_phi + h) * cosphi; double dZ = Z - (N_phi * oneesq + h) * sinphi; // update height and latitude h = h + (sinphi * dZ + cosphi * dP); dphi = dphi + (cosphi * dZ - sinphi * dP) / (N_phi + h); // test for convergence if (dP * dP + dZ * dZ < tolsq) break; // Not Converged--Warn user } dphi = dphi * rtd; phi_Lambda_h->phi = dphi; phi_Lambda_h->lambda = dlambda; phi_Lambda_h->height = h; topcent->height = h; #ifdef COUT_DEBUG printf("phi_Lambda_h->phi: %.10lf\t\tphi_Lambda_h->lambda %.10lf\t\tphi_Lambda_h->height: %.10lf\t\t\n",phi_Lambda_h->phi,phi_Lambda_h->lambda ,phi_Lambda_h->height); //yusuf for DEBUG #endif // check if stack pointer modify phi_lambda_h structure values!!!! } double tropo(double sinel, double hsta) { double rangeCorrection = 0; const double heightOfStatus = hsta; // hsta const double atm_Pressure = 1013.0; // p const double surfaceTemperature = 293.0; //t_kel const double humidity = 50.0; // hum const double heightOfPressure = 0.0; //hp const double heightOfTempMeas = 0.0; // htkel const double heightOfHumidityMeas = 0.0; // hhum const double a_e = 6378.137; // semi-major axis of earth ellipsoid const double b0 = 7.839257e-5; const double tlapse = -6.5; const double em = -978.77 / (2.8704e6 * tlapse * 1.0e-5); // is not in relevant Matlab file double tkhum = surfaceTemperature + tlapse * (heightOfHumidityMeas - heightOfTempMeas); double atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15); double e0 = 0.0611 * humidity * pow(10, atkel); #ifdef COUT_DEBUG printf("em= %.20lf \t\ttkhum= %.20lf\t\tatkel= %.20lf \t\te0= %.20lf \t\t\n",em,tkhum,atkel,e0); //TODO: for debug #endif double tksea = surfaceTemperature - tlapse * heightOfTempMeas; double tkelh = tksea + tlapse * heightOfPressure; double e0sea = e0 * pow((tksea / tkelh), (4 * em)); double tkelp = tksea + tlapse * heightOfPressure; double psea = atm_Pressure * pow((tksea / tkelp), em); #ifdef COUT_DEBUG printf("tksea= %.20lf \t\ttkelh= %.20lf\t\te0sea= %.20lf \t\ttkelp= %.20lf \t\tpsea %.20lf \t\t\n",tksea,tkelh,e0sea,tkelp,psea); //TODO: for debug #endif if (sinel < 0) { sinel = 0.0; } double tropo_delay = 0.0; bool done = false; double refsea = 77.624e-6 / tksea; double htop = 1.1385e-5 / refsea; refsea = refsea * psea; double powhtop = pow(((htop - heightOfStatus) / htop), 4); double ref = refsea * powhtop; double a; double b; double rtop; #ifdef COUT_DEBUG printf("refsea= %.20lf \t\thtop= %.20lf\t\tpowhtop= %.20lf \t\tref= %.20lf \t\tpsea %.20lf \t\t\n",refsea,htop,powhtop,ref); //TODO: for debug #endif while (1) { rtop = pow((a_e + htop), 2) - pow((a_e + heightOfStatus), 2) * (1 - pow(sinel, 2)); #ifdef COUT_DEBUG printf("1-rtop = %.20lf\n",rtop); //TODO: for debug #endif // check to see if geometry is crazy if (rtop < 0) { rtop = 0; } rtop = sqrt(rtop) - (a_e + heightOfStatus) * sinel; #ifdef COUT_DEBUG printf("2-rtop = %.20lf \n ",rtop); //TODO: for debug #endif a = -sinel / (htop - heightOfStatus); b = -b0 * (1 - pow(sinel, 2)) / (htop - heightOfStatus); #ifdef COUT_DEBUG printf("a = %.20lf b = %.20lf \n",a,b); //TODO: for debug #endif double rn[8]; for (int i = 0; i < 8; i++) rn[i] = 0; for (int i = 0; i < 8; i++) { rn[i] = pow(rtop, (i + 2)); #ifdef COUT_DEBUG printf("rn[%d] = %.20lf \n",i,rn[i]); //TODO: for debug #endif } double alpha[8] = {2 * a, 2 * pow(a, 2) + 4 * b / 3, a * (pow(a, 2) + 3 * b), pow(a, 4) / 5 + 2.4 * pow(a, 2) * b + 1.2 * pow(b, 2), 2 * a * b * (pow(a, 2) + 3 * b) / 3, pow(b, 2) * (6 * pow(a, 2) + 4 * b) * 1.428571e-1, 0, 0 }; #ifdef COUT_DEBUG printf("alpha[6] = %.20lf alpha[7] = %.20lf\n",alpha[6],alpha[7]); //TODO: for debug #endif if (pow(b, 2) > 1.0e-35) { alpha[6] = a * pow(b, 3) / 2; alpha[7] = pow(b, 4) / 9; } #ifdef COUT_DEBUG printf("alpha[6] = %.20lf alpha[7] = %.20lf\n",alpha[6],alpha[7]); //TODO: for debug #endif double dr = rtop; double sum = 0; double aux; for (int r = 0; r < 8; r++) sum = sum + alpha[r] * rn[r]; aux = sum; #ifdef COUT_DEBUG printf("aux = %.20lf\n",aux); //TODO: for debug #endif dr = dr + aux; tropo_delay = tropo_delay + dr * ref * 1000; #ifdef COUT_DEBUG printf("dr = %.20lf tropo_delay = %.20lf\n",dr,tropo_delay); //TODO: for debug #endif if (done == true) { rangeCorrection = tropo_delay; return rangeCorrection; break; } done = true; refsea = (371900.0e-6 / tksea - 12.92e-6) / tksea; htop = 1.1385e-5 * (1255 / tksea + 0.05) / refsea; ref = refsea * e0sea * pow(((htop - heightOfStatus) / htop), 4); #ifdef COUT_DEBUG printf("2 refsea= %.20lf \t\thtop= %.20lf\t\te0sea= %.20lf \t\tref= %.20lf \t\tpsea %.20lf \t\theightOfStatus %.20lf\n",refsea,htop,e0sea,ref,psea,heightOfStatus); //TODO: for debug #endif } #ifdef COUT_DEBUG printf("rangeCorrection: %.10lf\t\t\n",rangeCorrection); //yusuf for DEBUG #endif return rangeCorrection; } /* Function: t_phi_Lambda_h* NavEquations::cart2geo(double X, double Y, double Z, int i) - CART2GEO Conversion of Cartesian coordinates(X, Y, Z) to geographical - coordinates(phi, lambda, h) on a selected reference ellipsoid. - [phi, lambda, h] = cart2geo(X, Y, Z, i); - Choices i of Reference Ellipsoid for Geographical Coordinates - 1. International Ellipsoid 1924 - 2. International Ellipsoid 1967 - 3. World Geodetic System 1972 - 4. Geodetic Reference System 1980 - 5. World Geodetic System 1984 - Kai Borre 10 - 13 - 98 Parameters: x - [in] x coordinate in cartesian coordinate system y - [in] y coordinate in cartesian coordinate system z - [in] z coordinate in cartesian coordinate system i - [in] phi_Lambda_h - [out] height of station */ void cart2geo(double X, double Y, double Z, int i, t_phi_Lambda_h * phi_lambda_h) { double a[5] = { 6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0}; double f[5] = { 0.0033670033670033670033670033671, // 1 / 297, 0.00335292559522811629287134489199, // 1 / 298.247, 0.00335277945416750486153020854288, // 1 / 298.26, 0.00335281068118231893543414612613, // 1 / 298.257222101, 0.00335281066474748071984552861852 // 1 / 298.257223563//{1 / 297, 1 / 298.247, 1 / 298.26, 1 / 298.257222101, 1 / 298.257223563}; }; i = i - 1; double lambda = atan2(Y, X); double ex2 = (2 - f[i]) * f[i] / ((1 - f[i]) * (1 - f[i])); double c = a[i] * sqrt(1 + ex2); double phi = atan(Z / ((sqrt(X * X + Y * Y) * (1 - (2 - f[i])) * f[i]))); double h = 0.1; double oldh = 0; double iterations = 0.0; while ( absolute( h - oldh ) > 1.e-12 ) { oldh = h; double N = c / sqrt(1 + ex2 * cos(phi) * cos(phi)); phi = atan(Z / ((sqrt(X * X + Y * Y) * (1 - (2 - f[i]) * f[i] * N / (N + h))))); double dividend = X * X; double ySquare = Y * Y; dividend = dividend + ySquare; dividend = sqrt(dividend); double divider = cos(phi); double result = dividend / divider; result = result - N; h = sqrt(X*X + Y*Y) / cos(phi) - N; iterations = iterations + 1; if (iterations > 100) { GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV, "cart2geo height approximation did not converge, delta=%.20f\r\n", h - oldh); break; } } phi = phi * 180 / PI; lambda = lambda * 180 / PI; phi_lambda_h->phi = phi; phi_lambda_h->lambda = lambda; phi_lambda_h->height = h; } /* Function: int NavEquations::findUtmZone(double latitude, double longitude) - receives lattitude/longitude parameters and returns utm time. - May be called later for NMEA Parameters: lattitude - [in] longitude - [in] utmZone - [out] Universal time zone */ int findUtmZone(double latitude, double longitude) { int utmZone; #ifdef COUT_DEBUG if ((longitude > 180) || (longitude < -180)) printf("Longitude value exceeds limits(-180:180). \n"); if ((latitude > 84) || (latitude < -80)) printf("Latitude value exceeds limits (-80:84). \n"); #endif // Find zone ============================================================== // Start at 180 deg west = -180 deg utmZone = (int)((180 + longitude) / 6) + 1; // //fix((180 + longitude) / 6) + 1; // Correct zone numbers for particular areas ============================== if (latitude > 72) { // Corrections for zones 31 33 35 37 if ((longitude >= 0) && (longitude < 9)) utmZone = 31; else if((longitude >= 9) && (longitude < 21)) utmZone = 33; else if((longitude >= 21) && (longitude < 33)) utmZone = 35; else if((longitude >= 33) && (longitude < 42)) utmZone = 37; } else if((latitude >= 56) && (latitude < 64)) // Correction for zone 32 if ((longitude >= 3) && (longitude < 12)) utmZone = 32; return utmZone; } //double SquareRoot(double number) //{ // if (number < 0) { // return -1.0; // Square root of a negative number is not real // } // double accuracy = 0.001; // double x = number; // double guess = (x + number / x) / 2.0; // while (x - guess > accuracy) { // x = guess; // guess = (x + number / x) / 2.0; // } // return guess; //}