Skip to content

Commit 0c06275

Browse files
breadovenbreadoven
authored
Merge pull request iNavFlight#10818 from breadoven/abo_pos_estimator_changes
Position estimator improvements
2 parents d44e832 + 99c7e3d commit 0c06275

5 files changed

Lines changed: 50 additions & 61 deletions

File tree

‎docs/Settings.md‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -2248,7 +2248,7 @@ Weight of barometer climb rate measurements in estimated climb rate. Setting is
22482248

22492249
| Default | Min | Max |
22502250
| --- | --- | --- |
2251-
| 0.1 | 0 | 10 |
2251+
| 0.35 | 0 | 10 |
22522252

22532253
---
22542254

@@ -2258,7 +2258,7 @@ Weight of GPS altitude measurements in estimated altitude. Setting is used on bo
22582258

22592259
| Default | Min | Max |
22602260
| --- | --- | --- |
2261-
| 0.2 | 0 | 10 |
2261+
| 0.35 | 0 | 10 |
22622262

22632263
---
22642264

@@ -2268,7 +2268,7 @@ Weight of GPS climb rate measurements in estimated climb rate. Setting is used o
22682268

22692269
| Default | Min | Max |
22702270
| --- | --- | --- |
2271-
| 0.1 | 0 | 10 |
2271+
| 0.35 | 0 | 10 |
22722272

22732273
---
22742274

‎src/main/fc/settings.yaml‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -2452,19 +2452,19 @@ groups:
24522452
field: w_z_baro_v
24532453
min: 0
24542454
max: 10
2455-
default_value: 0.1
2455+
default_value: 0.35
24562456
- name: inav_w_z_gps_p
24572457
description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
24582458
field: w_z_gps_p
24592459
min: 0
24602460
max: 10
2461-
default_value: 0.2
2461+
default_value: 0.35
24622462
- name: inav_w_z_gps_v
24632463
description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors."
24642464
field: w_z_gps_v
24652465
min: 0
24662466
max: 10
2467-
default_value: 0.1
2467+
default_value: 0.35
24682468
- name: inav_w_xy_gps_p
24692469
description: "Weight of GPS coordinates in estimated UAV position and speed."
24702470
default_value: 1.0

‎src/main/navigation/navigation_pos_estimator.c‎

100755100644
Lines changed: 42 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -368,14 +368,6 @@ static void updateIMUEstimationWeight(const float dt)
368368
DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000);
369369
}
370370

371-
float navGetAccelerometerWeight(void)
372-
{
373-
const float accWeightScaled = posEstimator.imu.accWeightFactor;
374-
DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);
375-
376-
return accWeightScaled;
377-
}
378-
379371
static void updateIMUTopic(timeUs_t currentTimeUs)
380372
{
381373
const float dt = US2S(currentTimeUs - posEstimator.imu.lastUpdateTime);
@@ -392,25 +384,24 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
392384
/* Update acceleration weight based on vibration levels and clipping */
393385
updateIMUEstimationWeight(dt);
394386

395-
fpVector3_t accelBF;
387+
fpVector3_t accelReading;
396388

397389
/* Read acceleration data in body frame */
398-
accelBF.x = imuMeasuredAccelBF.x;
399-
accelBF.y = imuMeasuredAccelBF.y;
400-
accelBF.z = imuMeasuredAccelBF.z;
390+
accelReading.x = imuMeasuredAccelBF.x;
391+
accelReading.y = imuMeasuredAccelBF.y;
392+
accelReading.z = imuMeasuredAccelBF.z;
401393

402-
/* Correct accelerometer bias */
403-
accelBF.x -= posEstimator.imu.accelBias.x;
404-
accelBF.y -= posEstimator.imu.accelBias.y;
405-
accelBF.z -= posEstimator.imu.accelBias.z;
394+
/* Adjust reading from Body to Earth frame - from Forward-Right-Down to North-East-Up*/
395+
imuTransformVectorBodyToEarth(&accelReading);
406396

407-
/* Rotate vector to Earth frame - from Forward-Right-Down to North-East-Up*/
408-
imuTransformVectorBodyToEarth(&accelBF);
397+
/* Apply reading to NEU frame including correction for accelerometer bias */
398+
posEstimator.imu.accelNEU.x = accelReading.x + posEstimator.imu.accelBias.x;
399+
posEstimator.imu.accelNEU.y = accelReading.y + posEstimator.imu.accelBias.y;
400+
posEstimator.imu.accelNEU.z = accelReading.z + posEstimator.imu.accelBias.z;
409401

410-
/* Read acceleration data in NEU frame from IMU */
411-
posEstimator.imu.accelNEU.x = accelBF.x;
412-
posEstimator.imu.accelNEU.y = accelBF.y;
413-
posEstimator.imu.accelNEU.z = accelBF.z;
402+
DEBUG_SET(DEBUG_VIBE, 5, posEstimator.imu.accelBias.x);
403+
DEBUG_SET(DEBUG_VIBE, 6, posEstimator.imu.accelBias.y);
404+
DEBUG_SET(DEBUG_VIBE, 7, posEstimator.imu.accelBias.z);
414405

415406
/* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
416407
if (gyroConfig()->init_gyro_cal_enabled) {
@@ -611,25 +602,26 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
611602
if (isAirCushionEffectDetected && isMulticopterThrottleAboveMidHover()) {
612603
baroAltResidual = 0.0f;
613604
}
605+
614606
const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z);
615607
const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p;
608+
const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v;
616609

617610
ctx->estPosCorr.z += baroAltResidual * w_z_baro_p * ctx->dt;
618-
ctx->estVelCorr.z += baroAltResidual * sq(w_z_baro_p) * ctx->dt;
619-
ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_v * ctx->dt;
611+
ctx->estVelCorr.z += baroVelZResidual * w_z_baro_v * ctx->dt;
620612

621-
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, w_z_baro_p);
613+
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p);
622614

623615
// Accelerometer bias
624616
if (!isAirCushionEffectDetected) {
625-
ctx->accBiasCorr.z -= baroAltResidual * sq(w_z_baro_p);
617+
ctx->accBiasCorr.z += (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v));
626618
}
627619

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

631-
if (ctx->newFlags & EST_GPS_Z_VALID && wGps) {
632-
// Reset current estimate to GPS altitude if estimate not valid
623+
if (ctx->newFlags & EST_GPS_Z_VALID && (wGps || !(ctx->newFlags & EST_Z_VALID))) {
624+
// Reset current estimate to GPS altitude if estimate not valid (used for GPS and Baro)
633625
if (!(ctx->newFlags & EST_Z_VALID)) {
634626
ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
635627
ctx->estVelCorr.z += posEstimator.gps.vel.z - posEstimator.est.vel.z;
@@ -640,19 +632,24 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
640632
const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z);
641633
const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z);
642634
const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p;
635+
const float w_z_gps_v = positionEstimationConfig()->w_z_gps_v;
643636

644637
ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt;
645-
ctx->estVelCorr.z += gpsAltResidual * sq(w_z_gps_p) * ctx->dt;
646-
ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
638+
ctx->estVelCorr.z += gpsVelZResidual * w_z_gps_v * ctx->dt;
647639
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p);
648640

649641
// Accelerometer bias
650-
ctx->accBiasCorr.z -= gpsAltResidual * sq(w_z_gps_p);
642+
ctx->accBiasCorr.z += (gpsAltResidual * sq(w_z_gps_p) + gpsVelZResidual * sq(w_z_gps_v));
651643
}
652644

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

648+
// Factor corrections for sensor weightings to ensure magnitude consistency
649+
ctx->estPosCorr.z *= 2.0f / (wGps + wBaro);
650+
ctx->estVelCorr.z *= 2.0f / (wGps + wBaro);
651+
ctx->accBiasCorr.z *= 2.0f / (wGps + wBaro);
652+
656653
return correctOK;
657654
}
658655

@@ -684,17 +681,13 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
684681
ctx->estPosCorr.x += gpsPosXResidual * w_xy_gps_p * ctx->dt;
685682
ctx->estPosCorr.y += gpsPosYResidual * w_xy_gps_p * ctx->dt;
686683

687-
// Velocity from coordinates
688-
ctx->estVelCorr.x += gpsPosXResidual * sq(w_xy_gps_p) * ctx->dt;
689-
ctx->estVelCorr.y += gpsPosYResidual * sq(w_xy_gps_p) * ctx->dt;
690-
691684
// Velocity from direct measurement
692685
ctx->estVelCorr.x += gpsVelXResidual * w_xy_gps_v * ctx->dt;
693686
ctx->estVelCorr.y += gpsVelYResidual * w_xy_gps_v * ctx->dt;
694687

695688
// Accelerometer bias
696-
ctx->accBiasCorr.x -= gpsPosXResidual * sq(w_xy_gps_p);
697-
ctx->accBiasCorr.y -= gpsPosYResidual * sq(w_xy_gps_p);
689+
ctx->accBiasCorr.x += (gpsPosXResidual * sq(w_xy_gps_p) + gpsVelXResidual * sq(w_xy_gps_v));
690+
ctx->accBiasCorr.y += (gpsPosYResidual * sq(w_xy_gps_p) + gpsVelYResidual * sq(w_xy_gps_v));
698691

699692
/* Adjust EPH */
700693
ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p);
@@ -741,7 +734,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
741734
return;
742735
}
743736

744-
/* Calculate new EPH and EPV for the case we didn't update postion */
737+
/* Calculate new EPH and EPV for the case we didn't update position */
745738
ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
746739
ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
747740
ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs);
@@ -775,26 +768,24 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
775768
ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
776769
}
777770
// Boost the corrections based on accWeight
778-
const float accWeight = navGetAccelerometerWeight();
779-
vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f/accWeight);
780-
vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f/accWeight);
771+
vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f / posEstimator.imu.accWeightFactor);
772+
vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f / posEstimator.imu.accWeightFactor);
773+
781774
// Apply corrections
782775
vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr);
783776
vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);
784777

785778
/* Correct accelerometer bias */
786779
const float w_acc_bias = positionEstimationConfig()->w_acc_bias;
787780
if (w_acc_bias > 0.0f) {
788-
const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z);
789-
if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
790-
/* transform error vector from NEU frame to body frame */
791-
imuTransformVectorEarthToBody(&ctx.accBiasCorr);
792-
793-
/* Correct accel bias */
794-
posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt;
795-
posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt;
796-
posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt;
797-
}
781+
/* Correct accel bias */
782+
posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt;
783+
posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt;
784+
posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt;
785+
786+
posEstimator.imu.accelBias.x = constrainf(posEstimator.imu.accelBias.x, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE);
787+
posEstimator.imu.accelBias.y = constrainf(posEstimator.imu.accelBias.y, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE);
788+
posEstimator.imu.accelBias.z = constrainf(posEstimator.imu.accelBias.z, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE);
798789
}
799790

800791
/* Update ground course */

‎src/main/navigation/navigation_pos_estimator_agl.c‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -147,10 +147,9 @@ void estimationCalculateAGL(estimationContext_t * ctx)
147147
}
148148

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

155154
// Apply correction
156155
if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {

‎src/main/navigation/navigation_pos_estimator_private.h‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -185,5 +185,4 @@ extern navigationPosEstimator_t posEstimator;
185185
extern float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w);
186186
extern void estimationCalculateAGL(estimationContext_t * ctx);
187187
extern bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx);
188-
extern float navGetAccelerometerWeight(void);
189188

0 commit comments

Comments
 (0)