Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: FuseVelPosNED uses ext nav vel err in obs data check

Browse Source
This commit is contained in:
Randy Mackay 2020-06-01 19:12:20 +09:00
parent d37eec5fd8
commit 93d8458d2a
2 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -503,6 +503,7 @@ private:
const float gpsNEVelVarAccScale = 0.05f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
const float gpsDVelVarAccScale = 0.07f; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
const float gpsPosVarAccScale = 0.05f; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
const float extNavVelVarAccScale = 0.05f; // Scale factor applied to ext nav velocity measurement variance due to manoeuvre acceleration
const uint16_t magDelay_ms = 60; // Magnetometer measurement delay (msec)
const uint16_t tasDelay_ms = 100; // Airspeed measurement delay (msec)
const uint16_t tiltDriftTimeMax_ms = 15000; // Maximum number of ms allowed without any form of tilt aiding (GPS, flow, TAS, etc)

8
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -630,7 +630,13 @@ void NavEKF3_core::FuseVelPosNED()
// For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
// For horizontal GPS velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPS performance
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
float obs_data_chk;
if (extNavVelToFuse) {
obs_data_chk = sq(constrain_float(extNavVelDelayed.err, 0.05f, 5.0f)) + sq(frontend->extNavVelVarAccScale * accNavMag);
} else {
obs_data_chk = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
}
R_OBS_DATA_CHECKS[0] = R_OBS_DATA_CHECKS[1] = R_OBS_DATA_CHECKS[2] = obs_data_chk;
}
R_OBS[5] = posDownObsNoise;
for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];