Merge pull request #10243 from breadoven/abo_nav_vel_z_improvements

Nav velocity Z estimation improvements

Author: breadoven

docs/Settings.md

@@ -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.
@@ -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.

src/main/fc/settings.yaml

@@ -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
@@ -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
@@ -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

src/main/navigation/navigation.h

@@ -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

src/main/navigation/navigation_pos_estimator.c

@@ -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
@@ -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,
@@ -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
@@ -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)
@@ -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)) {
@@ -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;
@@ -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 {
@@ -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;
     }
 
@@ -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 */
@@ -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)) {
@@ -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;
@@ -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;
@@ -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);
@@ -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
@@ -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;
         }
     }