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

AP_NavEKF: Added constraints to Mavlink tuneable parameters

This commit is contained in:
Paul Riseborough 2014-01-22 17:32:28 +11:00 committed by Andrew Tridgell
parent b123e1bb16
commit 13b9daeff5
2 changed files with 40 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -28,7 +28,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: VELNE_NOISE // @Param: VELNE_NOISE
// @DisplayName: GPS horizontal velocity measurement noise (m/s) // @DisplayName: GPS horizontal velocity measurement noise (m/s)
// @Description: This is the RMS value of noise in the North and East GPS velocity measurements. Increasing it reduces the weighting on these measurements. // @Description: This is the RMS value of noise in the North and East GPS velocity measurements. Increasing it reduces the weighting on these measurements.
// @Range: 0.15 - 1.5 // @Range: 0.05 - 5.0
// @Increment: 0.05 // @Increment: 0.05
// @User: advanced // @User: advanced
AP_GROUPINFO("VELNE_NOISE", 0, NavEKF, _gpsHorizVelNoise, 0.15f), AP_GROUPINFO("VELNE_NOISE", 0, NavEKF, _gpsHorizVelNoise, 0.15f),
@ -36,7 +36,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: VELD_NOISE // @Param: VELD_NOISE
// @DisplayName: GPS vertical velocity measurement noise (m/s) // @DisplayName: GPS vertical velocity measurement noise (m/s)
// @Description: This is the RMS value of noise in the vertical GPS velocity measurement. Increasing it reduces the weighting on this measurement. // @Description: This is the RMS value of noise in the vertical GPS velocity measurement. Increasing it reduces the weighting on this measurement.
// @Range: 0.15 - 3.0 // @Range: 0.05 - 5.0
// @Increment: 0.05 // @Increment: 0.05
// @User: advanced // @User: advanced
AP_GROUPINFO("VELD_NOISE", 1, NavEKF, _gpsVertVelNoise, 0.30f), AP_GROUPINFO("VELD_NOISE", 1, NavEKF, _gpsVertVelNoise, 0.30f),
@ -44,7 +44,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: POSNE_NOISE // @Param: POSNE_NOISE
// @DisplayName: GPS horizontal position measurement noise (m) // @DisplayName: GPS horizontal position measurement noise (m)
// @Description: This is the RMS value of noise in the GPS horizontal position measurements. Increasing it reduces the weighting on these measurements. // @Description: This is the RMS value of noise in the GPS horizontal position measurements. Increasing it reduces the weighting on these measurements.
// @Range: 0.3 - 5.0 // @Range: 0.1 - 10.0
// @Increment: 0.1 // @Increment: 0.1
// @User: advanced // @User: advanced
AP_GROUPINFO("POSNE_NOISE", 2, NavEKF, _gpsHorizPosNoise, 0.50f), AP_GROUPINFO("POSNE_NOISE", 2, NavEKF, _gpsHorizPosNoise, 0.50f),
@ -52,7 +52,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: ALT_NOISE // @Param: ALT_NOISE
// @DisplayName: Altitude measurement noise (m) // @DisplayName: Altitude measurement noise (m)
// @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting on this measurement. // @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting on this measurement.
// @Range: 0.1 - 5.0 // @Range: 0.1 - 10.0
// @Increment: 0.1 // @Increment: 0.1
// @User: advanced // @User: advanced
AP_GROUPINFO("ALT_NOISE", 3, NavEKF, _baroAltNoise, 0.50f), AP_GROUPINFO("ALT_NOISE", 3, NavEKF, _baroAltNoise, 0.50f),
@ -100,7 +100,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: WIND_PNOISE // @Param: WIND_PNOISE
// @DisplayName: Wind velocity states process noise (m/s^2) // @DisplayName: Wind velocity states process noise (m/s^2)
// @Description: This noise controls the growth of wind state error estimates. Increasing it makes wind estimation faster and noisier. // @Description: This noise controls the growth of wind state error estimates. Increasing it makes wind estimation faster and noisier.
// @Range: 0.01 - 0.5 // @Range: 0.01 - 1.0
// @Increment: 0.1 // @Increment: 0.1
// @User: advanced // @User: advanced
AP_GROUPINFO("WIND_PNOISE", 9, NavEKF, _windVelProcessNoise, 0.1f), AP_GROUPINFO("WIND_PNOISE", 9, NavEKF, _windVelProcessNoise, 0.1f),
@ -184,7 +184,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: MAG_DELAY // @Param: MAG_DELAY
// @DisplayName: Magnetometer measurement delay (msec) // @DisplayName: Magnetometer measurement delay (msec)
// @Description: This is the number of msec that the magnetometer measurements lag behind the inertial measurements. // @Description: This is the number of msec that the magnetometer measurements lag behind the inertial measurements.
// @Range: 0 - 100 // @Range: 0 - 500
// @Increment: 10 // @Increment: 10
// @User: advanced // @User: advanced
AP_GROUPINFO("MAG_DELAY", 20, NavEKF, _msecMagDelay, 40), AP_GROUPINFO("MAG_DELAY", 20, NavEKF, _msecMagDelay, 40),
@ -208,7 +208,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: VEL_GATE // @Param: VEL_GATE
// @DisplayName: GPS velocity measurement gate size // @DisplayName: GPS velocity measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the GPS velocity measurement innovation consistency check. Decreasing it makes it more likely that good measurements willbe rejected. Increasing it makes it more likely that bad measurements will be accepted. // @Description: This parameter sets the number of standard deviations applied to the GPS velocity measurement innovation consistency check. Decreasing it makes it more likely that good measurements willbe rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 3 - 100 // @Range: 1 - 100
// @Increment: 1 // @Increment: 1
// @User: advanced // @User: advanced
AP_GROUPINFO("VEL_GATE", 23, NavEKF, _gpsVelInnovGate, 5), AP_GROUPINFO("VEL_GATE", 23, NavEKF, _gpsVelInnovGate, 5),
@ -216,7 +216,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: POS_GATE // @Param: POS_GATE
// @DisplayName: GPS position measurement gate size // @DisplayName: GPS position measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the GPS position measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. // @Description: This parameter sets the number of standard deviations applied to the GPS position measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 3 - 100 // @Range: 1 - 100
// @Increment: 1 // @Increment: 1
// @User: advanced // @User: advanced
AP_GROUPINFO("POS_GATE", 24, NavEKF, _gpsPosInnovGate, 10), AP_GROUPINFO("POS_GATE", 24, NavEKF, _gpsPosInnovGate, 10),
@ -224,7 +224,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: HGT_GATE // @Param: HGT_GATE
// @DisplayName: Height measurement gate size // @DisplayName: Height measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the height measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. // @Description: This parameter sets the number of standard deviations applied to the height measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 3 - 100 // @Range: 1 - 100
// @Increment: 1 // @Increment: 1
// @User: advanced // @User: advanced
AP_GROUPINFO("HGT_GATE", 25, NavEKF, _hgtInnovGate, 10), AP_GROUPINFO("HGT_GATE", 25, NavEKF, _hgtInnovGate, 10),
@ -232,7 +232,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: MAG_GATE // @Param: MAG_GATE
// @DisplayName: Magnetometer measurement gate size // @DisplayName: Magnetometer measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the magnetometer measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. // @Description: This parameter sets the number of standard deviations applied to the magnetometer measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 3 - 100 // @Range: 1 - 100
// @Increment: 1 // @Increment: 1
// @User: advanced // @User: advanced
AP_GROUPINFO("MAG_GATE", 26, NavEKF, _magInnovGate, 5), AP_GROUPINFO("MAG_GATE", 26, NavEKF, _magInnovGate, 5),
@ -240,7 +240,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: EAS_GATE // @Param: EAS_GATE
// @DisplayName: Airspeed measurement gate size // @DisplayName: Airspeed measurement gate size
// @Description: This parameter sets the number of standard deviations applied to the airspeed measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. // @Description: This parameter sets the number of standard deviations applied to the airspeed measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 3 - 100 // @Range: 1 - 100
// @Increment: 1 // @Increment: 1
// @User: advanced // @User: advanced
AP_GROUPINFO("EAS_GATE", 27, NavEKF, _tasInnovGate, 10), AP_GROUPINFO("EAS_GATE", 27, NavEKF, _tasInnovGate, 10),
@ -837,11 +837,11 @@ void NavEKF::CovariancePrediction()
// use filtered height rate to increase wind process noise when climbing or descending // use filtered height rate to increase wind process noise when climbing or descending
// this allows for wind gradient effects. // this allows for wind gradient effects.
windVelSigma = dt * _windVelProcessNoise * (1.0f + _wndVarHgtRateScale * fabsf(hgtRate)); windVelSigma = dt * constrain_float(_windVelProcessNoise, 0.01f, 1.0f) * (1.0f + constrain_float(_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate));
dAngBiasSigma = dt * _gyroBiasProcessNoise; dAngBiasSigma = dt * constrain_float(_gyroBiasProcessNoise, 1e-7f, 1e-5f);
dVelBiasSigma = dt * _accelBiasProcessNoise; dVelBiasSigma = dt * constrain_float(_accelBiasProcessNoise, 1e-4f, 1e-2f);
magEarthSigma = dt * _magEarthProcessNoise; magEarthSigma = dt * constrain_float(_magEarthProcessNoise, 1e-4f, 1e-2f);
magBodySigma = dt * _magBodyProcessNoise; magBodySigma = dt * constrain_float(_magBodyProcessNoise, 1e-4f, 1e-2f);
for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f;
for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma; for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma;
// scale gyro bias noise when on ground to allow for faster bias estimation // scale gyro bias noise when on ground to allow for faster bias estimation
@ -872,9 +872,11 @@ void NavEKF::CovariancePrediction()
day_b = states[11]; day_b = states[11];
daz_b = states[12]; daz_b = states[12];
dvz_b = states[13]; dvz_b = states[13];
_gyrNoise = constrain_float(_gyrNoise, 1e-3f, 5e-2f);
daxCov = sq(dt*_gyrNoise); daxCov = sq(dt*_gyrNoise);
dayCov = sq(dt*_gyrNoise); dayCov = sq(dt*_gyrNoise);
dazCov = sq(dt*_gyrNoise); dazCov = sq(dt*_gyrNoise);
_accNoise = constrain_float(_accNoise, 5e-2f, 1.0f);
dvxCov = sq(dt*_accNoise); dvxCov = sq(dt*_accNoise);
dvyCov = sq(dt*_accNoise); dvyCov = sq(dt*_accNoise);
dvzCov = sq(dt*_accNoise); dvzCov = sq(dt*_accNoise);
@ -1515,8 +1517,8 @@ void NavEKF::FuseVelPosNED()
// set the GPS data timeout depending on whether airspeed data is present // set the GPS data timeout depending on whether airspeed data is present
uint32_t gpsRetryTime; uint32_t gpsRetryTime;
if (useAirspeed) gpsRetryTime = _gpsRetryTimeUseTAS; if (useAirspeed) gpsRetryTime = constrain_int16(_gpsRetryTimeUseTAS, 1000, 30000);
else gpsRetryTime = _gpsRetryTimeNoTAS; else gpsRetryTime = constrain_int16(_gpsRetryTimeNoTAS, 1000, 30000);
// Form the observation vector // Form the observation vector
for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i];
@ -1529,19 +1531,19 @@ void NavEKF::FuseVelPosNED()
} }
// additional error in GPS velocity caused by manoeuvring // additional error in GPS velocity caused by manoeuvring
NEvelErr = _gpsNEVelVarAccScale * accNavMag; NEvelErr = constrain_float(_gpsNEVelVarAccScale, 0.05f, 0.5f) * accNavMag;
DvelErr = _gpsDVelVarAccScale * accNavMag; DvelErr = constrain_float(_gpsDVelVarAccScale, 0.05f, 0.5f) * accNavMag;
// additional error in GPS position caused by manoeuvring // additional error in GPS position caused by manoeuvring
posErr = _gpsPosVarAccScale*accNavMag; posErr = constrain_float(_gpsPosVarAccScale, 0.05f, 0.5f) * accNavMag;
// Estimate the GPS Velocity, GPS horiz position and height measurement variances. // Estimate the GPS Velocity, GPS horiz position and height measurement variances.
R_OBS[0] = gpsVarScaler*(sq(_gpsHorizVelNoise) + sq(NEvelErr)); R_OBS[0] = gpsVarScaler*(sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(NEvelErr));
R_OBS[1] = R_OBS[0]; R_OBS[1] = R_OBS[0];
R_OBS[2] = gpsVarScaler*(sq(_gpsVertVelNoise) + sq(DvelErr)); R_OBS[2] = gpsVarScaler*(sq(constrain_float(_gpsVertVelNoise, 0.05f, 5.0f)) + sq(DvelErr));
R_OBS[3] = gpsVarScaler*(sq(_gpsHorizPosNoise) + sq(posErr)); R_OBS[3] = gpsVarScaler*(sq(constrain_float(_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr));
R_OBS[4] = R_OBS[3]; R_OBS[4] = R_OBS[3];
R_OBS[5] = hgtVarScaler*sq(_baroAltNoise); R_OBS[5] = hgtVarScaler*sq(constrain_float(_baroAltNoise, 0.1f, 10.0f));
// calculate innovations and check GPS data validity using an innovation consistency check // calculate innovations and check GPS data validity using an innovation consistency check
if (fuseVelData) if (fuseVelData)
@ -1602,8 +1604,8 @@ void NavEKF::FuseVelPosNED()
{ {
// set the height data timeout depending on whether vertical velocity data is being used // set the height data timeout depending on whether vertical velocity data is being used
uint32_t hgtRetryTime; uint32_t hgtRetryTime;
if (_fusionModeGPS == 0) hgtRetryTime = _hgtRetryTimeMode0; if (_fusionModeGPS == 0) hgtRetryTime = constrain_int16(_hgtRetryTimeMode0, 1000, 30000);
else hgtRetryTime = _hgtRetryTimeMode12; else hgtRetryTime = constrain_int16(_hgtRetryTimeMode12, 1000, 30000);
// calculate height innovations // calculate height innovations
hgtInnov = statesAtHgtTime[9] - observation[5]; hgtInnov = statesAtHgtTime[9] - observation[5];
varInnovVelPos[5] = P[9][9] + R_OBS[5]; varInnovVelPos[5] = P[9][9] + R_OBS[5];
@ -1795,7 +1797,7 @@ void NavEKF::FuseMagnetometer()
MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias;
// scale magnetometer observation error with total angular rate // scale magnetometer observation error with total angular rate
R_MAG = sq(_magNoise) + sq(_magVarRateScale*dAngIMU.length() / dtIMU); R_MAG = sq(constrain_float(_magNoise, 0.05f, 0.5f)) + sq(_magVarRateScale*dAngIMU.length() / dtIMU);
// Calculate observation jacobians // Calculate observation jacobians
SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
@ -2053,7 +2055,7 @@ void NavEKF::FuseAirspeed()
float vwn; float vwn;
float vwe; float vwe;
float EAS2TAS = _ahrs->get_EAS2TAS(); float EAS2TAS = _ahrs->get_EAS2TAS();
const float R_TAS = sq(_easNoise * constrain_float(EAS2TAS, 0.9f, 10.0f)); const float R_TAS = sq(constrain_float(_easNoise, 0.5f, 5.0f) * constrain_float(EAS2TAS, 0.9f, 10.0f));
Vector3f SH_TAS; Vector3f SH_TAS;
float SK_TAS; float SK_TAS;
Vector22 H_TAS; Vector22 H_TAS;
@ -2435,8 +2437,8 @@ void NavEKF::readGpsData()
{ {
lastFixTime_ms = _ahrs->get_gps()->last_message_time_ms(); lastFixTime_ms = _ahrs->get_gps()->last_message_time_ms();
newDataGps = true; newDataGps = true;
RecallStates(statesAtVelTime, (IMUmsec - _msecVelDelay)); RecallStates(statesAtVelTime, (IMUmsec - constrain_int16(_msecVelDelay, 0, 500)));
RecallStates(statesAtPosTime, (IMUmsec - _msecPosDelay)); RecallStates(statesAtPosTime, (IMUmsec - constrain_int16(_msecPosDelay, 0, 500)));
velNED[0] = _ahrs->get_gps()->velocity_north(); // (rad) velNED[0] = _ahrs->get_gps()->velocity_north(); // (rad)
velNED[1] = _ahrs->get_gps()->velocity_east(); // (m/s) velNED[1] = _ahrs->get_gps()->velocity_east(); // (m/s)
velNED[2] = _ahrs->get_gps()->velocity_down(); // (m/s) velNED[2] = _ahrs->get_gps()->velocity_down(); // (m/s)
@ -2459,7 +2461,7 @@ void NavEKF::readHgtData()
hgtMea = _baro.get_altitude(); hgtMea = _baro.get_altitude();
newDataHgt = true; newDataHgt = true;
// recall states from compass measurement time // recall states from compass measurement time
RecallStates(statesAtHgtTime, (IMUmsec - _msecHgtDelay)); RecallStates(statesAtHgtTime, (IMUmsec - constrain_int16(_msecHgtDelay, 0, 500)));
} else { } else {
newDataHgt = false; newDataHgt = false;
} }
@ -2475,7 +2477,7 @@ void NavEKF::readMagData()
magData = _ahrs->get_compass()->get_field() * 0.001f + magBias; magData = _ahrs->get_compass()->get_field() * 0.001f + magBias;
// Recall states from compass measurement time // Recall states from compass measurement time
RecallStates(statesAtMagMeasTime, (IMUmsec - _msecMagDelay)); RecallStates(statesAtMagMeasTime, (IMUmsec - constrain_int16(_msecMagDelay, 0, 500)));
newDataMag = true; newDataMag = true;
} else { } else {
newDataMag = false; newDataMag = false;
@ -2491,7 +2493,7 @@ void NavEKF::readAirSpdData()
VtasMeas = aspeed->get_airspeed() * aspeed->get_EAS2TAS(); VtasMeas = aspeed->get_airspeed() * aspeed->get_EAS2TAS();
lastAirspeedUpdate = aspeed->last_update_ms(); lastAirspeedUpdate = aspeed->last_update_ms();
newDataTas = true; newDataTas = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - _msecTasDelay)); RecallStates(statesAtVtasMeasTime, (IMUmsec - constrain_int16(_msecTasDelay, 0, 500)));
} else { } else {
newDataTas = false; newDataTas = false;
} }

8
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -253,10 +253,10 @@ private:
AP_Int8 _hgtInnovGate; // Number of standard deviations applied to height innovation consistency check AP_Int8 _hgtInnovGate; // Number of standard deviations applied to height innovation consistency check
AP_Int8 _magInnovGate; // Number of standard deviations applied to magnetometer innovation consistency check AP_Int8 _magInnovGate; // Number of standard deviations applied to magnetometer innovation consistency check
AP_Int8 _tasInnovGate; // Number of standard deviations applied to true airspeed innovation consistency check AP_Int8 _tasInnovGate; // Number of standard deviations applied to true airspeed innovation consistency check
AP_Int32 _gpsRetryTimeUseTAS; // GPS retry time following innovation consistency fail if TAS measurements are used (msec) AP_Int16 _gpsRetryTimeUseTAS; // GPS retry time following innovation consistency fail if TAS measurements are used (msec)
AP_Int32 _gpsRetryTimeNoTAS; // GPS retry time following innovation consistency fail if no TAS measurements are used (msec) AP_Int16 _gpsRetryTimeNoTAS; // GPS retry time following innovation consistency fail if no TAS measurements are used (msec)
AP_Int32 _hgtRetryTimeMode0; // height measurement retry time following innovation consistency fail if GPS fusion mode is = 0 (msec) AP_Int16 _hgtRetryTimeMode0; // height measurement retry time following innovation consistency fail if GPS fusion mode is = 0 (msec)
AP_Int32 _hgtRetryTimeMode12; // height measurement retry time following innovation consistency fail if GPS fusion mode is > 0 (msec) AP_Int16 _hgtRetryTimeMode12; // height measurement retry time following innovation consistency fail if GPS fusion mode is > 0 (msec)
// Tuning parameters // Tuning parameters
float _gyroBiasNoiseScaler; // scale factor applied to gyro bias state process variance when on ground float _gyroBiasNoiseScaler; // scale factor applied to gyro bias state process variance when on ground