feat(telemetry): report Doppler velocity beside the ESKF velocity state
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:
@@ -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,
|
||||
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user