diff --git a/soc/software/gps_system/header/adapters/gps_system_uart.h b/soc/software/gps_system/header/adapters/gps_system_uart.h index 189e643..71ba09e 100644 --- a/soc/software/gps_system/header/adapters/gps_system_uart.h +++ b/soc/software/gps_system/header/adapters/gps_system_uart.h @@ -17,6 +17,11 @@ void gps_system_uart_publish_location( void gps_system_uart_publish_fused_location( const navigation_fusion_output_t* output); +/* Raw Doppler velocity beside the ESKF velocity state, for tuning the + * velocity covariance floors and the zero-velocity veto threshold. */ +void gps_system_uart_publish_velocity( + const navigation_fusion_output_t* output); + void gps_system_uart_publish_startup( bool fusion_enabled, bool fusion_initialized, diff --git a/soc/software/gps_system/header/fusion/navigation_fusion.h b/soc/software/gps_system/header/fusion/navigation_fusion.h index be44b70..6783fa5 100644 --- a/soc/software/gps_system/header/fusion/navigation_fusion.h +++ b/soc/software/gps_system/header/fusion/navigation_fusion.h @@ -13,7 +13,20 @@ typedef struct { uint64_t timestamp_ticks; uint64_t last_accepted_gps_ticks; double position_enu_m[3]; + /* ESKF velocity state. An IMU cannot measure speed -- this is the + * integrated result, corrected by GNSS, and is the only meaningful + * "inertial speed" the system has. */ double velocity_enu_mps[3]; + /* + * Last GNSS Doppler velocity in the same ENU frame, held so telemetry can + * difference it against the state above. Characterising the receiver's + * velocity noise needs the raw observation, not the filtered estimate. + */ + double gps_velocity_enu_mps[3]; + double gps_speed_mps; + double gps_velocity_residual_rms_mps; + uint64_t gps_velocity_sample_ticks; + bool gps_velocity_valid; double latitude_deg; double longitude_deg; double height_m; diff --git a/soc/software/gps_system/src/adapters/gps_system_uart.c b/soc/software/gps_system/src/adapters/gps_system_uart.c index 9f1d485..4541eb8 100644 --- a/soc/software/gps_system/src/adapters/gps_system_uart.c +++ b/soc/software/gps_system/src/adapters/gps_system_uart.c @@ -64,6 +64,50 @@ void gps_system_uart_publish_fused_location( output->height_m, (unsigned long)age_ms); } +/* + * Velocity telemetry for tuning. + * + * An IMU does not measure speed; it measures specific force. eskf_* below is + * the integrated velocity state after GNSS correction, which is the only + * inertial speed the system has, and gps_* is the raw Doppler observation that + * corrected it. Logging both in one line, in one frame, at one instant is + * what makes them differenceable -- reading them from separate records at + * different rates does not. + * + * The difference is the velocity innovation. With the platform held still it + * is the receiver's Doppler noise, which is what sizes the velocity covariance + * floors and the zero-velocity veto threshold; in motion it is the residual + * the filter is correcting. + */ +void gps_system_uart_publish_velocity(const navigation_fusion_output_t* output) +{ + if((output == NULL) || !output->local_valid || !output->gps_velocity_valid) + { + return; + } + double innovation[3]; + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + innovation[axis] = output->gps_velocity_enu_mps[axis] - + output->velocity_enu_mps[axis]; + } + GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION, + "vel gps_e=%.3lf gps_n=%.3lf gps_u=%.3lf gps_speed=%.3lf " + "gps_rms=%.3lf eskf_e=%.3lf eskf_n=%.3lf eskf_u=%.3lf " + "eskf_speed=%.3lf inn_e=%.3lf inn_n=%.3lf inn_u=%.3lf " + "vel_nis=%.3lf accepted=%lu rejected=%lu at_rest=%u\r\n", + output->gps_velocity_enu_mps[0], output->gps_velocity_enu_mps[1], + output->gps_velocity_enu_mps[2], output->gps_speed_mps, + output->gps_velocity_residual_rms_mps, + output->velocity_enu_mps[0], output->velocity_enu_mps[1], + output->velocity_enu_mps[2], output->speed_mps, + innovation[0], innovation[1], innovation[2], + output->last_velocity_nis, + (unsigned long)output->gps_velocity_accepted, + (unsigned long)output->gps_velocity_nis_rejected, + output->vehicle_at_rest ? 1u : 0u); +} + void gps_system_uart_publish_startup( bool fusion_enabled, bool fusion_initialized, @@ -139,6 +183,7 @@ void gps_system_uart_publish_periodic_fusion_status( (unsigned long)output->imu_processed_count, (unsigned long)imu.queue_overflow_count, (unsigned long)imu.estimated_missed_sample_count); + gps_system_uart_publish_velocity(output); return; } diff --git a/soc/software/gps_system/src/fusion/navigation_fusion.c b/soc/software/gps_system/src/fusion/navigation_fusion.c index 5c28764..ef6cb19 100644 --- a/soc/software/gps_system/src/fusion/navigation_fusion.c +++ b/soc/software/gps_system/src/fusion/navigation_fusion.c @@ -12,6 +12,13 @@ static fusion_engine_t fusion_engine; static fusion_rest_detector_t rest_detector; static bool fusion_initialized; static uint64_t imu_processed_count; +/* Last Doppler velocity handed to the filter, in ENU. Kept so telemetry can + * report the raw observation alongside the state it corrected. */ +static double last_gps_velocity_enu_mps[3]; +static double last_gps_speed_mps; +static double last_gps_velocity_rms_mps; +static uint64_t last_gps_velocity_ticks; +static bool last_gps_velocity_valid; bool navigation_fusion_init( uint64_t local_timebase_hz, @@ -96,6 +103,15 @@ bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement) { return false; } + last_gps_velocity_valid = navigation.velocity_valid; + if(navigation.velocity_valid) + { + memcpy(last_gps_velocity_enu_mps, navigation.velocity_enu_mps, + sizeof(last_gps_velocity_enu_mps)); + last_gps_speed_mps = measurement->speed_mps; + last_gps_velocity_rms_mps = measurement->velocity_residual_rms_mps; + last_gps_velocity_ticks = measurement->timestamp_ticks; + } return fusion_engine_push_navigation( &fusion_engine, &navigation, NULL, NULL, NULL, NULL); } @@ -141,6 +157,12 @@ bool navigation_fusion_get_output(navigation_fusion_output_t* output) sqrt(fmax(0.0, runtime->filter.covariance[axis][axis])); } output->speed_mps = eskf_speed_mps(&runtime->filter); + memcpy(output->gps_velocity_enu_mps, last_gps_velocity_enu_mps, + sizeof(output->gps_velocity_enu_mps)); + output->gps_speed_mps = last_gps_speed_mps; + output->gps_velocity_residual_rms_mps = last_gps_velocity_rms_mps; + output->gps_velocity_sample_ticks = last_gps_velocity_ticks; + output->gps_velocity_valid = last_gps_velocity_valid; output->linear_acceleration_norm_mps2 = runtime->motion.last_linear_accel_norm_mps2; output->last_position_nis = runtime->filter.last_position_nis;