zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF: Consistent initialisation of tuning parameters and variables 

Non user adjustable parameters are now declared as 'const' in the header.
The _ prefix has been removed from non user adjustable tuning parameters.
We use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary.
For consistency some signed integer type declarations have been changed to unsigned where appropriate.
master
priseborough 10 years ago
parent
f1dfa282dc
commit
c06f6e105e
2 changed files with 98 additions and 103 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 143
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 58
      libraries/AP_NavEKF/AP_NavEKF.h

143
libraries/AP_NavEKF/AP_NavEKF.cpp
Unescape View File

@ -362,21 +362,39 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { @@ -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<struct state_elements *>(&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) : @@ -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 @@ -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) @@ -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) @@ -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) @@ -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) @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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 @@ -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() @@ -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() @@ -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() @@ -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

58
libraries/AP_NavEKF/AP_NavEKF.h
Unescape View File

@ -337,7 +337,7 @@ private: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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.

Write Preview
Loading…
Cancel
Save