diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index e48b7538dc..3a21d211b7 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -362,21 +362,39 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { AP_GROUPEND }; - // constructor NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : _ahrs(ahrs), _baro(baro), state(*reinterpret_cast(&states)), - onGround(true), // initialise assuming we are on ground - manoeuvring(false), // initialise assuming we are not maneouvring - 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 - constPosMode(true), // forces position fusion with zero values - yawAligned(false), // set true when heading or yaw angle has been aligned - inhibitWindStates(true), // inhibit wind state updates on startup - inhibitMagStates(true) // inhibit magnetometer state updates on startup + gpsNEVelVarAccScale(0.05f), // Scale factor applied to horizontal velocity measurement variance due to manoeuvre acceleration + gpsDVelVarAccScale(0.07f), // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration + gpsPosVarAccScale(0.05f), // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration + msecHgtDelay(60), // Height measurement delay (msec) + msecMagDelay(40), // Magnetometer measurement delay (msec) + msecTasDelay(240), // Airspeed measurement delay (msec) + gpsRetryTimeUseTAS(10000), // GPS retry time with airspeed measurements (msec) + gpsRetryTimeNoTAS(10000), // GPS retry time without airspeed measurements (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) + 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 gyro bias state process noise when on ground + 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. + 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) + flowIntervalMax_ms(200) // maximum allowable time between flow fusion events #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN ,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")), @@ -388,40 +406,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : #endif { AP_Param::setup_object_defaults(this, var_info); - // Tuning parameters - _gpsNEVelVarAccScale = 0.05f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration - _gpsDVelVarAccScale = 0.07f; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration - _gpsPosVarAccScale = 0.05f; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration - _msecHgtDelay = 60; // Height measurement delay (msec) - _msecMagDelay = 40; // Magnetometer measurement delay (msec) - _msecTasDelay = 240; // Airspeed measurement delay (msec) - _gpsRetryTimeUseTAS = 10000; // GPS retry time with airspeed measurements (msec) - _gpsRetryTimeNoTAS = 10000; // GPS retry time without airspeed measurements (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) - _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 gyro bias state process noise when on ground - _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.2; // number of seconds between position and velocity corrections. This should be a multiple of the imu update interval. - - // Misc initial conditions - hgtRate = 0.0f; - mag_state.q0 = 1; - mag_state.DCM.identity(); - IMU1_weighting = 0.5f; - memset(&faultStatus, 0, sizeof(faultStatus)); - // variables added for optical flow fusion - 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) - flowIntervalMax_ms = 200; // maximum allowable time between flow fusion events + } // Check basic filter health metrics and return a consolidated health status @@ -453,7 +438,7 @@ bool NavEKF::healthy(void) const // resets position states to last GPS measurement or to zero if in constant position mode void NavEKF::ResetPosition(void) { - if (constPosMode || PV_AidingMode != ABSOLUTE) { + if (constPosMode || (PV_AidingMode != ABSOLUTE)) { state.position.x = 0; state.position.y = 0; } else if (!gpsNotAvailable) { @@ -517,7 +502,7 @@ void NavEKF::InitialiseFilterDynamic(void) statesInitialised = false; // Set re-used variables to zero - ZeroVariables(); + InitialiseVariables(); // get initial time deltat between IMU measurements (sec) dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f); @@ -530,13 +515,13 @@ void NavEKF::InitialiseFilterDynamic(void) inhibitLoadLeveling = true; } // set number of updates over which gps and baro measurements are applied to the velocity and position states - gpsUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecGpsAvg); + gpsUpdateCountMaxInv = (dtIMU * 1000.0f)/float(msecGpsAvg); gpsUpdateCountMax = uint8_t(1.0f/gpsUpdateCountMaxInv); - hgtUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecHgtAvg); + hgtUpdateCountMaxInv = (dtIMU * 1000.0f)/float(msecHgtAvg); hgtUpdateCountMax = uint8_t(1.0f/hgtUpdateCountMaxInv); - magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecMagAvg); + magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(msecMagAvg); magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv); - flowUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecFlowAvg); + flowUpdateCountMaxInv = (dtIMU * 1000.0f)/float(msecFlowAvg); flowUpdateCountMax = uint8_t(1.0f/flowUpdateCountMaxInv); // calculate initial orientation and earth magnetic field states @@ -578,7 +563,7 @@ void NavEKF::InitialiseFilterDynamic(void) void NavEKF::InitialiseFilterBootstrap(void) { // set re-used variables to zero - ZeroVariables(); + InitialiseVariables(); // get initial time deltat between IMU measurements (sec) dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f); @@ -592,11 +577,11 @@ void NavEKF::InitialiseFilterBootstrap(void) } // set number of updates over which gps and baro measurements are applied to the velocity and position states - gpsUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecGpsAvg); + gpsUpdateCountMaxInv = (dtIMU * 1000.0f)/float(msecGpsAvg); gpsUpdateCountMax = uint8_t(1.0f/gpsUpdateCountMaxInv); - hgtUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecHgtAvg); + hgtUpdateCountMaxInv = (dtIMU * 1000.0f)/float(msecHgtAvg); hgtUpdateCountMax = uint8_t(1.0f/hgtUpdateCountMaxInv); - magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecMagAvg); + magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(msecMagAvg); magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv); // acceleration vector in XYZ body axes measured by the IMU (m/s^2) @@ -741,7 +726,7 @@ void NavEKF::SelectVelPosFusion() readGpsData(); // Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift - uint32_t gpsRetryTimeout = useAirspeed() ? _gpsRetryTimeUseTAS : _gpsRetryTimeNoTAS; + uint16_t gpsRetryTimeout = useAirspeed() ? gpsRetryTimeUseTAS : gpsRetryTimeNoTAS; // If we haven't received GPS data for a while, then declare the position and velocity data as being timed out if (imuSampleTime_ms - lastFixTime_ms > gpsRetryTimeout) { @@ -820,7 +805,7 @@ void NavEKF::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 = (_fusionModeGPS == 0 && !velTimeout) ? _hgtRetryTimeMode0 : _hgtRetryTimeMode12; + hgtRetryTime = (_fusionModeGPS == 0 && !velTimeout) ? hgtRetryTimeMode0 : hgtRetryTimeMode12; if (imuSampleTime_ms - lastHgtMeasTime > hgtRetryTime) { hgtTimeout = true; } @@ -869,7 +854,7 @@ void NavEKF::SelectMagFusion() if (magHealth) { magTimeout = false; lastHealthyMagTime_ms = imuSampleTime_ms; - } else if ((imuSampleTime_ms - lastHealthyMagTime_ms) > _magFailTimeLimit_ms && use_compass()) { + } else if ((imuSampleTime_ms - lastHealthyMagTime_ms) > magFailTimeLimit_ms && use_compass()) { magTimeout = true; } @@ -984,7 +969,7 @@ void NavEKF::SelectTasFusion() readAirSpdData(); // If we haven't received airspeed data for a while, then declare the airspeed data as being timed out - if (imuSampleTime_ms - lastAirspeedUpdate > _tasRetryTime) { + if (imuSampleTime_ms - lastAirspeedUpdate > tasRetryTime) { tasTimeout = true; } @@ -1011,7 +996,7 @@ void NavEKF::SelectBetaFusion() // set to true if fusion is locked out due to magnetometer fusion on the same time step (done for load levelling) bool f_lockedOut = (magFusePerformed && !inhibitLoadLeveling); // set true when the fusion time interval has triggered - bool f_timeTrigger = ((imuSampleTime_ms - BETAmsecPrev) >= _msecBetaAvg); + bool f_timeTrigger = ((imuSampleTime_ms - BETAmsecPrev) >= msecBetaAvg); // 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() && posHealth); // set true when sideslip fusion is feasible (requires zero sideslip assumption to be valid and use of wind states) @@ -1186,7 +1171,7 @@ void NavEKF::CovariancePrediction() for (uint8_t i=10; i<=12; i++) { processNoise[i] = dAngBiasSigma; if (!vehicleArmed) { - processNoise[i] *= _gyroBiasNoiseScaler; + processNoise[i] *= gyroBiasNoiseScaler; } } processNoise[13] = dVelBiasSigma; @@ -1832,8 +1817,8 @@ void NavEKF::FuseVelPosNED() // set the GPS data timeout depending on whether airspeed data is present uint32_t gpsRetryTime; - if (useAirspeed()) gpsRetryTime = _gpsRetryTimeUseTAS; - else gpsRetryTime = _gpsRetryTimeNoTAS; + if (useAirspeed()) gpsRetryTime = gpsRetryTimeUseTAS; + else gpsRetryTime = gpsRetryTimeNoTAS; // 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 @@ -1853,11 +1838,11 @@ void NavEKF::FuseVelPosNED() observation[5] = -hgtMea; // calculate additional error in GPS velocity caused by manoeuvring - NEvelErr = _gpsNEVelVarAccScale * accNavMag; - DvelErr = _gpsDVelVarAccScale * accNavMag; + NEvelErr = gpsNEVelVarAccScale * accNavMag; + DvelErr = gpsDVelVarAccScale * accNavMag; // calculate additional error in GPS position caused by manoeuvring - posErr = _gpsPosVarAccScale * accNavMag; + posErr = gpsPosVarAccScale * accNavMag; // estimate the GPS Velocity, GPS horiz position and height measurement variances. R_OBS[0] = sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(NEvelErr); @@ -1870,7 +1855,7 @@ void NavEKF::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 (_fusionModeGPS == 0 && fuseVelData && (imuSampleTime_ms - lastHgtTime_ms) < (2 * _msecHgtAvg)) { + if (_fusionModeGPS == 0 && fuseVelData && (imuSampleTime_ms - lastHgtTime_ms) < (2 * msecHgtAvg)) { // calculate innovations for height and vertical GPS vel measurements float hgtErr = statesAtHgtTime.position.z - observation[5]; float velDErr = statesAtVelTime.velocity.z - observation[2]; @@ -2219,7 +2204,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 = sq(constrain_float(_magNoise, 0.01f, 0.5f)) + sq(_magVarRateScale*dAngIMU.length() / dtIMU); + R_MAG = sq(constrain_float(_magNoise, 0.01f, 0.5f)) + sq(magVarRateScale*dAngIMU.length() / dtIMU); // calculate observation jacobians SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; @@ -3175,7 +3160,7 @@ void NavEKF::FuseAirspeed() // fail if the ratio is > 1, but don't fail if bad IMU data tasHealth = ((tasTestRatio < 1.0f) || badIMUdata); - tasTimeout = (imuSampleTime_ms - tasFailTime) > _tasRetryTime; + tasTimeout = (imuSampleTime_ms - tasFailTime) > tasRetryTime; // 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)) @@ -4017,7 +4002,7 @@ void NavEKF::readHgtData() newDataHgt = true; // get states that wer stored at the time closest to the measurement time, taking measurement delay into account - RecallStates(statesAtHgtTime, (imuSampleTime_ms - _msecHgtDelay)); + RecallStates(statesAtHgtTime, (imuSampleTime_ms - msecHgtDelay)); } else { newDataHgt = false; } @@ -4034,7 +4019,7 @@ void NavEKF::readMagData() magData = _ahrs->get_compass()->get_field() * 0.001f; // get states stored at time closest to measurement time after allowance for measurement delay - RecallStates(statesAtMagMeasTime, (imuSampleTime_ms - _msecMagDelay)); + RecallStates(statesAtMagMeasTime, (imuSampleTime_ms - msecMagDelay)); // let other processes know that new compass data has arrived newDataMag = true; @@ -4056,7 +4041,7 @@ void NavEKF::readAirSpdData() VtasMeas = aspeed->get_airspeed() * aspeed->get_EAS2TAS(); lastAirspeedUpdate = aspeed->last_update_ms(); newDataTas = true; - RecallStates(statesAtVtasMeasTime, (imuSampleTime_ms - _msecTasDelay)); + RecallStates(statesAtVtasMeasTime, (imuSampleTime_ms - msecTasDelay)); } else { newDataTas = false; } @@ -4296,8 +4281,8 @@ void NavEKF::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f offset = gpsPosGlitchOffsetNE; } -// zero stored variables - this needs to be called before a full filter initialisation -void NavEKF::ZeroVariables() +// Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary. +void NavEKF::InitialiseVariables() { // initialise time stamps imuSampleTime_ms = hal.scheduler->millis(); @@ -4323,6 +4308,7 @@ void NavEKF::ZeroVariables() rngMeaTime_ms = imuSampleTime_ms; ekfStartTime_ms = imuSampleTime_ms; + // initialise other variables gpsNoiseScaler = 1.0f; velTimeout = true; posTimeout = true; @@ -4360,7 +4346,6 @@ void NavEKF::ZeroVariables() memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta)); memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta)); memset(&flowIncrStateDelta[0], 0, sizeof(flowIncrStateDelta)); - // optical flow variables newDataFlow = false; flowDataValid = false; newDataRng = false; @@ -4383,6 +4368,16 @@ void NavEKF::ZeroVariables() vehicleArmed = false; prevVehicleArmed = false; constPosMode = true; + memset(&faultStatus, 0, sizeof(faultStatus)); + hgtRate = 0.0f; + mag_state.q0 = 1; + mag_state.DCM.identity(); + IMU1_weighting = 0.5f; + onGround = true; + manoeuvring = false; + yawAligned = false; + inhibitWindStates = true; + inhibitMagStates = true; } // return true if we should use the airspeed sensor diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 29da39fd69..f7f62bf2d3 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -337,7 +337,7 @@ private: Quaternion calcQuatAndFieldStates(float roll, float pitch); // zero stored variables - void ZeroVariables(); + void InitialiseVariables(); // reset the horizontal position states uing the last GPS measurement void ResetPosition(void); @@ -421,26 +421,34 @@ private: AP_Int8 _fallback; // EKF-to-DCM fallback strictness. 0 = trust EKF more, 1 = fallback more conservatively. // Tuning parameters - AP_Float _gpsNEVelVarAccScale; // scale factor applied to NE velocity measurement variance due to Vdot - AP_Float _gpsDVelVarAccScale; // scale factor applied to D velocity measurement variance due to Vdot - AP_Float _gpsPosVarAccScale; // scale factor applied to position measurement variance due to Vdot - AP_Int16 _msecHgtDelay; // effective average delay of height measurements rel to (msec) - AP_Int16 _msecMagDelay; // effective average delay of magnetometer measurements rel to IMU (msec) - AP_Int16 _msecTasDelay; // effective average delay of airspeed measurements rel to IMU (msec) - AP_Int16 _gpsRetryTimeUseTAS; // GPS retry time following innovation consistency fail if TAS measurements are used (msec) - AP_Int16 _gpsRetryTimeNoTAS; // GPS retry time following innovation consistency fail if no TAS measurements are used (msec) - AP_Int16 _hgtRetryTimeMode0; // 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) - AP_Int16 _tasRetryTime; // true airspeed measurement retry time following innovation consistency fail (msec) - uint32_t _magFailTimeLimit_ms; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec) - float _gyroBiasNoiseScaler; // scale factor applied to gyro bias state process variance when on ground - float _magVarRateScale; // scale factor applied to magnetometer variance due to angular rate - uint16_t _msecGpsAvg; // average number of msec between GPS measurements - uint16_t _msecHgtAvg; // average number of msec between height measurements - uint16_t _msecMagAvg; // average number of msec between magnetometer measurements - uint16_t _msecBetaAvg; // Average number of msec between synthetic sideslip measurements - uint16_t _msecBetaMax; // maximum number of msec between synthetic sideslip measurements - float dtVelPos; // average of msec between position and velocity corrections + 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 float 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 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 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 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 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) + const uint32_t flowIntervalMax_ms; // maximum allowable time between flow fusion events // Variables bool statesInitialised; // boolean true when filter states have been initialised @@ -507,8 +515,6 @@ private: bool fuseVtasData; // boolean true when airspeed data is to be fused float VtasMeas; // true airspeed measurement (m/s) state_elements statesAtVtasMeasTime; // filter states at the effective measurement time - const ftype covTimeStepMax; // maximum time allowed between covariance predictions - const ftype covDelAngMax; // maximum delta angle between covariance predictions bool covPredStep; // boolean set to true when a covariance prediction step has been performed bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step bool magFuseRequired; // boolean set to true when magnetometer fusion will be perfomred in the next time step @@ -516,7 +522,6 @@ private: bool tasFuseStep; // boolean set to true when airspeed fusion is being performed uint32_t TASmsecPrev; // time stamp of last TAS fusion step uint32_t BETAmsecPrev; // time stamp of last synthetic sideslip fusion step - const uint32_t TASmsecMax; // maximum allowed interval between TAS fusion steps uint32_t MAGmsecPrev; // time stamp of last compass fusion step uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step bool inhibitLoadLeveling; // boolean that turns off delay of fusion to level processor loading @@ -603,14 +608,11 @@ private: uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec) uint8_t flowQuality; // unsigned integer representing quality of optical flow data. 255 is maximum quality. uint32_t rngMeaTime_ms; // time stamp from latest range measurement (msec) - float DCM33FlowMin; // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high. - float fScaleFactorPnoise; // Process noise added to focal length scale factor state variance at each time step Vector3f omegaAcrossFlowTime; // body angular rates averaged across the optical flow sample period Matrix3f Tnb_flow; // transformation matrix from nav to body axes at the middle of the optical flow sample period Matrix3f Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period float varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2 float innovOptFlow[2]; // optical flow LOS innovations (rad/sec) - uint8_t flowTimeDeltaAvg_ms; // average interval between optical flow measurements (msec) float Popt[2][2]; // state covariance matrix float flowStates[2]; // flow states [scale factor, terrain position] float prevPosN; // north position at last measurement @@ -622,7 +624,6 @@ private: float rngMea; // range finder measurement (m) bool inhibitGndState; // true when the terrain position state is to remain constant uint32_t prevFlowFusionTime_ms; // time the last flow measurement was fused - uint32_t flowIntervalMax_ms; // maximum allowable time between flow fusion events bool fScaleInhibit; // true when the focal length scale factor is to remain constant float flowTestRatio[2]; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail float auxFlowTestRatio[2]; // sum of squares of optical flow innovations divided by fail threshold used by aux filter @@ -637,7 +638,6 @@ private: bool constVelMode; // true when fusing a constant velocity to maintain attitude reference when either optical flow or GPS measurements are lost after arming bool lastConstVelMode; // last value of holdVelocity Vector2f heldVelNE; // velocity held when no aiding is available - uint16_t _msecFlowAvg; // average number of msec between synthetic sideslip measurements uint8_t PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS enum {ABSOLUTE=0, // GPS aiding is being used (optical flow may also be used) so position estimates are absolute. NONE=1, // no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift.