From 2998aa1a6a2e98f5837b52d63351a8a166a034eb Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 5 Jan 2014 17:28:21 +1100 Subject: [PATCH] AP_NavEKF: Amended tuning parameter list, and changed from variance to noise definitions --- libraries/AP_NavEKF/AP_NavEKF.cpp | 50 +++++++++++++++++-------------- libraries/AP_NavEKF/AP_NavEKF.h | 21 ++++++++----- 2 files changed, 41 insertions(+), 30 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index e859f0db12..1429753b67 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -50,17 +50,23 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : #endif { // Tuning parameters - _gpsHorizVelVar = 0.1f; // GPS horizontal velocity variance (m/s)^2 - _gpsVertVelVar = 0.1f; // GPS vertical velocity variance (m/s)^2 - _gpsHorizPosVar = 4.0f; // GPS horizontal position variance m^2 - _gpsVertPosVar = 1.0f; // GPS or Baro vertical position variance m^2 + _gpsHorizVelNoise = 0.3f; // GPS horizontal velocity noise : m/s + _gpsVertVelNoise = 0.3f; // GPS vertical velocity noise : m/s + _gpsHorizPosNoise = 2.0f; // GPS horizontal position noise m + _gpsVertPosNoise = 1.0f; // GPS or Baro vertical position variance : m^2 _gpsVelVarAccScale = 0.2f; // scale factor applied to velocity variance due to Vdot _gpsPosVarAccScale = 0.2f; // scale factor applied to position variance due to Vdot - _magVar = 0.0025f; // magnetometer measurement variance Gauss^2 + _magNoise = 0.05f; // magnetometer measurement noise : gauss _magVarRateScale = 0.05f; // scale factor applied to magnetometer variance due to angular rate - _easVar = 2.0f; // equivalent airspeed noise variance (m/s)^2 - _windStateNoise = 0.1f; // RMS rate of change of wind (m/s^2) + _easNoise = 1.4f; // equivalent airspeed noise : m/s + _windStateNoise = 0.1f; // RMS rate of change of wind : (m/s^2) _wndVarHgtRateScale = 0.5f; // scale factor applied to wind process noise from height rate + _gyrNoise = 1.4544411e-2f; // gyro process noise : rad/s + _accNoise = 0.5f; // accelerometer process noise : m/s^2 + _dAngBiasNoise = 5.0e-7f; // gyro bias state noise : rad/s^2 + _magEarthNoise = 3.0e-4f; // earth magnetic field process noise : gauss/sec + _magBodyNoise = 3.0e-4f; // earth magnetic field process noise : gauss/sec + // Misc initial conditions hgtRate = 0.0f; mag_state.q0 = 1; @@ -457,9 +463,9 @@ void NavEKF::CovariancePrediction() // use filtered height rate to increase wind process noise when climbing or descending // this allows for wind gradient effects. windVelSigma = dt * _windStateNoise * (1.0f + _wndVarHgtRateScale * fabsf(hgtRate)); - dAngBiasSigma = dt * 5.0e-7f; - magEarthSigma = dt * 3.0e-4f; - magBodySigma = dt * 3.0e-4f; + dAngBiasSigma = dt * _dAngBiasNoise; + magEarthSigma = dt * _magEarthNoise; + magBodySigma = dt * _magBodyNoise; for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma; @@ -483,13 +489,13 @@ void NavEKF::CovariancePrediction() dax_b = states[10]; day_b = states[11]; daz_b = states[12]; - daxCov = sq(dt*1.4544411e-2f); - dayCov = sq(dt*1.4544411e-2f); - dazCov = sq(dt*1.4544411e-2f); + daxCov = sq(dt*_gyrNoise); + dayCov = sq(dt*_gyrNoise); + dazCov = sq(dt*_gyrNoise); if (onGround) dazCov = dazCov * sq(yawVarScale); - dvxCov = sq(dt*0.5f); - dvyCov = sq(dt*0.5f); - dvzCov = sq(dt*0.5f); + dvxCov = sq(dt*_accNoise); + dvyCov = sq(dt*_accNoise); + dvzCov = sq(dt*_accNoise); // Predicted covariance calculation SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3; @@ -1097,12 +1103,12 @@ void NavEKF::FuseVelPosNED() posErr = _gpsPosVarAccScale*accNavMag; // Estimate the GPS Velocity, GPS horiz position and height measurement variances. - R_OBS[0] = _gpsHorizVelVar + sq(velErr); + R_OBS[0] = sq(_gpsHorizVelNoise) + sq(velErr); R_OBS[1] = R_OBS[0]; - R_OBS[2] = _gpsVertVelVar + sq(velErr); - R_OBS[3] = _gpsHorizPosVar + sq(posErr); + R_OBS[2] = sq(_gpsVertVelNoise) + sq(velErr); + R_OBS[3] = sq(_gpsHorizPosNoise) + sq(posErr); R_OBS[4] = R_OBS[3]; - R_OBS[5] = _gpsVertPosVar; + R_OBS[5] = sq(_gpsVertPosNoise); // Set innovation variances to zero default for (uint8_t i = 0; i<=5; i++) @@ -1347,7 +1353,7 @@ void NavEKF::FuseMagnetometer() MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; // scale magnetometer observation error with total angular rate - R_MAG = _magVar + sq(_magVarRateScale*dAngIMU.length() / dtIMU); + R_MAG = sq(_magNoise) + sq(_magVarRateScale*dAngIMU.length() / dtIMU); // Calculate observation jacobians SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; @@ -1593,7 +1599,7 @@ void NavEKF::FuseAirspeed() float vwn; float vwe; float EAS2TAS = _ahrs->get_EAS2TAS(); - const float R_TAS = _easVar*sq(constrain_float(EAS2TAS,1.0f,10.0f)); + const float R_TAS = sq(_easNoise * constrain_float(EAS2TAS, 0.9f, 10.0f)); Vector3f SH_TAS; float SK_TAS; Vector21 H_TAS; diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 9ce17686aa..9859a877d0 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -189,17 +189,22 @@ private: bool statesInitialised; // Tuning Parameters - ftype _gpsHorizVelVar; // GPS horizontal velocity variance (m/s)^2 - ftype _gpsVertVelVar; // GPS vertical velocity variance (m/s)^2 - ftype _gpsHorizPosVar; // GPS horizontal position variance m^2 - ftype _gpsVertPosVar; // GPS vertical position variance m^2 + ftype _gpsHorizVelNoise; // GPS horizontal velocity noise : m/s + ftype _gpsVertVelNoise; // GPS vertical velocity noise : m/s + ftype _gpsHorizPosNoise; // GPS horizontal position noise m + ftype _gpsVertPosNoise; // GPS or Baro vertical position variance : m^2 ftype _gpsVelVarAccScale; // scale factor applied to velocity variance due to Vdot ftype _gpsPosVarAccScale; // scale factor applied to position variance due to Vdot - ftype _magVar; // magnetometer measurement variance Gauss^2 - ftype _magVarRateScale; // scale factor applied to magnetometer variance due to Vdot - ftype _easVar; // equivalent airspeed noise variance (m/s)^2 - ftype _windStateNoise; // RMS rate of change of wind (m/s^2) + ftype _magNoise; // magnetometer measurement noise : gauss + ftype _magVarRateScale; // scale factor applied to magnetometer variance due to angular rate + ftype _easNoise; // equivalent airspeed noise : m/s + ftype _windStateNoise; // rate of change of wind : m/s^2 ftype _wndVarHgtRateScale; // scale factor applied to wind process noise from height rate + ftype _gyrNoise; // gyro process noise : rad/s + ftype _accNoise; // accelerometer process noise : m/s^2 + ftype _dAngBiasNoise; // gyro bias state noise : rad/s^2 + ftype _magEarthNoise; // earth magnetic field process noise : gauss/sec + ftype _magBodyNoise; // earth magnetic field process noise : gauss/sec Vector21 states; // state matrix - 4 x quaternions, 3 x Vel, 3 x Pos, 3 x gyro bias, 3 x accel bias, 2 x wind vel, 3 x earth mag field, 3 x body mag field