Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Remove unused parameters and improve naming consistency

This commit is contained in:
Paul Riseborough 2015-09-23 22:19:48 +10:00 committed by Andrew Tridgell
parent 8afb26087d
commit 4a7714e15d
3 changed files with 43 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -215,7 +215,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
// @Increment: 10
// @User: Advanced
// @Units: milliseconds
AP_GROUPINFO("VEL_DELAY", 15, NavEKF2, _msecGpsDelay, 220),
AP_GROUPINFO("VEL_DELAY", 15, NavEKF2, _gpsDelay_ms, 220),
// @Param: GPS_TYPE
// @DisplayName: GPS mode control
@ -312,7 +312,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
// @Increment: 10
// @User: Advanced
// @Units: milliseconds
AP_GROUPINFO("FLOW_DELAY", 27, NavEKF2, _msecFlowDelay, FLOW_MEAS_DELAY),
AP_GROUPINFO("FLOW_DELAY", 27, NavEKF2, _flowDelay_ms, FLOW_MEAS_DELAY),
// @Param: RNG_GATE
// @DisplayName: Range finder measurement gate size
@ -355,29 +355,23 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
gpsNEVelVarAccScale(0.05f), // Scale factor applied to horizontal velocity measurement variance due to manoeuvre acceleration - used when GPS doesn't report speed error
gpsDVelVarAccScale(0.07f), // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration - used when GPS doesn't report speed error
gpsPosVarAccScale(0.05f), // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
msecHgtDelay(60), // Height measurement delay (msec)
msecMagDelay(60), // Magnetometer measurement delay (msec)
msecTasDelay(240), // Airspeed measurement delay (msec)
gpsRetryTimeUseTAS(10000), // GPS retry time with airspeed measurements (msec)
gpsRetryTimeNoTAS(7000), // GPS retry time without airspeed measurements (msec)
gpsFailTimeWithFlow(1000), // If we have no GPS for longer than this and we have optical flow, then we will switch across to using optical flow (msec)
hgtRetryTimeMode0(10000), // Height retry time with vertical velocity measurement (msec)
hgtRetryTimeMode12(5000), // Height retry time without vertical velocity measurement (msec)
tasRetryTime(5000), // True airspeed timeout and retry interval (msec)
hgtDelay_ms(60), // Height measurement delay (msec)
magDelay_ms(60), // Magnetometer measurement delay (msec)
tasDelay_ms(240), // Airspeed measurement delay (msec)
gpsRetryTimeUseTAS_ms(10000), // GPS retry time with airspeed measurements (msec)
gpsRetryTimeNoTAS_ms(7000), // GPS retry time without airspeed measurements (msec)
gpsFailTimeWithFlow_ms(1000), // If we have no GPS for longer than this and we have optical flow, then we will switch across to using optical flow (msec)
hgtRetryTimeMode0_ms(10000), // Height retry time with vertical velocity measurement (msec)
hgtRetryTimeMode12_ms(5000), // Height retry time without vertical velocity measurement (msec)
tasRetryTime_ms(5000), // True airspeed timeout and retry interval (msec)
magFailTimeLimit_ms(10000), // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
magVarRateScale(0.05f), // scale factor applied to magnetometer variance due to angular rate
gyroBiasNoiseScaler(2.0f), // scale factor applied to imu gyro bias learning before the vehicle is armed
accelBiasNoiseScaler(1.0f), // scale factor applied to imu accel bias learning before the vehicle is armed
msecGpsAvg(200), // average number of msec between GPS measurements
msecHgtAvg(100), // average number of msec between height measurements
msecMagAvg(100), // average number of msec between magnetometer measurements
msecBetaAvg(100), // average number of msec between synthetic sideslip measurements
msecBetaMax(200), // maximum number of msec between synthetic sideslip measurements
msecFlowAvg(100), // average number of msec between optical flow measurements
dtVelPos(0.2f), // number of seconds between position and velocity corrections. This should be a multiple of the imu update interval.
hgtAvg_ms(100), // average number of msec between height measurements
betaAvg_ms(100), // average number of msec between synthetic sideslip measurements
covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates
covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates
TASmsecMax(200), // maximum allowed interval between airspeed measurement updates
DCM33FlowMin(0.71f), // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
fScaleFactorPnoise(1e-10f), // Process noise added to focal length scale factor state variance at each time step
flowTimeDeltaAvg_ms(100), // average interval between optical flow measurements (msec)

32
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -245,7 +245,7 @@ private:
AP_Float _accNoise; // accelerometer process noise : m/s^2
AP_Float _gyroBiasProcessNoise; // gyro bias state process noise : rad/s
AP_Float _accelBiasProcessNoise;// accel bias state process noise : m/s^2
AP_Int16 _msecGpsDelay; // effective average delay of GPS measurements relative to time of receipt (msec)
AP_Int16 _gpsDelay_ms; // effective average delay of GPS measurements relative to time of receipt (msec)
AP_Int8 _fusionModeGPS; // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity
AP_Int8 _gpsVelInnovGate; // Number of standard deviations applied to GPS velocity innovation consistency check
AP_Int8 _gpsPosInnovGate; // Number of standard deviations applied to GPS position innovation consistency check
@ -257,7 +257,7 @@ private:
AP_Int8 _gndGradientSigma; // RMS terrain gradient percentage assumed by the terrain height estimation.
AP_Float _flowNoise; // optical flow rate measurement noise
AP_Int8 _flowInnovGate; // Number of standard deviations applied to optical flow innovation consistency check
AP_Int8 _msecFlowDelay; // effective average delay of optical flow measurements rel to IMU (msec)
AP_Int8 _flowDelay_ms; // effective average delay of optical flow measurements rel to IMU (msec)
AP_Int8 _rngInnovGate; // Number of standard deviations applied to range finder innovation consistency check
AP_Float _maxFlowRate; // Maximum flow rate magnitude that will be accepted by the filter
AP_Int8 _fallback; // EKF-to-DCM fallback strictness. 0 = trust EKF more, 1 = fallback more conservatively.
@ -268,29 +268,23 @@ private:
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
const float gpsDVelVarAccScale; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
const float gpsPosVarAccScale; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
const uint16_t msecHgtDelay; // Height measurement delay (msec)
const uint16_t msecMagDelay; // Magnetometer measurement delay (msec)
const uint16_t msecTasDelay; // Airspeed measurement delay (msec)
const uint16_t gpsRetryTimeUseTAS; // GPS retry time with airspeed measurements (msec)
const uint16_t gpsRetryTimeNoTAS; // GPS retry time without airspeed measurements (msec)
const uint16_t gpsFailTimeWithFlow; // If we have no GPs for longer than this and we have optical flow, then we will switch across to using optical flow (msec)
const uint16_t hgtRetryTimeMode0; // Height retry time with vertical velocity measurement (msec)
const uint16_t hgtRetryTimeMode12; // Height retry time without vertical velocity measurement (msec)
const uint16_t tasRetryTime; // True airspeed timeout and retry interval (msec)
const uint16_t hgtDelay_ms; // Height measurement delay (msec)
const uint16_t magDelay_ms; // Magnetometer measurement delay (msec)
const uint16_t tasDelay_ms; // Airspeed measurement delay (msec)
const uint16_t gpsRetryTimeUseTAS_ms; // GPS retry time with airspeed measurements (msec)
const uint16_t gpsRetryTimeNoTAS_ms; // GPS retry time without airspeed measurements (msec)
const uint16_t gpsFailTimeWithFlow_ms; // If we have no GPs for longer than this and we have optical flow, then we will switch across to using optical flow (msec)
const uint16_t hgtRetryTimeMode0_ms; // Height retry time with vertical velocity measurement (msec)
const uint16_t hgtRetryTimeMode12_ms; // Height retry time without vertical velocity measurement (msec)
const uint16_t tasRetryTime_ms; // True airspeed timeout and retry interval (msec)
const uint32_t magFailTimeLimit_ms; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
const float magVarRateScale; // scale factor applied to magnetometer variance due to angular rate
const float gyroBiasNoiseScaler; // scale factor applied to gyro bias state process noise when on ground
const float accelBiasNoiseScaler; // scale factor applied to accel bias state process noise when on ground
const uint16_t msecGpsAvg; // average number of msec between GPS measurements
const uint16_t msecHgtAvg; // average number of msec between height measurements
const uint16_t msecMagAvg; // average number of msec between magnetometer measurements
const uint16_t msecBetaAvg; // average number of msec between synthetic sideslip measurements
const uint16_t msecBetaMax; // maximum number of msec between synthetic sideslip measurements
const uint16_t msecFlowAvg; // average number of msec between optical flow measurements
const float dtVelPos; // number of seconds between position and velocity corrections. This should be a multiple of the imu update interval.
const uint16_t hgtAvg_ms; // average number of msec between height measurements
const uint16_t betaAvg_ms; // average number of msec between synthetic sideslip measurements
const float covTimeStepMax; // maximum time (sec) between covariance prediction updates
const float covDelAngMax; // maximum delta angle between covariance prediction updates
const uint32_t TASmsecMax; // maximum allowed interval between airspeed measurement updates
const float DCM33FlowMin; // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
const float fScaleFactorPnoise; // Process noise added to focal length scale factor state variance at each time step
const uint8_t flowTimeDeltaAvg_ms; // average interval between optical flow measurements (msec)

34
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -331,7 +331,7 @@ void NavEKF2_core::SelectVelPosFusion()
// If we haven't received height data for a while, then declare the height data as being timed out
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
hgtRetryTime = (useGpsVertVel && !velTimeout) ? frontend.hgtRetryTimeMode0 : frontend.hgtRetryTimeMode12;
hgtRetryTime = (useGpsVertVel && !velTimeout) ? frontend.hgtRetryTimeMode0_ms : frontend.hgtRetryTimeMode12_ms;
if (imuSampleTime_ms - lastHgtReceived_ms > hgtRetryTime) {
hgtTimeout = true;
}
@ -355,9 +355,9 @@ void NavEKF2_core::SelectVelPosFusion()
// of GPS and severe loss of position accuracy.
uint32_t gpsRetryTime;
if (useAirspeed()) {
gpsRetryTime = frontend.gpsRetryTimeUseTAS;
gpsRetryTime = frontend.gpsRetryTimeUseTAS_ms;
} else {
gpsRetryTime = frontend.gpsRetryTimeNoTAS;
gpsRetryTime = frontend.gpsRetryTimeNoTAS_ms;
}
if ((posTestRatio > 2.0f) && ((imuSampleTime_ms - lastPosFailTime) < gpsRetryTime) && ((imuSampleTime_ms - lastPosFailTime) > gpsRetryTime/2) && fusePosData) {
lastGpsAidBadTime_ms = imuSampleTime_ms;
@ -413,7 +413,7 @@ void NavEKF2_core::SelectTasFusion()
readAirSpdData();
// If we haven't received airspeed data for a while, then declare the airspeed data as being timed out
if (imuSampleTime_ms - tasDataNew.time_ms > frontend.tasRetryTime) {
if (imuSampleTime_ms - tasDataNew.time_ms > frontend.tasRetryTime_ms) {
tasTimeout = true;
}
@ -436,9 +436,9 @@ void NavEKF2_core::SelectTasFusion()
void NavEKF2_core::SelectBetaFusion()
{
// set true when the fusion time interval has triggered
bool f_timeTrigger = ((imuSampleTime_ms - BETAmsecPrev) >= frontend.msecBetaAvg);
bool f_timeTrigger = ((imuSampleTime_ms - BETAmsecPrev) >= frontend.betaAvg_ms);
// set true when use of synthetic sideslip fusion is necessary because we have limited sensor data or are dead reckoning position
bool f_required = !(use_compass() && useAirspeed() && ((imuSampleTime_ms - lastPosPassTime) < frontend.gpsRetryTimeNoTAS));
bool f_required = !(use_compass() && useAirspeed() && ((imuSampleTime_ms - lastPosPassTime) < frontend.gpsRetryTimeNoTAS_ms));
// set true when sideslip fusion is feasible (requires zero sideslip assumption to be valid and use of wind states)
bool f_feasible = (assume_zero_sideslip() && !inhibitWindStates);
// use synthetic sideslip fusion if feasible, required and enough time has lapsed since the last fusion
@ -1293,8 +1293,8 @@ void NavEKF2_core::FuseVelPosNED()
// set the GPS data timeout depending on whether airspeed data is present
uint32_t gpsRetryTime;
if (useAirspeed()) gpsRetryTime = frontend.gpsRetryTimeUseTAS;
else gpsRetryTime = frontend.gpsRetryTimeNoTAS;
if (useAirspeed()) gpsRetryTime = frontend.gpsRetryTimeUseTAS_ms;
else gpsRetryTime = frontend.gpsRetryTimeNoTAS_ms;
// form the observation vector and zero velocity and horizontal position observations if in constant position mode
// If in constant velocity mode, hold the last known horizontal velocity vector
@ -1348,7 +1348,7 @@ void NavEKF2_core::FuseVelPosNED()
// if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
if (useGpsVertVel && fuseVelData && (imuSampleTime_ms - lastHgtReceived_ms) < (2 * frontend.msecHgtAvg)) {
if (useGpsVertVel && fuseVelData && (imuSampleTime_ms - lastHgtReceived_ms) < (2 * frontend.hgtAvg_ms)) {
// calculate innovations for height and vertical GPS vel measurements
float hgtErr = stateStruct.position.z - observation[5];
float velDErr = stateStruct.velocity.z - observation[2];
@ -2065,7 +2065,7 @@ void NavEKF2_core::FuseAirspeed()
// fail if the ratio is > 1, but don't fail if bad IMU data
tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);
tasTimeout = (imuSampleTime_ms - lastTasPassTime) > frontend.tasRetryTime;
tasTimeout = (imuSampleTime_ms - lastTasPassTime) > frontend.tasRetryTime_ms;
// test the ratio before fusing data, forcing fusion if airspeed and position are timed out as we have no choice but to try and use airspeed to constrain error growth
if (tasHealth || (tasTimeout && posTimeout)) {
@ -3737,7 +3737,7 @@ void NavEKF2_core::readGpsData()
// estimate when the GPS fix was valid, allowing for GPS processing and other delays
// ideally we should be using a timing signal from the GPS receiver to set this time
gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend._msecGpsDelay;
gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend._gpsDelay_ms;
// read the NED velocity from the GPS
gpsDataNew.vel = _ahrs->get_gps().velocity();
@ -3812,7 +3812,7 @@ void NavEKF2_core::readGpsData()
gpsNotAvailable = true;
if (imuSampleTime_ms - gpsDataNew.time_ms > 200) {
gpsDataNew.pos.zero();
gpsDataNew.time_ms = imuSampleTime_ms-frontend._msecGpsDelay;
gpsDataNew.time_ms = imuSampleTime_ms-frontend._gpsDelay_ms;
// save measurement to buffer to be fused later
StoreGPS();
}
@ -3854,7 +3854,7 @@ void NavEKF2_core::readHgtData()
// it is is reset to last height measurement on disarming in performArmingChecks()
if (!getTakeoffExpected()) {
const float gndHgtFiltTC = 0.5f;
const float dtBaro = frontend.msecHgtAvg*1.0e-3f;
const float dtBaro = frontend.hgtAvg_ms*1.0e-3f;
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
} else if (filterArmed && getTakeoffExpected()) {
@ -3867,7 +3867,7 @@ void NavEKF2_core::readHgtData()
lastHgtReceived_ms = _baro.get_last_update();
// estimate of time height measurement was taken, allowing for delays
hgtMeasTime_ms = lastHgtReceived_ms - frontend.msecHgtDelay;
hgtMeasTime_ms = lastHgtReceived_ms - frontend.hgtDelay_ms;
// save baro measurement to buffer to be fused later
baroDataNew.time_ms = hgtMeasTime_ms;
@ -3883,7 +3883,7 @@ void NavEKF2_core::readMagData()
lastMagUpdate = _ahrs->get_compass()->last_update_usec();
// estimate of time magnetometer measurement was taken, allowing for delays
magMeasTime_ms = imuSampleTime_ms - frontend.msecMagDelay;
magMeasTime_ms = imuSampleTime_ms - frontend.magDelay_ms;
// read compass data and scale to improve numerical conditioning
magDataNew.mag = _ahrs->get_compass()->get_field() * 0.001f;
@ -3919,7 +3919,7 @@ void NavEKF2_core::readAirSpdData()
aspeed->last_update_ms() != timeTasReceived_ms) {
tasDataNew.tas = aspeed->get_airspeed() * aspeed->get_EAS2TAS();
timeTasReceived_ms = aspeed->last_update_ms();
tasDataNew.time_ms = timeTasReceived_ms - frontend.msecTasDelay;
tasDataNew.time_ms = timeTasReceived_ms - frontend.tasDelay_ms;
newDataTas = true;
StoreTAS();
RecallTAS();
@ -3971,7 +3971,7 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
// record time last observation was received so we can detect loss of data elsewhere
flowValidMeaTime_ms = imuSampleTime_ms;
// estimate sample time of the measurement
ofDataNew.time_ms = imuSampleTime_ms - frontend._msecFlowDelay - frontend.flowTimeDeltaAvg_ms/2;
ofDataNew.time_ms = imuSampleTime_ms - frontend._flowDelay_ms - frontend.flowTimeDeltaAvg_ms/2;
// Save data to buffer
StoreOF();
// Check for data at the fusion time horizon