Skip to content

Commit

Permalink
Merge pull request #10367 from RomanLut/submit-fix-z-pos-estimator
Browse files Browse the repository at this point in the history
fixed altitude estimator error estimation
  • Loading branch information
sensei-hacker authored Sep 18, 2024
2 parents 7603127 + c1565a7 commit c5febe2
Showing 1 changed file with 1 addition and 1 deletion.
2 changes: 1 addition & 1 deletion src/main/navigation/navigation_pos_estimator.c
Original file line number Diff line number Diff line change
Expand Up @@ -617,7 +617,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
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, gpsAltResudual), positionEstimationConfig()->w_z_gps_p);
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResudual)), positionEstimationConfig()->w_z_gps_p);

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

0 comments on commit c5febe2

Please sign in to comment.