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

feat(gps): split Doppler paths for ESKF and PPS

Browse Source
This commit is contained in:
onur
2026-07-26 01:59:08 +03:00
parent 49b8609bb4
commit 23650be635
35 changed files with 3884 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/src/adapters/gps_system_uart.c
+2 -1
View File
@@ -27,12 +27,13 @@ void gps_system_uart_publish_location(
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_LOCATION,
"source=%s lat=%.20lf lon=%.20lf height=%.3lf "
"gdop=%.10lf pdop=%.10lf hdop=%.10lf vdop=%.10lf tdop=%.10lf "
"tracked=%u used=%u velocity_valid=%u speed=%.3lf "
"tracked=%u used=%u velocity_valid=%u velocity_fde=%u speed=%.3lf "
"velocity_rms=%.3lf\r\n",
source, latitude_deg, longitude_deg, height_m,
gps->gdop, gps->pdop, gps->hdop, gps->vdop, gps->tdop,
gps->tracked_satellite_count, gps->used_satellite_count,
gps->velocity_valid ? 1u : 0u,
gps->velocity_fde_applied ? 1u : 0u,
gps->speed_mps, gps->velocity_residual_rms_mps);
}
soc/software/gps_system/src/gps/gps_interrupt_handlers.c
+4 -4
View File
@@ -156,13 +156,13 @@ static void gps_update_tracking_measurement_isr(
* A 0.05 pole at the 50 Hz navigation-bit cadence has an approximately
* 0.4 s time constant while retaining real receiver dynamics.
*/
measurement.doppler_hz =
measurement.filtered_doppler_hz =
GPS_TRACK_DOPPLER_SMOOTH_ALPHA * measurement.doppler_hz +
(1.0 - GPS_TRACK_DOPPLER_SMOOTH_ALPHA) *
previous->doppler_hz;
measurement.pseudorange_rate_mps =
previous->filtered_doppler_hz;
measurement.filtered_pseudorange_rate_mps =
gps_tracking_doppler_to_range_rate_mps(
measurement.doppler_hz);
measurement.filtered_doppler_hz);
}
measurement.local_timestamp_ticks = local_timestamp_ticks;
gps_latest_tracking_measurement[tracking_index] = measurement;
soc/software/gps_system/src/gps/gps_ip.c
+62 -8
View File
@@ -853,6 +853,9 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
bool ok = false;
bool pps_clock_bias_drift_valid = false;
double pps_clock_bias_drift_mps = 0.0;
bool pps_velocity_valid = false;
double pps_velocity_residual_rms_mps = 0.0;
uint32_t pps_velocity_excluded_row_mask = 0u;
gps_nav_solution_t solution;
memset(&solution, 0, sizeof(solution));
memset(solution_out, 0, sizeof(*solution_out));
@@ -954,6 +957,10 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
solution.doppler_hz[row] = measurements[row].doppler_hz;
solution.pseudorange_rate_mps[row] =
measurements[row].pseudorange_rate_mps;
solution.filtered_doppler_hz[row] =
measurements[row].filtered_doppler_hz;
solution.filtered_pseudorange_rate_mps[row] =
measurements[row].filtered_pseudorange_rate_mps;
}
if(ok && solution.solution_valid)
{
@@ -976,7 +983,7 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
velocity_measurements[row].valid = false;
}
}
solution.velocity_valid = gps_tracking_velocity_wls(
solution.velocity_valid = gps_tracking_velocity_wls_robust(
solution.ecef_m,
satellite_positions_array,
satellite_velocities_array,
@@ -986,7 +993,10 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
solution.velocity_ecef_mps,
solution.velocity_covariance_ecef_mps2,
&solution.clock_drift_mps,
&solution.velocity_residual_rms_mps);
&solution.velocity_residual_rms_mps,
&solution.velocity_excluded_row_mask);
solution.velocity_fde_applied =
solution.velocity_excluded_row_mask != 0u;
if(solution.velocity_valid)
{
solution.speed_mps =
@@ -996,15 +1006,57 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
solution.velocity_ecef_mps[1] +
solution.velocity_ecef_mps[2] *
solution.velocity_ecef_mps[2]);
}
/*
* PPS keeps the existing slow Doppler path as a low-noise derivative
* predictor for the position-WLS reference datum. This result is not
* published to fusion; fusion receives the raw robust solution above.
*/
for(unsigned int row = 0u; row < satellite_count; row++)
{
velocity_measurements[row].doppler_hz =
measurements[row].filtered_doppler_hz;
velocity_measurements[row].pseudorange_rate_mps =
measurements[row].filtered_pseudorange_rate_mps;
}
double pps_velocity_ecef_mps[3];
double pps_velocity_covariance_ecef_mps2[3][3];
double pps_receiver_clock_drift_mps = 0.0;
pps_velocity_valid = gps_tracking_velocity_wls_robust(
solution.ecef_m,
satellite_positions_array,
satellite_velocities_array,
clockDrift,
velocity_measurements,
satellite_count,
pps_velocity_ecef_mps,
pps_velocity_covariance_ecef_mps2,
&pps_receiver_clock_drift_mps,
&pps_velocity_residual_rms_mps,
&pps_velocity_excluded_row_mask);
if(pps_velocity_valid)
{
pps_clock_bias_drift_valid =
gps_pps_reference_bias_drift(
solution.ecef_m,
solution.velocity_ecef_mps,
pps_velocity_ecef_mps,
satellite_positions_array[reference_index],
satellite_velocities_array[reference_index],
clockDrift[reference_index],
&pps_clock_bias_drift_mps);
}
if(solution.velocity_fde_applied ||
(pps_velocity_excluded_row_mask != 0u))
{
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV,
"velocity FDE raw_mask=0x%02lx pps_mask=0x%02lx "
"raw_rms=%.3lf pps_rms=%.3lf\r\n",
(unsigned long)solution.velocity_excluded_row_mask,
(unsigned long)pps_velocity_excluded_row_mask,
solution.velocity_residual_rms_mps,
pps_velocity_residual_rms_mps);
}
}
if(ok && solution.fde_applied)
{
@@ -1056,12 +1108,13 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
(cn0_count != 0u) ? minimum_cn0 : NAN;
pps_quality.chi_square_ratio = solution.chi_square_ratio;
pps_quality.velocity_residual_rms_mps =
solution.velocity_valid ?
solution.velocity_residual_rms_mps : 0.0;
pps_velocity_valid ? pps_velocity_residual_rms_mps : 0.0;
pps_quality.used_satellites = solution.used_satellite_count;
pps_quality.residual_test_valid = solution.residual_test_valid;
pps_quality.velocity_valid = solution.velocity_valid;
pps_quality.fde_applied = solution.fde_applied;
pps_quality.velocity_valid = pps_velocity_valid;
pps_quality.fde_applied =
solution.fde_applied ||
(pps_velocity_excluded_row_mask != 0u);
}
bool pps_profile_changed = false;
/*
@@ -1141,7 +1194,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
"drift=%.3lf tdop=%.2lf\r\n",
gps_pps_profile_name(pps_profile),
solution.clock_bias_m,
solution.clock_drift_mps,
pps_clock_bias_drift_valid ?
pps_clock_bias_drift_mps : NAN,
solution.tdop);
goto pps_anchor_done;
}
soc/software/gps_system/src/gps/gps_tracking_measurement.c
+192 -37
View File
@@ -8,6 +8,8 @@
#define GPS_TRACK_MEASUREMENT_MIN_SATELLITES 4u
#define GPS_TRACK_MEASUREMENT_MAX_SATELLITES GPS_TRCK_IP_COUNT
#define GPS_TRACK_MEASUREMENT_EPSILON 1.0e-12
#define GPS_TRACK_VELOCITY_FDE_MIN_SATELLITES 6u
#define GPS_TRACK_VELOCITY_FDE_SIGMA_GATE 3.5
static double clamp_double(double value, double minimum, double maximum)
{
@@ -135,6 +137,10 @@ bool gps_tracking_measurement_read(
measurement_out->pseudorange_rate_mps =
gps_tracking_doppler_to_range_rate_mps(
measurement_out->doppler_hz);
measurement_out->filtered_doppler_hz =
measurement_out->doppler_hz;
measurement_out->filtered_pseudorange_rate_mps =
measurement_out->pseudorange_rate_mps;
unsigned int version =
meta >> GPS_TRCK_MEAS_META_VERSION_SHIFT;
@@ -149,7 +155,9 @@ bool gps_tracking_measurement_read(
(measurement_out->cn0_sample_count == 20u) &&
cn0_valid &&
isfinite(measurement_out->doppler_hz) &&
isfinite(measurement_out->pseudorange_rate_mps);
isfinite(measurement_out->pseudorange_rate_mps) &&
isfinite(measurement_out->filtered_doppler_hz) &&
isfinite(measurement_out->filtered_pseudorange_rate_mps);
return measurement_out->valid;
}
@@ -261,7 +269,94 @@ static bool invert_four_by_four(double input[4][4], double inverse[4][4])
return true;
}
bool gps_tracking_velocity_wls(
typedef struct {
double solution[4];
double normal_inverse[4][4];
double residual_mps[GPS_TRACK_MEASUREMENT_MAX_SATELLITES];
double residual_square_sum;
double weighted_residual_square_sum;
unsigned int row_count;
} gps_velocity_wls_candidate_t;
static bool gps_velocity_solve_candidate(
const double design[][4],
const double observation[],
const double weights[],
unsigned int used,
unsigned int excluded_row,
gps_velocity_wls_candidate_t* candidate_out)
{
if((design == NULL) || (observation == NULL) || (weights == NULL) ||
(candidate_out == NULL) ||
(used < GPS_TRACK_MEASUREMENT_MIN_SATELLITES) ||
(used > GPS_TRACK_MEASUREMENT_MAX_SATELLITES))
{
return false;
}
double normal[4][4] = {{0.0}};
double rhs[4] = {0.0};
unsigned int selected = 0u;
for(unsigned int measurement_row = 0u;
measurement_row < used;
measurement_row++)
{
if(measurement_row == excluded_row)
{
continue;
}
double weight = weights[measurement_row];
for(unsigned int row = 0u; row < 4u; row++)
{
rhs[row] += weight * design[measurement_row][row] *
observation[measurement_row];
for(unsigned int column = 0u; column < 4u; column++)
{
normal[row][column] +=
weight * design[measurement_row][row] *
design[measurement_row][column];
}
}
selected++;
}
if(selected < GPS_TRACK_MEASUREMENT_MIN_SATELLITES)
{
return false;
}
memset(candidate_out, 0, sizeof(*candidate_out));
if(!solve_four_by_four(normal, rhs, candidate_out->solution) ||
!invert_four_by_four(normal, candidate_out->normal_inverse))
{
return false;
}
for(unsigned int measurement_row = 0u;
measurement_row < used;
measurement_row++)
{
candidate_out->residual_mps[measurement_row] = NAN;
if(measurement_row == excluded_row)
{
continue;
}
double predicted = 0.0;
for(unsigned int column = 0u; column < 4u; column++)
{
predicted += design[measurement_row][column] *
candidate_out->solution[column];
}
double residual = observation[measurement_row] - predicted;
candidate_out->residual_mps[measurement_row] = residual;
candidate_out->residual_square_sum += residual * residual;
candidate_out->weighted_residual_square_sum +=
weights[measurement_row] * residual * residual;
}
candidate_out->row_count = selected;
return isfinite(candidate_out->residual_square_sum) &&
isfinite(candidate_out->weighted_residual_square_sum);
}
static bool gps_tracking_velocity_wls_internal(
const double receiver_ecef_m[3],
const double satellite_positions_m[][3],
const double satellite_velocities_mps[][3],
@@ -271,8 +366,14 @@ bool gps_tracking_velocity_wls(
double receiver_velocity_ecef_mps_out[3],
double receiver_velocity_covariance_ecef_mps2_out[3][3],
double* receiver_clock_drift_mps_out,
double* residual_rms_mps_out)
double* residual_rms_mps_out,
bool robust,
uint32_t* excluded_measurement_mask_out)
{
if(excluded_measurement_mask_out != NULL)
{
*excluded_measurement_mask_out = 0u;
}
if((receiver_ecef_m == NULL) || (satellite_positions_m == NULL) ||
(satellite_velocities_mps == NULL) ||
(satellite_clock_drifts_sps == NULL) ||
@@ -287,11 +388,10 @@ bool gps_tracking_velocity_wls(
return false;
}
double normal[4][4] = {{0.0}};
double rhs[4] = {0.0};
double design[GPS_TRACK_MEASUREMENT_MAX_SATELLITES][4];
double observation[GPS_TRACK_MEASUREMENT_MAX_SATELLITES];
double weights[GPS_TRACK_MEASUREMENT_MAX_SATELLITES];
unsigned int measurement_index[GPS_TRACK_MEASUREMENT_MAX_SATELLITES];
unsigned int used = 0u;
for(unsigned int satellite = 0u; satellite < count; satellite++)
@@ -342,17 +442,7 @@ bool gps_tracking_velocity_wls(
sigma_rate_mps = clamp_double(sigma_rate_mps, 0.05, 5.0);
double weight = 1.0 / (sigma_rate_mps * sigma_rate_mps);
weights[used] = weight;
for(unsigned int row = 0u; row < 4u; row++)
{
rhs[row] +=
weight * design[used][row] * observation[used];
for(unsigned int column = 0u; column < 4u; column++)
{
normal[row][column] +=
weight * design[used][row] *
design[used][column];
}
}
measurement_index[used] = satellite;
used++;
}
@@ -361,27 +451,47 @@ bool gps_tracking_velocity_wls(
return false;
}
double solution[4];
double normal_inverse[4][4];
if(!solve_four_by_four(normal, rhs, solution) ||
!invert_four_by_four(normal, normal_inverse))
gps_velocity_wls_candidate_t full;
if(!gps_velocity_solve_candidate(
design, observation, weights, used,
GPS_TRACK_MEASUREMENT_MAX_SATELLITES, &full))
{
return false;
}
double residual_square_sum = 0.0;
double weighted_residual_square_sum = 0.0;
for(unsigned int row = 0u; row < used; row++)
gps_velocity_wls_candidate_t selected = full;
unsigned int excluded_row = GPS_TRACK_MEASUREMENT_MAX_SATELLITES;
if(robust && (used >= GPS_TRACK_VELOCITY_FDE_MIN_SATELLITES))
{
double predicted = 0.0;
for(unsigned int column = 0u; column < 4u; column++)
double maximum_normalized_residual = 0.0;
for(unsigned int row = 0u; row < used; row++)
{
predicted += design[row][column] * solution[column];
double normalized =
fabs(full.residual_mps[row]) * sqrt(weights[row]);
maximum_normalized_residual =
fmax(maximum_normalized_residual, normalized);
}
double residual = observation[row] - predicted;
residual_square_sum += residual * residual;
weighted_residual_square_sum +=
weights[row] * residual * residual;
if(maximum_normalized_residual > GPS_TRACK_VELOCITY_FDE_SIGMA_GATE)
{
double best_score = INFINITY;
for(unsigned int trial_row = 0u; trial_row < used; trial_row++)
{
gps_velocity_wls_candidate_t trial;
if(gps_velocity_solve_candidate(
design, observation, weights, used, trial_row, &trial) &&
(trial.weighted_residual_square_sum < best_score))
{
best_score = trial.weighted_residual_square_sum;
selected = trial;
excluded_row = trial_row;
}
}
}
}
if((excluded_row < used) && (excluded_measurement_mask_out != NULL))
{
*excluded_measurement_mask_out =
1u << measurement_index[excluded_row];
}
/*
@@ -392,17 +502,19 @@ bool gps_tracking_velocity_wls(
* the poor geometry appear directly in the inverse normal matrix.
*/
double covariance_scale = 1.0;
if(used > 4u)
if(selected.row_count > 4u)
{
covariance_scale = fmax(
1.0, weighted_residual_square_sum / (double)(used - 4u));
1.0,
selected.weighted_residual_square_sum /
(double)(selected.row_count - 4u));
}
for(unsigned int row = 0u; row < 3u; row++)
{
for(unsigned int column = 0u; column < 3u; column++)
{
receiver_velocity_covariance_ecef_mps2_out[row][column] =
covariance_scale * normal_inverse[row][column];
covariance_scale * selected.normal_inverse[row][column];
if(!isfinite(
receiver_velocity_covariance_ecef_mps2_out[row][column]))
{
@@ -410,10 +522,53 @@ bool gps_tracking_velocity_wls(
}
}
}
memcpy(receiver_velocity_ecef_mps_out, solution,
3u * sizeof(solution[0]));
*receiver_clock_drift_mps_out = solution[3];
memcpy(receiver_velocity_ecef_mps_out, selected.solution,
3u * sizeof(selected.solution[0]));
*receiver_clock_drift_mps_out = selected.solution[3];
*residual_rms_mps_out =
sqrt(residual_square_sum / (double)used);
sqrt(selected.residual_square_sum / (double)selected.row_count);
return isfinite(*residual_rms_mps_out);
}
bool gps_tracking_velocity_wls(
const double receiver_ecef_m[3],
const double satellite_positions_m[][3],
const double satellite_velocities_mps[][3],
const double satellite_clock_drifts_sps[],
const gps_tracking_measurement_t measurements[],
unsigned int count,
double receiver_velocity_ecef_mps_out[3],
double receiver_velocity_covariance_ecef_mps2_out[3][3],
double* receiver_clock_drift_mps_out,
double* residual_rms_mps_out)
{
return gps_tracking_velocity_wls_internal(
receiver_ecef_m, satellite_positions_m,
satellite_velocities_mps, satellite_clock_drifts_sps,
measurements, count, receiver_velocity_ecef_mps_out,
receiver_velocity_covariance_ecef_mps2_out,
receiver_clock_drift_mps_out, residual_rms_mps_out,
false, NULL);
}
bool gps_tracking_velocity_wls_robust(
const double receiver_ecef_m[3],
const double satellite_positions_m[][3],
const double satellite_velocities_mps[][3],
const double satellite_clock_drifts_sps[],
const gps_tracking_measurement_t measurements[],
unsigned int count,
double receiver_velocity_ecef_mps_out[3],
double receiver_velocity_covariance_ecef_mps2_out[3][3],
double* receiver_clock_drift_mps_out,
double* residual_rms_mps_out,
uint32_t* excluded_measurement_mask_out)
{
return gps_tracking_velocity_wls_internal(
receiver_ecef_m, satellite_positions_m,
satellite_velocities_mps, satellite_clock_drifts_sps,
measurements, count, receiver_velocity_ecef_mps_out,
receiver_velocity_covariance_ecef_mps2_out,
receiver_clock_drift_mps_out, residual_rms_mps_out,
true, excluded_measurement_mask_out);
}