Skip to content

Commit

Permalink
Merge pull request iNavFlight#10243 from breadoven/abo_nav_vel_z_impr…
Browse files Browse the repository at this point in the history 
…ovements

Nav velocity Z estimation improvements
  • Loading branch information
@breadoven
breadoven authored Oct 3, 2024
2 parents 678434f + 1ed4875 commit 806b47a
Show file tree
Hide file tree
Showing 5 changed files with 122 additions and 56 deletions.
20 changes: 20 additions & 0 deletions docs/Settings.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2002,6 +2002,16 @@ Uncertainty value for barometric sensor [cm]

---

### inav_default_alt_sensor

Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use.

| Default | Min | Max |
| --- | --- | --- |
| GPS | | |

---

### inav_gravity_cal_tolerance

Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value.
Expand Down Expand Up @@ -2122,6 +2132,16 @@ Weight of barometer measurements in estimated altitude and climb rate. Setting i

---

### inav_w_z_baro_v

Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors.

| Default | Min | Max |
| --- | --- | --- |
| 0.1 | 0 | 10 |

---

### inav_w_z_gps_p

Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors.
Expand Down
14 changes: 14 additions & 0 deletions src/main/fc/settings.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -201,6 +201,9 @@ tables:
- name: mavlink_radio_type
values: ["GENERIC", "ELRS", "SIK"]
enum: mavlinkRadio_e
- name: default_altitude_source
values: ["GPS", "BARO", "GPS ONLY", "BARO ONLY"]
enum: navDefaultAltitudeSensor_e

constants:
RPYL_PID_MIN: 0
Expand Down Expand Up @@ -2410,6 +2413,12 @@ groups:
min: 0
max: 10
default_value: 0.35
- name: inav_w_z_baro_v
description: "Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors."
field: w_z_baro_v
min: 0
max: 10
default_value: 0.1
- name: inav_w_z_gps_p
description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
field: w_z_gps_p
Expand Down Expand Up @@ -2464,6 +2473,11 @@ groups:
field: baro_epv
min: 0
max: 9999
- name: inav_default_alt_sensor
description: "Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use."
default_value: "GPS"
field: default_alt_sensor
table: default_altitude_source

- name: PG_NAV_CONFIG
type: navConfig_t
Expand Down
30 changes: 16 additions & 14 deletions src/main/navigation/navigation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -231,35 +231,37 @@ typedef enum {

typedef struct positionEstimationConfig_s {
uint8_t automatic_mag_declination;
uint8_t reset_altitude_type; // from nav_reset_type_e
uint8_t reset_home_type; // nav_reset_type_e
uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s)
uint8_t reset_altitude_type; // from nav_reset_type_e
uint8_t reset_home_type; // nav_reset_type_e
uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s)
uint8_t allow_dead_reckoning;

uint16_t max_surface_altitude;

float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements
float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements
float w_z_baro_v; // Weight (cutoff frequency) for barometer climb rate measurements

float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements
float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements
float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements
float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements

float w_z_gps_p; // GPS altitude data is very noisy and should be used only on airplanes
float w_z_gps_v; // Weight (cutoff frequency) for GPS climb rate measurements
float w_z_gps_p; // GPS altitude data is very noisy and should be used only on airplanes
float w_z_gps_v; // Weight (cutoff frequency) for GPS climb rate measurements

float w_xy_gps_p; // Weight (cutoff frequency) for GPS position measurements
float w_xy_gps_v; // Weight (cutoff frequency) for GPS velocity measurements
float w_xy_gps_p; // Weight (cutoff frequency) for GPS position measurements
float w_xy_gps_v; // Weight (cutoff frequency) for GPS velocity measurements

float w_xy_flow_p;
float w_xy_flow_v;

float w_z_res_v; // When velocity sources lost slowly decrease estimated velocity with this weight
float w_z_res_v; // When velocity sources lost slowly decrease estimated velocity with this weight
float w_xy_res_v;

float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.

float max_eph_epv; // Max estimated position error acceptable for estimation (cm)
float baro_epv; // Baro position error
float max_eph_epv; // Max estimated position error acceptable for estimation (cm)
float baro_epv; // Baro position error

uint8_t default_alt_sensor; // default altitude sensor source
#ifdef USE_GPS_FIX_ESTIMATION
uint8_t allow_gps_fix_estimation;
#endif
Expand Down
107 changes: 65 additions & 42 deletions src/main/navigation/navigation_pos_estimator.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@
navigationPosEstimator_t posEstimator;
static float initialBaroAltitudeOffset = 0.0f;

PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 7);
PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 8);

PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
// Inertial position estimator parameters
Expand All @@ -69,6 +69,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
.max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,

.w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT,
.w_z_baro_v = SETTING_INAV_W_Z_BARO_V_DEFAULT,

.w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT,
.w_z_surface_v = SETTING_INAV_W_Z_SURFACE_V_DEFAULT,
Expand All @@ -89,6 +90,8 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,

.max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT,
.baro_epv = SETTING_INAV_BARO_EPV_DEFAULT,

.default_alt_sensor = SETTING_INAV_DEFAULT_ALT_SENSOR_DEFAULT,
#ifdef USE_GPS_FIX_ESTIMATION
.allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT
#endif
Expand Down Expand Up @@ -342,6 +345,7 @@ static bool gravityCalibrationComplete(void)

return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
}

#define ACC_VIB_FACTOR_S 1.0f
#define ACC_VIB_FACTOR_E 3.0f
static void updateIMUEstimationWeight(const float dt)
Expand Down Expand Up @@ -423,7 +427,6 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal;
}

/* If calibration is incomplete - report zero acceleration */
if (gravityCalibrationComplete()) {
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) {
Expand All @@ -432,7 +435,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
#endif
posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS;
}
else {
else { // If calibration is incomplete - report zero acceleration
posEstimator.imu.accelNEU.x = 0.0f;
posEstimator.imu.accelNEU.y = 0.0f;
posEstimator.imu.accelNEU.z = 0.0f;
Expand Down Expand Up @@ -476,14 +479,16 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
/* Figure out if we have valid position data from our data sources */
uint32_t newFlags = 0;

const float max_eph_epv = positionEstimationConfig()->max_eph_epv;

if ((sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && posControl.gpsOrigin.valid &&
((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
(posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) {
if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) {
(posEstimator.gps.eph < max_eph_epv)) {
if (posEstimator.gps.epv < max_eph_epv) {
newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID;
}
else {
Expand All @@ -503,11 +508,11 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
newFlags |= EST_FLOW_VALID;
}

if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
if (posEstimator.est.eph < max_eph_epv) {
newFlags |= EST_XY_VALID;
}

if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
if (posEstimator.est.epv < max_eph_epv) {
newFlags |= EST_Z_VALID;
}

Expand All @@ -521,7 +526,9 @@ static void estimationPredict(estimationContext_t * ctx)
if ((ctx->newFlags & EST_Z_VALID)) {
posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
if (ARMING_FLAG(WAS_EVER_ARMED)) { // Hold at zero until first armed
posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
}
}

/* Prediction step: XY-axis */
Expand Down Expand Up @@ -552,20 +559,28 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count

bool correctOK = false;
const uint8_t defaultAltitudeSource = positionEstimationConfig()->default_alt_sensor;
float wGps = defaultAltitudeSource == ALTITUDE_SOURCE_BARO_ONLY && ctx->newFlags & EST_BARO_VALID ? 0.0f : 1.0f;
float wBaro = defaultAltitudeSource == ALTITUDE_SOURCE_GPS_ONLY && ctx->newFlags & EST_GPS_Z_VALID ? 0.0f : 1.0f;

float wBaro = 0.0f;
if (ctx->newFlags & EST_BARO_VALID) {
// Ignore baro if difference is too big, baro is probably wrong
const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f;
if (wBaro && ctx->newFlags & EST_BARO_VALID && wGps && ctx->newFlags & EST_GPS_Z_VALID) {
const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt);

// Fade out the baro to prevent sudden jump
const float start_epv = positionEstimationConfig()->max_eph_epv;
const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f;
// Fade out the non default sensor to prevent sudden jump
uint16_t residualErrorEpvLimit = defaultAltitudeSource == ALTITUDE_SOURCE_BARO ? 2 * positionEstimationConfig()->baro_epv : positionEstimationConfig()->max_eph_epv;
const float start_epv = residualErrorEpvLimit;
const float end_epv = residualErrorEpvLimit * 2.0f;

// Calculate residual gps/baro sensor weighting based on assumed default altitude source = GPS
wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);

if (defaultAltitudeSource == ALTITUDE_SOURCE_BARO) { // flip residual sensor weighting if default = BARO
wGps = wBaro;
wBaro = 1.0f;
}
}

// Always use Baro if no GPS otherwise only use if accuracy OK compared to GPS
if (wBaro > 0.01f) {
if (ctx->newFlags & EST_BARO_VALID && wBaro) {
timeUs_t currentTimeUs = micros();

if (!ARMING_FLAG(ARMED)) {
Expand All @@ -588,21 +603,25 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));

// Altitude
const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
ctx->estPosCorr.z += wBaro * baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
ctx->estVelCorr.z += wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
const float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z);
const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z);
const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p;

ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
ctx->estPosCorr.z += baroAltResidual * w_z_baro_p * ctx->dt;
ctx->estVelCorr.z += baroAltResidual * sq(w_z_baro_p) * ctx->dt;
ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_v * ctx->dt;

ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, w_z_baro_p);

// Accelerometer bias
if (!isAirCushionEffectDetected) {
ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
ctx->accBiasCorr.z -= baroAltResidual * sq(w_z_baro_p);
}

correctOK = true;
correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed
}

if (ctx->newFlags & EST_GPS_Z_VALID) {
if (ctx->newFlags & EST_GPS_Z_VALID && wGps) {
// Reset current estimate to GPS altitude if estimate not valid
if (!(ctx->newFlags & EST_Z_VALID)) {
ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
Expand All @@ -611,19 +630,20 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
}
else {
// Altitude
const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z;
const float gpsVelZResudual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z);
const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z);
const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p;

ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResudual)), positionEstimationConfig()->w_z_gps_p);
ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt;
ctx->estVelCorr.z += gpsAltResidual * sq(w_z_gps_p) * ctx->dt;
ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p);

// Accelerometer bias
ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
ctx->accBiasCorr.z -= gpsAltResidual * sq(w_z_gps_p);
}

correctOK = true;
correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed
}

return correctOK;
Expand Down Expand Up @@ -700,21 +720,23 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
{
estimationContext_t ctx;

const float max_eph_epv = positionEstimationConfig()->max_eph_epv;

/* Calculate dT */
ctx.dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime);
posEstimator.est.lastUpdateTime = currentTimeUs;

/* If IMU is not ready we can't estimate anything */
if (!isImuReady()) {
posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
posEstimator.est.eph = max_eph_epv + 0.001f;
posEstimator.est.epv = max_eph_epv + 0.001f;
posEstimator.flags = 0;
return;
}

/* Calculate new EPH and EPV for the case we didn't update postion */
ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs);
vectorZero(&ctx.estPosCorr);
vectorZero(&ctx.estVelCorr);
Expand All @@ -737,12 +759,12 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
estimationCalculateCorrection_XY_FLOW(&ctx);

// If we can't apply correction or accuracy is off the charts - decay velocity to zero
if (!estXYCorrectOk || ctx.newEPH > positionEstimationConfig()->max_eph_epv) {
if (!estXYCorrectOk || ctx.newEPH > max_eph_epv) {
ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
}

if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) {
if (!estZCorrectOk || ctx.newEPV > max_eph_epv) {
ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
}
// Boost the corrections based on accWeight
Expand All @@ -754,16 +776,17 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);

/* Correct accelerometer bias */
if (positionEstimationConfig()->w_acc_bias > 0.0f) {
const float w_acc_bias = positionEstimationConfig()->w_acc_bias;
if (w_acc_bias > 0.0f) {
const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z);
if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
/* transform error vector from NEU frame to body frame */
imuTransformVectorEarthToBody(&ctx.accBiasCorr);

/* Correct accel bias */
posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * positionEstimationConfig()->w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * positionEstimationConfig()->w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * positionEstimationConfig()->w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt;
}
}

Expand Down
Loading

0 comments on commit 806b47a

Please sign in to comment.