Skip to content

Position estimator improvements #10818

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Position estimator improvements #10818

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
c7f5f1d
pos estimator changes
breadoven Apr 17, 2025
99ccca5
Update navigation_pos_estimator.c
breadoven Apr 18, 2025
b7ce2f4
fixes
breadoven Apr 21, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions docs/Settings.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2218,7 +2218,7 @@ Weight of barometer climb rate measurements in estimated climb rate. Setting is

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

---

Expand All @@ -2228,7 +2228,7 @@ Weight of GPS altitude measurements in estimated altitude. Setting is used on bo

| Default | Min | Max |
| --- | --- | --- |
| 0.2 | 0 | 10 |
| 0.35 | 0 | 10 |

---

Expand All @@ -2238,7 +2238,7 @@ Weight of GPS climb rate measurements in estimated climb rate. Setting is used o

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

---

Expand Down
6 changes: 3 additions & 3 deletions src/main/fc/settings.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2429,19 +2429,19 @@ groups:
field: w_z_baro_v
min: 0
max: 10
default_value: 0.1
default_value: 0.35
- 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
min: 0
max: 10
default_value: 0.2
default_value: 0.35
- name: inav_w_z_gps_v
description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors."
field: w_z_gps_v
min: 0
max: 10
default_value: 0.1
default_value: 0.35
- name: inav_w_xy_gps_p
description: "Weight of GPS coordinates in estimated UAV position and speed."
default_value: 1.0
Expand Down
88 changes: 38 additions & 50 deletions src/main/navigation/navigation_pos_estimator.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -367,14 +367,6 @@ static void updateIMUEstimationWeight(const float dt)
DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000);
}

float navGetAccelerometerWeight(void)
{
const float accWeightScaled = posEstimator.imu.accWeightFactor;
DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);

return accWeightScaled;
}

static void updateIMUTopic(timeUs_t currentTimeUs)
{
const float dt = US2S(currentTimeUs - posEstimator.imu.lastUpdateTime);
Expand All @@ -391,25 +383,20 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
/* Update acceleration weight based on vibration levels and clipping */
updateIMUEstimationWeight(dt);

fpVector3_t accelBF;
fpVector3_t accelReading;

/* Read acceleration data in body frame */
accelBF.x = imuMeasuredAccelBF.x;
accelBF.y = imuMeasuredAccelBF.y;
accelBF.z = imuMeasuredAccelBF.z;

/* Correct accelerometer bias */
accelBF.x -= posEstimator.imu.accelBias.x;
accelBF.y -= posEstimator.imu.accelBias.y;
accelBF.z -= posEstimator.imu.accelBias.z;
accelReading.x = imuMeasuredAccelBF.x;
accelReading.y = imuMeasuredAccelBF.y;
accelReading.z = imuMeasuredAccelBF.z;

/* Rotate vector to Earth frame - from Forward-Right-Down to North-East-Up*/
imuTransformVectorBodyToEarth(&accelBF);
/* Adjust reading from Body to Earth frame - from Forward-Right-Down to North-East-Up*/
imuTransformVectorBodyToEarth(&accelReading);

/* Read acceleration data in NEU frame from IMU */
posEstimator.imu.accelNEU.x = accelBF.x;
posEstimator.imu.accelNEU.y = accelBF.y;
posEstimator.imu.accelNEU.z = accelBF.z;
/* Apply reading to NEU frame including correction for accelerometer bias */
posEstimator.imu.accelNEU.x = accelReading.x + posEstimator.imu.accelBias.x;
posEstimator.imu.accelNEU.y = accelReading.y + posEstimator.imu.accelBias.y;
posEstimator.imu.accelNEU.z = accelReading.z + posEstimator.imu.accelBias.z;

/* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
if (gyroConfig()->init_gyro_cal_enabled) {
Expand Down Expand Up @@ -600,22 +587,22 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
// We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it
bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
(((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
(posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));

// Altitude
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;
const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v;

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->estVelCorr.z += baroVelZResidual * 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 -= baroAltResidual * sq(w_z_baro_p);
ctx->accBiasCorr.z += (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v));
}

correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed
Expand All @@ -633,19 +620,26 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
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;
const float w_z_gps_v = positionEstimationConfig()->w_z_gps_v;

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->estVelCorr.z += gpsVelZResidual * 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 -= gpsAltResidual * sq(w_z_gps_p);
ctx->accBiasCorr.z += (gpsAltResidual * sq(w_z_gps_p) + gpsVelZResidual * sq(w_z_gps_v));
}

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

// Double corrections if only single sensor used
if (wGps == 0 || wBaro == 0) {
ctx->estPosCorr.z *= 2.0f;
ctx->estVelCorr.z *= 2.0f;
ctx->accBiasCorr.z *= 2.0f;
}

return correctOK;
}

Expand Down Expand Up @@ -677,17 +671,13 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
ctx->estPosCorr.x += gpsPosXResidual * w_xy_gps_p * ctx->dt;
ctx->estPosCorr.y += gpsPosYResidual * w_xy_gps_p * ctx->dt;

// Velocity from coordinates
ctx->estVelCorr.x += gpsPosXResidual * sq(w_xy_gps_p) * ctx->dt;
ctx->estVelCorr.y += gpsPosYResidual * sq(w_xy_gps_p) * ctx->dt;

// Velocity from direct measurement
ctx->estVelCorr.x += gpsVelXResidual * w_xy_gps_v * ctx->dt;
ctx->estVelCorr.y += gpsVelYResidual * w_xy_gps_v * ctx->dt;

// Accelerometer bias
ctx->accBiasCorr.x -= gpsPosXResidual * sq(w_xy_gps_p);
ctx->accBiasCorr.y -= gpsPosYResidual * sq(w_xy_gps_p);
ctx->accBiasCorr.x += (gpsPosXResidual * sq(w_xy_gps_p) + gpsVelXResidual * sq(w_xy_gps_v));
ctx->accBiasCorr.y += (gpsPosYResidual * sq(w_xy_gps_p) + gpsVelYResidual * sq(w_xy_gps_v));

/* Adjust EPH */
ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p);
Expand Down Expand Up @@ -734,7 +724,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
return;
}

/* Calculate new EPH and EPV for the case we didn't update postion */
/* Calculate new EPH and EPV for the case we didn't update position */
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);
Expand Down Expand Up @@ -768,26 +758,24 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
}
// Boost the corrections based on accWeight
const float accWeight = navGetAccelerometerWeight();
vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f/accWeight);
vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f/accWeight);
vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f / posEstimator.imu.accWeightFactor);
vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f / posEstimator.imu.accWeightFactor);

// Apply corrections
vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr);
vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);

/* Correct accelerometer bias */
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 * 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;
}
ctx.accBiasCorr.x = constrainf(ctx.accBiasCorr.x, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE);
ctx.accBiasCorr.y = constrainf(ctx.accBiasCorr.y, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE);
ctx.accBiasCorr.z = constrainf(ctx.accBiasCorr.z, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE);

/* Correct accel bias */
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;
}

/* Update ground course */
Expand Down
5 changes: 2 additions & 3 deletions src/main/navigation/navigation_pos_estimator_agl.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,10 +147,9 @@ void estimationCalculateAGL(estimationContext_t * ctx)
}

// Update estimate
const float accWeight = navGetAccelerometerWeight();
posEstimator.est.aglAlt += posEstimator.est.aglVel * ctx->dt;
posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * posEstimator.imu.accWeightFactor;
posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(posEstimator.imu.accWeightFactor);

// Apply correction
if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
Expand Down
1 change: 0 additions & 1 deletion src/main/navigation/navigation_pos_estimator_private.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -195,5 +195,4 @@ extern navigationPosEstimator_t posEstimator;
extern float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w);
extern void estimationCalculateAGL(estimationContext_t * ctx);
extern bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx);
extern float navGetAccelerometerWeight(void);

Loading