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

feat(fusion): fuse Doppler velocity in ESKF

Browse Source
This commit is contained in:
onur
2026-07-25 13:11:33 +03:00
parent 2da5fb7153
commit 971be036a0
19 changed files with 647 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/src/adapters/gps_fusion_publisher.c
+3
View File
@@ -27,6 +27,9 @@ bool gps_fusion_build_measurement(
memcpy(measurement_out->velocity_ecef_mps,
solution->velocity_ecef_mps,
sizeof(measurement_out->velocity_ecef_mps));
memcpy(measurement_out->velocity_covariance_ecef_mps2,
solution->velocity_covariance_ecef_mps2,
sizeof(measurement_out->velocity_covariance_ecef_mps2));
measurement_out->speed_mps = solution->speed_mps;
measurement_out->velocity_residual_rms_mps =
solution->velocity_residual_rms_mps;
soc/software/gps_system/src/adapters/gps_system_uart.c
+5 -1
View File
@@ -116,15 +116,19 @@ void gps_system_uart_publish_periodic_fusion_status(
{
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
"state=%s lat=%.10lf lon=%.10lf height=%.3lf "
"speed=%.3lf nis=%.3lf gps=%lu/%lu recoveries=%lu "
"speed=%.3lf pos_nis=%.3lf vel_nis=%.3lf "
"gps=%lu/%lu velocity=%lu/%lu recoveries=%lu "
"zupt=%lu replay=%lu "
"sigma=%.2lf,%.2lf,%.2lf imu=%lu fused_imu=%lu "
"overflow=%lu missed=%lu\r\n",
fusion_motion_state_name(output->motion_state),
output->latitude_deg, output->longitude_deg, output->height_m,
output->speed_mps, output->last_position_nis,
output->last_velocity_nis,
(unsigned long)output->gps_accepted,
(unsigned long)output->gps_nis_rejected,
(unsigned long)output->gps_velocity_accepted,
(unsigned long)output->gps_velocity_nis_rejected,
(unsigned long)output->gps_position_recoveries,
(unsigned long)output->zero_update_count,
(unsigned long)output->replay_count,
soc/software/gps_system/src/fusion/eskf.c
+50
View File
@@ -224,6 +224,7 @@ void eskf_default_config(eskf_config_t* config)
config->gravity_mps2 = 9.80665;
config->max_propagation_step_s = 0.02;
config->position_nis_gate = 16.26623619623813; /* chi-square 3 DOF, 99.9%. */
config->velocity_nis_gate = 16.26623619623813; /* chi-square 3 DOF, 99.9%. */
config->covariance_diagonal_floor = 1.0e-12;
}
@@ -319,6 +320,7 @@ bool eskf_initialize(eskf_t* filter,
}
covariance_symmetrize_and_floor(filter, config->covariance_diagonal_floor);
filter->last_position_nis = NAN;
filter->last_velocity_nis = NAN;
filter->initialized = true;
return true;
}
@@ -806,6 +808,54 @@ eskf_update_result_t eskf_position_update(
return ESKF_UPDATE_ACCEPTED;
}
eskf_update_result_t eskf_velocity_update(
eskf_t* filter,
const eskf_config_t* config,
const double velocity_mps[3],
const double covariance_mps2[9],
double* nis_out)
{
if(nis_out != NULL)
{
*nis_out = NAN;
}
if((filter == NULL) || !filter->initialized || (config == NULL) ||
!vector_is_finite(velocity_mps, 3u) ||
!vector_is_finite(covariance_mps2, 9u))
{
return ESKF_UPDATE_INVALID;
}
double residual[3];
for(unsigned int i = 0u; i < 3u; i++)
{
residual[i] = velocity_mps[i] - filter->velocity_mps[i];
}
double nis = NAN;
if(!eskf_selected_update(
filter, ESKF_VELOCITY_INDEX, residual, covariance_mps2,
true, config->velocity_nis_gate, &nis))
{
filter->last_velocity_nis = nis;
if(isfinite(nis) && (nis > config->velocity_nis_gate))
{
filter->velocity_update_rejected_count++;
if(nis_out != NULL)
{
*nis_out = nis;
}
return ESKF_UPDATE_REJECTED_NIS;
}
return ESKF_UPDATE_INVALID;
}
filter->last_velocity_nis = nis;
filter->velocity_update_accepted_count++;
if(nis_out != NULL)
{
*nis_out = nis;
}
return ESKF_UPDATE_ACCEPTED;
}
bool eskf_reset_position(
eskf_t* filter,
const double position_m[3],
soc/software/gps_system/src/fusion/fusion_engine.c
+135 -37
View File
@@ -176,6 +176,10 @@ bool fusion_engine_apply_imu_quality(
static bool config_valid(const fusion_engine_config_t* config)
{
return (config != NULL) && (config->timebase_hz != 0u) &&
isfinite(config->eskf.position_nis_gate) &&
(config->eskf.position_nis_gate > 0.0) &&
isfinite(config->eskf.velocity_nis_gate) &&
(config->eskf.velocity_nis_gate > 0.0) &&
isfinite(config->calibration_duration_s) &&
(config->calibration_duration_s > 0.0) &&
(config->calibration_min_samples >= 2u) &&
@@ -230,6 +234,18 @@ static bool position_valid(const fusion_position_measurement_t* measurement)
isfinite(measurement->source_quality);
}
static bool navigation_valid(
const fusion_navigation_measurement_t* measurement)
{
if((measurement == NULL) || !position_valid(&measurement->position))
{
return false;
}
return !measurement->velocity_valid ||
(finite_values(measurement->velocity_enu_mps, 3u) &&
finite_values(measurement->velocity_covariance_enu_mps2, 9u));
}
static void calibration_reset(fusion_engine_t* engine)
{
memset(&engine->calibration, 0, sizeof(engine->calibration));
@@ -865,13 +881,17 @@ static eskf_update_result_t runtime_apply_position(
return result;
}
static bool process_position_now(fusion_engine_t* engine,
const fusion_position_measurement_t* measurement,
eskf_update_result_t* result_out,
double* nis_out)
static bool process_navigation_now(
fusion_engine_t* engine,
const fusion_navigation_measurement_t* measurement,
eskf_update_result_t* position_result_out,
double* position_nis_out,
eskf_update_result_t* velocity_result_out,
double* velocity_nis_out)
{
const fusion_position_measurement_t* position = &measurement->position;
if(!engine->checkpoint_valid ||
(measurement->timestamp_ticks < engine->checkpoint.filter_timestamp_ticks))
(position->timestamp_ticks < engine->checkpoint.filter_timestamp_ticks))
{
engine->diagnostics.position_stale_rejected++;
return false;
@@ -882,7 +902,7 @@ static bool process_position_now(fusion_engine_t* engine,
for(unsigned int i = 0 ; i < engine->imu_history_count ; i++)
{
const fusion_imu_measurement_t* imu = history_at(engine, i);
if(imu->timestamp_ticks > measurement->timestamp_ticks)
if(imu->timestamp_ticks > position->timestamp_ticks)
{
next_imu = imu;
break;
@@ -894,7 +914,7 @@ static bool process_position_now(fusion_engine_t* engine,
}
replayed++;
}
if(revised.filter_timestamp_ticks < measurement->timestamp_ticks)
if(revised.filter_timestamp_ticks < position->timestamp_ticks)
{
if(!revised.have_held_imu)
{
@@ -902,12 +922,12 @@ static bool process_position_now(fusion_engine_t* engine,
return false;
}
fusion_imu_measurement_t endpoint = revised.held_imu;
endpoint.timestamp_ticks = measurement->timestamp_ticks;
endpoint.timestamp_ticks = position->timestamp_ticks;
if((next_imu != NULL) &&
(next_imu->timestamp_ticks > revised.held_imu.timestamp_ticks))
{
endpoint = interpolate_imu(&revised.held_imu, next_imu,
measurement->timestamp_ticks);
position->timestamp_ticks);
}
if(!runtime_propagate_to(engine, &revised, &endpoint))
{
@@ -915,31 +935,31 @@ static bool process_position_now(fusion_engine_t* engine,
return false;
}
}
double nis = NAN;
eskf_update_result_t result = runtime_apply_position(engine, &revised,
measurement, &nis);
if(result == ESKF_UPDATE_INVALID)
double position_nis = NAN;
eskf_update_result_t position_result = runtime_apply_position(
engine, &revised, position, &position_nis);
if(position_result == ESKF_UPDATE_INVALID)
{
return false;
}
if(result == ESKF_UPDATE_ACCEPTED)
if(position_result == ESKF_UPDATE_ACCEPTED)
{
position_recovery_clear(engine);
}
else if((result == ESKF_UPDATE_REJECTED_NIS) &&
position_recovery_ready(engine, &revised, measurement) &&
eskf_reset_position(&revised.filter, measurement->position_enu_m,
measurement->covariance_enu_m2))
else if((position_result == ESKF_UPDATE_REJECTED_NIS) &&
position_recovery_ready(engine, &revised, position) &&
eskf_reset_position(&revised.filter, position->position_enu_m,
position->covariance_enu_m2))
{
/* This is a deliberate position-state reset, not a Kalman update with
* a disabled gate. The independent IMU-derived attitude and bias state
* survive, while invalid position cross-covariances are discarded. */
runtime_record_recovered_position(&revised, measurement);
runtime_record_recovered_position(&revised, position);
engine->diagnostics.position_recovery_count++;
position_recovery_clear(engine);
result = ESKF_UPDATE_ACCEPTED;
position_result = ESKF_UPDATE_ACCEPTED;
}
if(result == ESKF_UPDATE_ACCEPTED)
if(position_result == ESKF_UPDATE_ACCEPTED)
{
engine->diagnostics.position_accepted++;
}
@@ -947,9 +967,34 @@ static bool process_position_now(fusion_engine_t* engine,
{
engine->diagnostics.position_nis_rejected++;
}
double velocity_nis = NAN;
eskf_update_result_t velocity_result = ESKF_UPDATE_INVALID;
if(measurement->velocity_valid)
{
velocity_result = eskf_velocity_update(
&revised.filter, &engine->config.eskf,
measurement->velocity_enu_mps,
measurement->velocity_covariance_enu_mps2,
&velocity_nis);
if(velocity_result == ESKF_UPDATE_ACCEPTED)
{
engine->diagnostics.velocity_accepted++;
}
else if(velocity_result == ESKF_UPDATE_REJECTED_NIS)
{
engine->diagnostics.velocity_nis_rejected++;
}
else
{
/* A malformed optional Doppler covariance must not discard a valid
* position update from the same epoch. */
velocity_result = ESKF_UPDATE_INVALID;
}
}
engine->checkpoint = revised;
engine->checkpoint_valid = true;
history_drop_through(engine, measurement->timestamp_ticks);
history_drop_through(engine, position->timestamp_ticks);
for(unsigned int i = 0 ; i < engine->imu_history_count ; i++)
{
if(!runtime_apply_imu(engine, &revised, history_at(engine, i), false))
@@ -962,19 +1007,27 @@ static bool process_position_now(fusion_engine_t* engine,
engine->current = revised;
engine->diagnostics.replay_count++;
engine->diagnostics.replayed_imu_count += replayed;
if(result_out != NULL)
if(position_result_out != NULL)
{
*result_out = result;
*position_result_out = position_result;
}
if(nis_out != NULL)
if(position_nis_out != NULL)
{
*nis_out = nis;
*position_nis_out = position_nis;
}
if(velocity_result_out != NULL)
{
*velocity_result_out = velocity_result;
}
if(velocity_nis_out != NULL)
{
*velocity_nis_out = velocity_nis;
}
return true;
}
static bool pending_insert(fusion_engine_t* engine,
const fusion_position_measurement_t* measurement)
const fusion_navigation_measurement_t* measurement)
{
if(engine->pending_position_count >= FUSION_POSITION_PENDING_CAPACITY)
{
@@ -983,8 +1036,8 @@ static bool pending_insert(fusion_engine_t* engine,
}
unsigned int index = engine->pending_position_count;
while((index != 0u) &&
(engine->pending_positions[index - 1u].timestamp_ticks >
measurement->timestamp_ticks))
(engine->pending_positions[index - 1u].position.timestamp_ticks >
measurement->position.timestamp_ticks))
{
engine->pending_positions[index] = engine->pending_positions[index - 1u];
index--;
@@ -997,16 +1050,18 @@ static bool pending_insert(fusion_engine_t* engine,
static void process_ready_pending_positions(fusion_engine_t* engine)
{
while((engine->pending_position_count != 0u) &&
(engine->pending_positions[0].timestamp_ticks <=
(engine->pending_positions[0].position.timestamp_ticks <=
engine->current.filter_timestamp_ticks))
{
fusion_position_measurement_t measurement = engine->pending_positions[0];
fusion_navigation_measurement_t measurement =
engine->pending_positions[0];
for(unsigned int i = 1u ; i < engine->pending_position_count ; i++)
{
engine->pending_positions[i - 1u] = engine->pending_positions[i];
}
engine->pending_position_count--;
(void)process_position_now(engine, &measurement, NULL, NULL);
(void)process_navigation_now(
engine, &measurement, NULL, NULL, NULL, NULL);
if(engine->state == FUSION_ENGINE_FAULT)
{
return;
@@ -1056,6 +1111,7 @@ bool fusion_engine_push_position(
eskf_update_result_t* update_result_out,
double* nis_out)
{
fusion_navigation_measurement_t navigation;
if(update_result_out != NULL)
{
*update_result_out = ESKF_UPDATE_INVALID;
@@ -1064,28 +1120,70 @@ bool fusion_engine_push_position(
{
*nis_out = NAN;
}
if(measurement == NULL)
{
return false;
}
memset(&navigation, 0, sizeof(navigation));
navigation.position = *measurement;
return fusion_engine_push_navigation(
engine, &navigation, update_result_out, nis_out, NULL, NULL);
}
bool fusion_engine_push_navigation(
fusion_engine_t* engine,
const fusion_navigation_measurement_t* measurement,
eskf_update_result_t* position_update_result_out,
double* position_nis_out,
eskf_update_result_t* velocity_update_result_out,
double* velocity_nis_out)
{
if(position_update_result_out != NULL)
{
*position_update_result_out = ESKF_UPDATE_INVALID;
}
if(position_nis_out != NULL)
{
*position_nis_out = NAN;
}
if(velocity_update_result_out != NULL)
{
*velocity_update_result_out = ESKF_UPDATE_INVALID;
}
if(velocity_nis_out != NULL)
{
*velocity_nis_out = NAN;
}
if((engine == NULL) || (engine->state == FUSION_ENGINE_FAULT) ||
!position_valid(measurement))
!navigation_valid(measurement))
{
return false;
}
engine->diagnostics.position_received++;
if(!measurement->source_valid)
if(measurement->velocity_valid)
{
engine->diagnostics.velocity_received++;
}
if(!measurement->position.source_valid)
{
engine->diagnostics.position_source_rejected++;
return false;
}
if(engine->state == FUSION_ENGINE_CALIBRATING)
{
engine->initial_position = *measurement;
engine->initial_position = measurement->position;
engine->have_initial_position = true;
return true;
}
if(measurement->timestamp_ticks > engine->current.filter_timestamp_ticks)
if(measurement->position.timestamp_ticks >
engine->current.filter_timestamp_ticks)
{
return pending_insert(engine, measurement);
}
return process_position_now(engine, measurement, update_result_out, nis_out);
return process_navigation_now(
engine, measurement,
position_update_result_out, position_nis_out,
velocity_update_result_out, velocity_nis_out);
}
bool fusion_engine_get_runtime(const fusion_engine_t* engine,
soc/software/gps_system/src/fusion/fusion_gps_adapter.c
+108 -22
View File
@@ -80,6 +80,14 @@ void fusion_gps_adapter_default_config(fusion_gps_adapter_config_t* config)
config->minimum_horizontal_std_m = 0.50;
config->minimum_vertical_std_m = 1.00;
config->maximum_position_std_m = 1000.0;
/*
* Board data shows several m/s stationary speed error with weak
* four/five-satellite geometry even after Doppler IIR smoothing. These
* floors keep repeated, correlated velocity fixes useful but conservative.
*/
config->minimum_horizontal_velocity_std_mps = 2.0;
config->minimum_vertical_velocity_std_mps = 3.0;
config->maximum_velocity_std_mps = 100.0;
}
bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter,
@@ -88,7 +96,11 @@ bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter,
if((adapter == NULL) || (config == NULL) ||
!(config->minimum_horizontal_std_m > 0.0) ||
!(config->minimum_vertical_std_m > 0.0) ||
!(config->maximum_position_std_m >= config->minimum_vertical_std_m))
!(config->maximum_position_std_m >= config->minimum_vertical_std_m) ||
!(config->minimum_horizontal_velocity_std_mps > 0.0) ||
!(config->minimum_vertical_velocity_std_mps > 0.0) ||
!(config->maximum_velocity_std_mps >=
config->minimum_vertical_velocity_std_mps))
{
return false;
}
@@ -97,19 +109,19 @@ bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter,
return true;
}
static bool sanitize_covariance(const fusion_gps_adapter_t* adapter,
const double input[3][3], double output[9])
static bool sanitize_covariance(
double input[3][3],
double horizontal_std_floor,
double vertical_std_floor,
double maximum_std,
double output[9])
{
double variance_floor[3] = {
adapter->config.minimum_horizontal_std_m *
adapter->config.minimum_horizontal_std_m,
adapter->config.minimum_horizontal_std_m *
adapter->config.minimum_horizontal_std_m,
adapter->config.minimum_vertical_std_m *
adapter->config.minimum_vertical_std_m
horizontal_std_floor * horizontal_std_floor,
horizontal_std_floor * horizontal_std_floor,
vertical_std_floor * vertical_std_floor
};
double variance_ceiling = adapter->config.maximum_position_std_m *
adapter->config.maximum_position_std_m;
double variance_ceiling = maximum_std * maximum_std;
for(unsigned int axis = 0 ; axis < 3u ; axis++)
{
double variance = input[axis][axis];
@@ -124,7 +136,8 @@ static bool sanitize_covariance(const fusion_gps_adapter_t* adapter,
{
for(unsigned int col = row + 1u ; col < 3u ; col++)
{
double covariance = 0.5 * (input[row][col] + input[col][row]);
double covariance = 0.5 *
(input[row][col] + input[col][row]);
if(!isfinite(covariance))
{
return false;
@@ -146,23 +159,32 @@ static bool sanitize_covariance(const fusion_gps_adapter_t* adapter,
(determinant > DBL_EPSILON) && isfinite(determinant);
}
bool fusion_gps_adapter_convert(fusion_gps_adapter_t* adapter,
const fusion_gps_measurement_t* gps,
fusion_position_measurement_t* position_out)
bool fusion_gps_adapter_convert_navigation(
fusion_gps_adapter_t* adapter,
const fusion_gps_measurement_t* gps,
fusion_navigation_measurement_t* navigation_out)
{
if((adapter == NULL) || (gps == NULL) || (position_out == NULL) ||
if((adapter == NULL) || (gps == NULL) || (navigation_out == NULL) ||
!llh_valid(gps->latitude_deg, gps->longitude_deg, gps->height_m))
{
return false;
}
memset(position_out, 0, sizeof(*position_out));
memset(navigation_out, 0, sizeof(*navigation_out));
fusion_position_measurement_t* position_out = &navigation_out->position;
position_out->timestamp_ticks = gps->timestamp_ticks;
position_out->source_valid = gps->solution_valid;
position_out->source_quality =
(isfinite(gps->chi_square_ratio) && (gps->chi_square_ratio >= 0.0))
? 100.0 / (1.0 + gps->chi_square_ratio) : 0.0;
if(!sanitize_covariance(adapter, gps->covariance_enu_m2,
position_out->covariance_enu_m2))
double position_covariance_enu_m2[3][3];
memcpy(position_covariance_enu_m2, gps->covariance_enu_m2,
sizeof(position_covariance_enu_m2));
if(!sanitize_covariance(
position_covariance_enu_m2,
adapter->config.minimum_horizontal_std_m,
adapter->config.minimum_vertical_std_m,
adapter->config.maximum_position_std_m,
position_out->covariance_enu_m2))
{
return false;
}
@@ -189,9 +211,73 @@ bool fusion_gps_adapter_convert(fusion_gps_adapter_t* adapter,
adapter->ecef_to_enu[row][col] * delta_ecef_m[col];
}
}
return isfinite(position_out->position_enu_m[0]) &&
isfinite(position_out->position_enu_m[1]) &&
isfinite(position_out->position_enu_m[2]);
if(!isfinite(position_out->position_enu_m[0]) ||
!isfinite(position_out->position_enu_m[1]) ||
!isfinite(position_out->position_enu_m[2]))
{
return false;
}
if(gps->solution_valid && gps->velocity_valid)
{
bool velocity_finite = true;
for(unsigned int row = 0u; row < 3u; row++)
{
for(unsigned int column = 0u; column < 3u; column++)
{
navigation_out->velocity_enu_mps[row] +=
adapter->ecef_to_enu[row][column] *
gps->velocity_ecef_mps[column];
}
velocity_finite =
velocity_finite &&
isfinite(navigation_out->velocity_enu_mps[row]);
}
double covariance_enu_mps2[3][3] = {{0.0}};
for(unsigned int row = 0u; row < 3u; row++)
{
for(unsigned int column = 0u; column < 3u; column++)
{
for(unsigned int first = 0u; first < 3u; first++)
{
for(unsigned int second = 0u; second < 3u; second++)
{
covariance_enu_mps2[row][column] +=
adapter->ecef_to_enu[row][first] *
gps->velocity_covariance_ecef_mps2[first][second] *
adapter->ecef_to_enu[column][second];
}
}
}
}
navigation_out->velocity_valid =
velocity_finite &&
sanitize_covariance(
covariance_enu_mps2,
adapter->config.minimum_horizontal_velocity_std_mps,
adapter->config.minimum_vertical_velocity_std_mps,
adapter->config.maximum_velocity_std_mps,
navigation_out->velocity_covariance_enu_mps2);
}
return true;
}
bool fusion_gps_adapter_convert(fusion_gps_adapter_t* adapter,
const fusion_gps_measurement_t* gps,
fusion_position_measurement_t* position_out)
{
if(position_out == NULL)
{
return false;
}
fusion_navigation_measurement_t navigation;
if(!fusion_gps_adapter_convert_navigation(adapter, gps, &navigation))
{
return false;
}
*position_out = navigation.position;
return true;
}
bool fusion_gps_adapter_local_to_geodetic(
soc/software/gps_system/src/fusion/navigation_fusion.c
+10 -3
View File
@@ -90,12 +90,14 @@ bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement)
{
return false;
}
fusion_position_measurement_t position;
if(!fusion_gps_adapter_convert(&gps_adapter, measurement, &position))
fusion_navigation_measurement_t navigation;
if(!fusion_gps_adapter_convert_navigation(
&gps_adapter, measurement, &navigation))
{
return false;
}
return fusion_engine_push_position(&fusion_engine, &position, NULL, NULL);
return fusion_engine_push_navigation(
&fusion_engine, &navigation, NULL, NULL, NULL, NULL);
}
bool navigation_fusion_get_output(navigation_fusion_output_t* output)
@@ -111,6 +113,10 @@ bool navigation_fusion_get_output(navigation_fusion_output_t* output)
output->gps_nis_rejected = fusion_engine.diagnostics.position_nis_rejected;
output->gps_position_recoveries =
fusion_engine.diagnostics.position_recovery_count;
output->gps_velocity_accepted =
fusion_engine.diagnostics.velocity_accepted;
output->gps_velocity_nis_rejected =
fusion_engine.diagnostics.velocity_nis_rejected;
output->zero_update_count =
fusion_engine.diagnostics.zero_velocity_update_count;
output->replay_count = fusion_engine.diagnostics.replay_count;
@@ -136,6 +142,7 @@ bool navigation_fusion_get_output(navigation_fusion_output_t* output)
}
output->speed_mps = eskf_speed_mps(&runtime->filter);
output->last_position_nis = runtime->filter.last_position_nis;
output->last_velocity_nis = runtime->filter.last_velocity_nis;
output->motion_state = runtime->motion.state;
output->vehicle_at_rest = runtime->vehicle_at_rest;
output->local_valid = true;
soc/software/gps_system/src/gps/gps_ip.c
+1
View File
@@ -945,6 +945,7 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
velocity_measurements,
satellite_count,
solution.velocity_ecef_mps,
solution.velocity_covariance_ecef_mps2,
&solution.clock_drift_mps,
&solution.velocity_residual_rms_mps);
if(solution.velocity_valid)
soc/software/gps_system/src/gps/gps_tracking_measurement.c
+65 -1
View File
@@ -232,6 +232,35 @@ static bool solve_four_by_four(
return true;
}
static bool invert_four_by_four(double input[4][4], double inverse[4][4])
{
for(unsigned int column = 0u; column < 4u; column++)
{
double unit[4] = {0.0, 0.0, 0.0, 0.0};
double solution[4];
unit[column] = 1.0;
if(!solve_four_by_four(input, unit, solution))
{
return false;
}
for(unsigned int row = 0u; row < 4u; row++)
{
inverse[row][column] = solution[row];
}
}
for(unsigned int row = 0u; row < 4u; row++)
{
for(unsigned int column = row + 1u; column < 4u; column++)
{
double symmetric =
0.5 * (inverse[row][column] + inverse[column][row]);
inverse[row][column] = symmetric;
inverse[column][row] = symmetric;
}
}
return true;
}
bool gps_tracking_velocity_wls(
const double receiver_ecef_m[3],
const double satellite_positions_m[][3],
@@ -240,6 +269,7 @@ bool gps_tracking_velocity_wls(
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)
{
@@ -248,6 +278,7 @@ bool gps_tracking_velocity_wls(
(satellite_clock_drifts_sps == NULL) ||
(measurements == NULL) ||
(receiver_velocity_ecef_mps_out == NULL) ||
(receiver_velocity_covariance_ecef_mps2_out == NULL) ||
(receiver_clock_drift_mps_out == NULL) ||
(residual_rms_mps_out == NULL) ||
(count < GPS_TRACK_MEASUREMENT_MIN_SATELLITES) ||
@@ -260,6 +291,7 @@ bool gps_tracking_velocity_wls(
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 used = 0u;
for(unsigned int satellite = 0u; satellite < count; satellite++)
@@ -309,6 +341,7 @@ bool gps_tracking_velocity_wls(
30.0 * pow(10.0, -measurement->cn0_dbhz / 20.0);
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] +=
@@ -329,12 +362,15 @@ bool gps_tracking_velocity_wls(
}
double solution[4];
if(!solve_four_by_four(normal, rhs, solution))
double normal_inverse[4][4];
if(!solve_four_by_four(normal, rhs, solution) ||
!invert_four_by_four(normal, normal_inverse))
{
return false;
}
double residual_square_sum = 0.0;
double weighted_residual_square_sum = 0.0;
for(unsigned int row = 0u; row < used; row++)
{
double predicted = 0.0;
@@ -344,8 +380,36 @@ bool gps_tracking_velocity_wls(
}
double residual = observation[row] - predicted;
residual_square_sum += residual * residual;
weighted_residual_square_sum +=
weights[row] * residual * residual;
}
/*
* The C/N0 model supplies absolute range-rate sigmas. Inflate the formal
* inverse-normal covariance when the post-fit residuals are larger than
* that model predicts; never shrink it from a fortuitously quiet epoch.
* Four-satellite epochs have zero redundancy, so retain scale 1 and let
* the poor geometry appear directly in the inverse normal matrix.
*/
double covariance_scale = 1.0;
if(used > 4u)
{
covariance_scale = fmax(
1.0, weighted_residual_square_sum / (double)(used - 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];
if(!isfinite(
receiver_velocity_covariance_ecef_mps2_out[row][column]))
{
return false;
}
}
}
memcpy(receiver_velocity_ecef_mps_out, solution,
3u * sizeof(solution[0]));
*receiver_clock_drift_mps_out = solution[3];