onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add GPS integrity checks and GPS IMU ESKF fusion tools

Browse Source
This commit is contained in:
onur
2026-07-13 11:08:12 +03:00
parent 8260862854
commit 4edb665d52
50 changed files with 21581 additions and 14357 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/src/NavEquations.c
+536 -52
View File
@@ -4,6 +4,7 @@
#include "gps_ip_defines.h"
#include "gps.h"
#include <math.h>
#include <string.h>
#include "defines.h"
@@ -370,6 +371,36 @@ static double covariance_along_axis(const mat_type* covariance, const double axi
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,
@@ -492,22 +523,118 @@ static double klobuchar_delay_meters(const ionoutc_t* ionoutc,
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
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,
double solution_out[4])
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.
static bool last_solution_valid = false;
static double last_solution[4] = {0.0, 0.0, 0.0, 0.0};
if((satpos == NULL) || (obs == NULL) || (az == NULL) || (el == NULL) ||
(ura_indices == NULL) ||
(ura_indices == NULL) || (solution_out == NULL) ||
(satpos->matrix == NULL) || (obs->matrix == NULL) ||
(ActiveSatellitesNumber < MIN_NEEDED_SATELLITE) ||
((unsigned int)ActiveSatellitesNumber > GPS_TRCK_IP_COUNT))
@@ -516,6 +643,8 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
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
//*****************************************************************************************************************
@@ -601,14 +730,14 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
//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* satWeights = (double*)malloc(ActiveSatellitesNumber * sizeof(double));
if((posVec == NULL) || (omc == NULL) || (A == NULL) || (satWeights == NULL))
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(satWeights);
free(absoluteWeights);
free(phi_lambda_h);
free(topcent);
return false;
@@ -619,30 +748,31 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
initialize_all(0.0, A);
for(int i = 0 ; i < ActiveSatellitesNumber ; i++)
{
satWeights[i] = least_squares_measurement_weight(ura_indices[i], 0.0, false);
absoluteWeights[i] = least_squares_measurement_weight(ura_indices[i], 0.0, false);
}
bool seeded_from_last_solution = last_solution_valid;
bool seeded_from_last_solution = seed_valid && (seed != NULL);
for(int i = 0 ; seeded_from_last_solution && (i <= POSITION_VECTOR_COL) ; i++)
{
if(!isfinite(last_solution[i]))
if(!isfinite(seed[i]))
{
seeded_from_last_solution = false;
last_solution_valid = false;
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "discarded non-finite cached position\r\n");
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] = last_solution[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)
{
@@ -713,7 +843,7 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
//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;
satWeights[i] = least_squares_measurement_weight(ura_indices[i], topcent->elv, true);
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);
@@ -771,13 +901,18 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
// 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..
//}
if(!least_squares_normalize_weights(satWeights, ActiveSatellitesNumber))
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, satWeights, xVec);
bool weighted_ok = least_squares_solve_weighted_update(A, omc, solveWeights, xVec);
if(!weighted_ok)
{
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
@@ -805,9 +940,14 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
#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 norm = normVect(xVec);
double update_norm = sqrt((xVec[0] * xVec[0]) +
(xVec[1] * xVec[1]) +
(xVec[2] * xVec[2]) +
(xVec[3] * xVec[3]));
if(norm < 1e-4)
/* 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;
}
@@ -827,7 +967,7 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
ecef_radius, X1, Y1, Z1);
free_matrix(A);
free_matrix(posVec);
free(satWeights);
free(absoluteWeights);
free(phi_lambda_h);
free(topcent);
return false;
@@ -841,29 +981,108 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
phi_lambda_h->phi, phi_lambda_h->lambda, phi_lambda_h->height);
free_matrix(A);
free_matrix(posVec);
free(satWeights);
free(absoluteWeights);
free(phi_lambda_h);
free(topcent);
return false;
}
/* DOP describes geometry, not the URA/elevation stochastic model used by
* WLS. Build an unweighted normal matrix, then rotate its ECEF position
* covariance into local ENU before reporting horizontal and vertical DOP. */
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);
free_matrix(A);
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(satWeights);
free(absoluteWeights);
free(phi_lambda_h);
free(topcent);
return false;
@@ -897,8 +1116,10 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
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(satWeights);
free(absoluteWeights);
free(phi_lambda_h);
free(topcent);
return false;
@@ -909,37 +1130,85 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
double hdop = sqrt(hdop2);
double vdop = sqrt(vdop2);
double tdop = sqrt(tdop2);
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
"DOP GDOP=%.10lf PDOP=%.10lf HDOP=%.10lf VDOP=%.10lf TDOP=%.10lf\r\n",
gdop, pdop, hdop, vdop, tdop);
(void)gdop;
(void)pdop;
(void)hdop;
(void)vdop;
(void)tdop;
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
"position lat=%.20lf lon=%.20lf height=%.20lf\r\n",
phi_lambda_h->phi, phi_lambda_h->lambda, phi_lambda_h->height);
int UTMzone = findUtmZone(phi_lambda_h->phi, phi_lambda_h->lambda);
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "UTM zone=%d\r\n", UTMzone);
(void)UTMzone;
for(int i = 0 ; i <= POSITION_VECTOR_COL ; i++)
const double* enu_axes[3] = {east, north, up};
for(int row = 0 ; row < 3 ; row++)
{
last_solution[i] = posVec->matrix[0][i];
if(solution_out != NULL)
for(int col = 0 ; col < 3 ; col++)
{
solution_out[i] = posVec->matrix[0][i];
solution_out->covariance_enu_m2[row][col] =
covariance_between_axes(position_covariance, enu_axes[row], enu_axes[col]);
}
}
last_solution_valid = true;
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(satWeights);
free(absoluteWeights);
//===============================================================================================
return true;
@@ -947,12 +1216,227 @@ least_squares_fail:
free_matrix(A);
free_matrix(omc);
free_matrix(posVec);
free(satWeights);
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;