Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

development of posestimator fusion

Browse source
This commit is contained in:
shota 2023-10-22 18:16:19 +09:00
parent 8e3ee38e51
commit 00b46bc0d6
2 changed files with 7 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/fc/settings.yaml
View file

@ -2324,13 +2324,13 @@ groups:
field: w_z_gps_p field: w_z_gps_p
min: 0 min: 0
max: 10 max: 10
default_value: 0.2 default_value: 0.5
- name: inav_w_z_gps_v - 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." description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors."
field: w_z_gps_v field: w_z_gps_v
min: 0 min: 0
max: 10 max: 10
default_value: 0.1 default_value: 0.5
- name: inav_w_xy_gps_p - name: inav_w_xy_gps_p
description: "Weight of GPS coordinates in estimated UAV position and speed." description: "Weight of GPS coordinates in estimated UAV position and speed."
default_value: 1.0 default_value: 1.0

16
src/main/navigation/navigation_pos_estimator.c
View file

@ -571,9 +571,10 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count
bool correctOK = false; bool correctOK = false;
const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); //ignore baro is deference is too big
//use both baro and gps //use both baro and gps
if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (gpsBaroResidual < positionEstimationConfig()->max_eph_epv)) {
if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro)) {
timeUs_t currentTimeUs = micros(); timeUs_t currentTimeUs = micros();
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
@ -602,14 +603,6 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt; ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
// If GPS is available - also use GPS climb rate
if (ctx->newFlags & EST_GPS_Z_VALID) {
// Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution
const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
const float gpsRocScaler = bellCurve(gpsRocResidual, 250.0f);
ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt;
}
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p); ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
// Accelerometer bias // Accelerometer bias
@ -629,10 +622,11 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
else { else {
// Altitude // Altitude
const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z; const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z;
const float gpsVelZResudual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt; 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 += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
ctx->estVelCorr.z += (posEstimator.gps.vel.z - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_gps_v * 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, gpsAltResudual), positionEstimationConfig()->w_z_gps_p);
// Accelerometer bias // Accelerometer bias