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

feat(telemetry): report Doppler velocity beside the ESKF velocity state

Browse Source 
Tuning the velocity covariance floors and the zero-velocity veto
threshold needs the receiver's velocity noise, which cannot be recovered
from the existing records: the raw observation and the filter state were
logged in different lines at different rates, so they could not be
differenced.

Hold the converted ENU Doppler velocity from the last fix and publish it
alongside the filter state in one line at one instant, with their
difference (the velocity innovation) and the at-rest flag. With the
platform still, the raw reading is the noise itself.

An IMU does not measure speed; the eskf_* fields are the integrated
result after GNSS correction, and are the only inertial speed available.

First capture, stationary: per-axis sigma 0.12 E, 0.25 N, 0.59 U m/s.
The vertical channel carries most of it, which the previous scalar speed
field could not show.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
onur cayan
2026-08-07 22:06:01 +03:00
committed by onur
parent 18d338fe50
commit 80ef92ba01
4 changed files with 85 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/header/adapters/gps_system_uart.h
+5
View File
@@ -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,
soc/software/gps_system/header/fusion/navigation_fusion.h
+13
View File
@@ -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;
soc/software/gps_system/src/adapters/gps_system_uart.c
+45
View File
@@ -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;
}
soc/software/gps_system/src/fusion/navigation_fusion.c
+22
View File
@@ -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;