1856 lines
67 KiB
C
1856 lines
67 KiB
C
#include <stdlib.h>
|
||
#include "NavEquations.h"
|
||
#include "Navigation.h"
|
||
#include "gps_ip_defines.h"
|
||
#include "gps.h"
|
||
#include <math.h>
|
||
#include <string.h>
|
||
#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;
|
||
//}
|