onur cayan
80ef92ba01
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>
84 lines
2.8 KiB
C
84 lines
2.8 KiB
C
#ifndef NAVIGATION_FUSION_H
|
|
#define NAVIGATION_FUSION_H
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
|
|
#include "fusion/fusion_engine.h"
|
|
#include "fusion/fusion_measurement.h"
|
|
#include "fusion/fusion_motion.h"
|
|
#include "imu/imu_sample.h"
|
|
|
|
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;
|
|
double position_std_m[3];
|
|
double speed_mps;
|
|
double linear_acceleration_norm_mps2;
|
|
double last_position_nis;
|
|
double last_velocity_nis;
|
|
fusion_motion_state_t motion_state;
|
|
uint64_t gps_accepted;
|
|
uint64_t gps_nis_rejected;
|
|
uint64_t gps_position_recoveries;
|
|
uint64_t gps_velocity_accepted;
|
|
uint64_t gps_velocity_nis_rejected;
|
|
uint64_t zero_update_count;
|
|
uint64_t replay_count;
|
|
uint64_t stale_gps_count;
|
|
uint64_t imu_processed_count;
|
|
unsigned int calibration_sample_count;
|
|
fusion_engine_state_t engine_state;
|
|
bool local_valid;
|
|
bool global_valid;
|
|
bool vehicle_at_rest;
|
|
/* Compatibility alias for UART publishers: valid means global LLH valid. */
|
|
bool valid;
|
|
} navigation_fusion_output_t;
|
|
|
|
/* Initialize the sensor-independent 15-state ESKF and GPS-to-ENU adapter. */
|
|
bool navigation_fusion_init(
|
|
uint64_t local_timebase_hz,
|
|
const fusion_imu_quality_profile_t* imu_quality,
|
|
bool platform_landed);
|
|
|
|
/* PX4-style external platform fact. For this bench prototype `true` means
|
|
* unarmed/on-ground; a mobile integration must drive it from its own
|
|
* arm/contact/land supervisor. */
|
|
void navigation_fusion_set_platform_landed(
|
|
bool platform_landed,
|
|
uint64_t timestamp_ticks);
|
|
|
|
/* Prediction input: every timestamped IMU sample is integrated; it is not an
|
|
* ESKF observation and is never rate-selected in this layer. */
|
|
bool navigation_fusion_push_imu(const imu_sample_t* measurement);
|
|
|
|
/* Observation input: convert a valid LLH fix and optional Doppler velocity to
|
|
* local ENU, apply both at the captured GPS epoch, then replay newer IMU
|
|
* history to current time. */
|
|
bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement);
|
|
|
|
/* local_valid can exist without GPS. global_valid/valid require a GPS origin. */
|
|
bool navigation_fusion_get_output(navigation_fusion_output_t* output);
|
|
const char* navigation_fusion_state_name(fusion_engine_state_t state);
|
|
|
|
#endif
|