Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Consistent initialisation of tuning parameters and variables

Browse Source 
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.
This commit is contained in:
priseborough 2015-01-01 12:01:22 +11:00
parent f1dfa282dc
commit c06f6e105e
2 changed files with 97 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

141
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -362,21 +362,39 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
AP_GROUPEND AP_GROUPEND
}; };
// constructor // constructor
NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
_ahrs(ahrs), _ahrs(ahrs),
_baro(baro), _baro(baro),
state(*reinterpret_cast<struct state_elements *>(&states)), state(*reinterpret_cast<struct state_elements *>(&states)),
onGround(true), // initialise assuming we are on ground gpsNEVelVarAccScale(0.05f), // Scale factor applied to horizontal velocity measurement variance due to manoeuvre acceleration
manoeuvring(false), // initialise assuming we are not maneouvring gpsDVelVarAccScale(0.07f), // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates gpsPosVarAccScale(0.05f), // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates msecHgtDelay(60), // Height measurement delay (msec)
TASmsecMax(200), // maximum allowed interval between airspeed measurement updates msecMagDelay(40), // Magnetometer measurement delay (msec)
constPosMode(true), // forces position fusion with zero values msecTasDelay(240), // Airspeed measurement delay (msec)
yawAligned(false), // set true when heading or yaw angle has been aligned gpsRetryTimeUseTAS(10000), // GPS retry time with airspeed measurements (msec)
inhibitWindStates(true), // inhibit wind state updates on startup gpsRetryTimeNoTAS(10000), // GPS retry time without airspeed measurements (msec)
inhibitMagStates(true) // inhibit magnetometer state updates on startup 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 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")), ,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")),
@ -388,40 +406,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
#endif #endif
{ {
AP_Param::setup_object_defaults(this, var_info); 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 // 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 // resets position states to last GPS measurement or to zero if in constant position mode
void NavEKF::ResetPosition(void) void NavEKF::ResetPosition(void)
{ {
if (constPosMode || PV_AidingMode != ABSOLUTE) { if (constPosMode || (PV_AidingMode != ABSOLUTE)) {
state.position.x = 0; state.position.x = 0;
state.position.y = 0; state.position.y = 0;
} else if (!gpsNotAvailable) { } else if (!gpsNotAvailable) {
@ -517,7 +502,7 @@ void NavEKF::InitialiseFilterDynamic(void)
statesInitialised = false; statesInitialised = false;
// Set re-used variables to zero // Set re-used variables to zero
ZeroVariables(); InitialiseVariables();
// get initial time deltat between IMU measurements (sec) // get initial time deltat between IMU measurements (sec)
dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f); dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f);
@ -530,13 +515,13 @@ void NavEKF::InitialiseFilterDynamic(void)
inhibitLoadLeveling = true; inhibitLoadLeveling = true;
} }
// set number of updates over which gps and baro measurements are applied to the velocity and position states // 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); gpsUpdateCountMax = uint8_t(1.0f/gpsUpdateCountMaxInv);
hgtUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecHgtAvg); hgtUpdateCountMaxInv = (dtIMU * 1000.0f)/float(msecHgtAvg);
hgtUpdateCountMax = uint8_t(1.0f/hgtUpdateCountMaxInv); hgtUpdateCountMax = uint8_t(1.0f/hgtUpdateCountMaxInv);
magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecMagAvg); magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(msecMagAvg);
magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv); magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv);
flowUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecFlowAvg); flowUpdateCountMaxInv = (dtIMU * 1000.0f)/float(msecFlowAvg);
flowUpdateCountMax = uint8_t(1.0f/flowUpdateCountMaxInv); flowUpdateCountMax = uint8_t(1.0f/flowUpdateCountMaxInv);
// calculate initial orientation and earth magnetic field states // calculate initial orientation and earth magnetic field states
@ -578,7 +563,7 @@ void NavEKF::InitialiseFilterDynamic(void)
void NavEKF::InitialiseFilterBootstrap(void) void NavEKF::InitialiseFilterBootstrap(void)
{ {
// set re-used variables to zero // set re-used variables to zero
ZeroVariables(); InitialiseVariables();
// get initial time deltat between IMU measurements (sec) // get initial time deltat between IMU measurements (sec)
dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f); 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 // 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); gpsUpdateCountMax = uint8_t(1.0f/gpsUpdateCountMaxInv);
hgtUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecHgtAvg); hgtUpdateCountMaxInv = (dtIMU * 1000.0f)/float(msecHgtAvg);
hgtUpdateCountMax = uint8_t(1.0f/hgtUpdateCountMaxInv); hgtUpdateCountMax = uint8_t(1.0f/hgtUpdateCountMaxInv);
magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecMagAvg); magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(msecMagAvg);
magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv); magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv);
// acceleration vector in XYZ body axes measured by the IMU (m/s^2) // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
@ -741,7 +726,7 @@ void NavEKF::SelectVelPosFusion()
readGpsData(); readGpsData();
// Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift // 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 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) { 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 // 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 // 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) { if (imuSampleTime_ms - lastHgtMeasTime > hgtRetryTime) {
hgtTimeout = true; hgtTimeout = true;
} }
@ -869,7 +854,7 @@ void NavEKF::SelectMagFusion()
if (magHealth) { if (magHealth) {
magTimeout = false; magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms; 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; magTimeout = true;
} }
@ -984,7 +969,7 @@ void NavEKF::SelectTasFusion()
readAirSpdData(); readAirSpdData();
// If we haven't received airspeed data for a while, then declare the airspeed data as being timed out // 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; 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) // 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); bool f_lockedOut = (magFusePerformed && !inhibitLoadLeveling);
// set true when the fusion time interval has triggered // 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 // 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); 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) // 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++) { for (uint8_t i=10; i<=12; i++) {
processNoise[i] = dAngBiasSigma; processNoise[i] = dAngBiasSigma;
if (!vehicleArmed) { if (!vehicleArmed) {
processNoise[i] *= _gyroBiasNoiseScaler; processNoise[i] *= gyroBiasNoiseScaler;
} }
} }
processNoise[13] = dVelBiasSigma; processNoise[13] = dVelBiasSigma;
@ -1832,8 +1817,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 = gpsRetryTimeUseTAS;
else gpsRetryTime = _gpsRetryTimeNoTAS; else gpsRetryTime = gpsRetryTimeNoTAS;
// form the observation vector and zero velocity and horizontal position observations if in constant position mode // 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 // If in constant velocity mode, hold the last known horizontal velocity vector
@ -1853,11 +1838,11 @@ void NavEKF::FuseVelPosNED()
observation[5] = -hgtMea; observation[5] = -hgtMea;
// calculate additional error in GPS velocity caused by manoeuvring // calculate additional error in GPS velocity caused by manoeuvring
NEvelErr = _gpsNEVelVarAccScale * accNavMag; NEvelErr = gpsNEVelVarAccScale * accNavMag;
DvelErr = _gpsDVelVarAccScale * accNavMag; DvelErr = gpsDVelVarAccScale * accNavMag;
// calculate additional error in GPS position caused by manoeuvring // 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. // estimate the GPS Velocity, GPS horiz position and height measurement variances.
R_OBS[0] = sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(NEvelErr); 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 // 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 // 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. // 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 // calculate innovations for height and vertical GPS vel measurements
float hgtErr = statesAtHgtTime.position.z - observation[5]; float hgtErr = statesAtHgtTime.position.z - observation[5];
float velDErr = statesAtVelTime.velocity.z - observation[2]; 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; 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(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 // 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;
@ -3175,7 +3160,7 @@ void NavEKF::FuseAirspeed()
// fail if the ratio is > 1, but don't fail if bad IMU data // fail if the ratio is > 1, but don't fail if bad IMU data
tasHealth = ((tasTestRatio < 1.0f) || badIMUdata); 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 // 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)) if (tasHealth || (tasTimeout && posTimeout))
@ -4017,7 +4002,7 @@ void NavEKF::readHgtData()
newDataHgt = true; newDataHgt = true;
// get states that wer stored at the time closest to the measurement time, taking measurement delay into account // 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 { } else {
newDataHgt = false; newDataHgt = false;
} }
@ -4034,7 +4019,7 @@ void NavEKF::readMagData()
magData = _ahrs->get_compass()->get_field() * 0.001f; magData = _ahrs->get_compass()->get_field() * 0.001f;
// get states stored at time closest to measurement time after allowance for measurement delay // 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 // let other processes know that new compass data has arrived
newDataMag = true; newDataMag = true;
@ -4056,7 +4041,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, (imuSampleTime_ms - _msecTasDelay)); RecallStates(statesAtVtasMeasTime, (imuSampleTime_ms - msecTasDelay));
} else { } else {
newDataTas = false; newDataTas = false;
} }
@ -4296,8 +4281,8 @@ void NavEKF::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f
offset = gpsPosGlitchOffsetNE; offset = gpsPosGlitchOffsetNE;
} }
// zero stored variables - this needs to be called before a full filter initialisation // 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::ZeroVariables() void NavEKF::InitialiseVariables()
{ {
// initialise time stamps // initialise time stamps
imuSampleTime_ms = hal.scheduler->millis(); imuSampleTime_ms = hal.scheduler->millis();
@ -4323,6 +4308,7 @@ void NavEKF::ZeroVariables()
rngMeaTime_ms = imuSampleTime_ms; rngMeaTime_ms = imuSampleTime_ms;
ekfStartTime_ms = imuSampleTime_ms; ekfStartTime_ms = imuSampleTime_ms;
// initialise other variables
gpsNoiseScaler = 1.0f; gpsNoiseScaler = 1.0f;
velTimeout = true; velTimeout = true;
posTimeout = true; posTimeout = true;
@ -4360,7 +4346,6 @@ void NavEKF::ZeroVariables()
memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta)); memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta));
memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta)); memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta));
memset(&flowIncrStateDelta[0], 0, sizeof(flowIncrStateDelta)); memset(&flowIncrStateDelta[0], 0, sizeof(flowIncrStateDelta));
// optical flow variables
newDataFlow = false; newDataFlow = false;
flowDataValid = false; flowDataValid = false;
newDataRng = false; newDataRng = false;
@ -4383,6 +4368,16 @@ void NavEKF::ZeroVariables()
vehicleArmed = false; vehicleArmed = false;
prevVehicleArmed = false; prevVehicleArmed = false;
constPosMode = true; 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 // return true if we should use the airspeed sensor

58
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -337,7 +337,7 @@ private:
Quaternion calcQuatAndFieldStates(float roll, float pitch); Quaternion calcQuatAndFieldStates(float roll, float pitch);
// zero stored variables // zero stored variables
void ZeroVariables(); void InitialiseVariables();
// reset the horizontal position states uing the last GPS measurement // reset the horizontal position states uing the last GPS measurement
void ResetPosition(void); void ResetPosition(void);
@ -421,26 +421,34 @@ private:
AP_Int8 _fallback; // EKF-to-DCM fallback strictness. 0 = trust EKF more, 1 = fallback more conservatively. AP_Int8 _fallback; // EKF-to-DCM fallback strictness. 0 = trust EKF more, 1 = fallback more conservatively.
// Tuning parameters // Tuning parameters
AP_Float _gpsNEVelVarAccScale; // scale factor applied to NE velocity measurement variance due to Vdot const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
AP_Float _gpsDVelVarAccScale; // scale factor applied to D velocity measurement variance due to Vdot const float gpsDVelVarAccScale; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
AP_Float _gpsPosVarAccScale; // scale factor applied to position measurement variance due to Vdot const float gpsPosVarAccScale; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
AP_Int16 _msecHgtDelay; // effective average delay of height measurements rel to (msec) const float msecHgtDelay; // Height measurement delay (msec)
AP_Int16 _msecMagDelay; // effective average delay of magnetometer measurements rel to IMU (msec) const uint16_t msecMagDelay; // Magnetometer measurement delay (msec)
AP_Int16 _msecTasDelay; // effective average delay of airspeed measurements rel to IMU (msec) const uint16_t msecTasDelay; // Airspeed measurement delay (msec)
AP_Int16 _gpsRetryTimeUseTAS; // GPS retry time following innovation consistency fail if TAS measurements are used (msec) const uint16_t gpsRetryTimeUseTAS; // GPS retry time with airspeed measurements (msec)
AP_Int16 _gpsRetryTimeNoTAS; // GPS retry time following innovation consistency fail if no TAS measurements are used (msec) const uint16_t gpsRetryTimeNoTAS; // GPS retry time without airspeed measurements (msec)
AP_Int16 _hgtRetryTimeMode0; // height measurement retry time following innovation consistency fail if GPS fusion mode is = 0 (msec) const uint16_t hgtRetryTimeMode0; // Height retry time with vertical velocity measurement (msec)
AP_Int16 _hgtRetryTimeMode12; // height measurement retry time following innovation consistency fail if GPS fusion mode is > 0 (msec) const uint16_t hgtRetryTimeMode12; // Height retry time without vertical velocity measurement (msec)
AP_Int16 _tasRetryTime; // true airspeed measurement retry time following innovation consistency fail (msec) const uint16_t tasRetryTime; // True airspeed timeout and retry interval (msec)
uint32_t _magFailTimeLimit_ms; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec) const 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 const float magVarRateScale; // scale factor applied to magnetometer variance due to angular rate
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
uint16_t _msecGpsAvg; // average number of msec between GPS measurements const uint16_t msecGpsAvg; // average number of msec between GPS measurements
uint16_t _msecHgtAvg; // average number of msec between height measurements const uint16_t msecHgtAvg; // average number of msec between height measurements
uint16_t _msecMagAvg; // average number of msec between magnetometer measurements const uint16_t msecMagAvg; // average number of msec between magnetometer measurements
uint16_t _msecBetaAvg; // Average number of msec between synthetic sideslip measurements const uint16_t msecBetaAvg; // average number of msec between synthetic sideslip measurements
uint16_t _msecBetaMax; // maximum number of msec between synthetic sideslip measurements const uint16_t msecBetaMax; // maximum number of msec between synthetic sideslip measurements
float dtVelPos; // average of msec between position and velocity corrections 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 // Variables
bool statesInitialised; // boolean true when filter states have been initialised 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 bool fuseVtasData; // boolean true when airspeed data is to be fused
float VtasMeas; // true airspeed measurement (m/s) float VtasMeas; // true airspeed measurement (m/s)
state_elements statesAtVtasMeasTime; // filter states at the effective measurement time 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 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 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 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 bool tasFuseStep; // boolean set to true when airspeed fusion is being performed
uint32_t TASmsecPrev; // time stamp of last TAS fusion step uint32_t TASmsecPrev; // time stamp of last TAS fusion step
uint32_t BETAmsecPrev; // time stamp of last synthetic sideslip 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 MAGmsecPrev; // time stamp of last compass fusion step
uint32_t HGTmsecPrev; // time stamp of last height measurement 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 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) 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. 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) 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 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 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 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 varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2
float innovOptFlow[2]; // optical flow LOS innovations (rad/sec) 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 Popt[2][2]; // state covariance matrix
float flowStates[2]; // flow states [scale factor, terrain position] float flowStates[2]; // flow states [scale factor, terrain position]
float prevPosN; // north position at last measurement float prevPosN; // north position at last measurement
@ -622,7 +624,6 @@ private:
float rngMea; // range finder measurement (m) float rngMea; // range finder measurement (m)
bool inhibitGndState; // true when the terrain position state is to remain constant 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 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 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 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 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 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 bool lastConstVelMode; // last value of holdVelocity
Vector2f heldVelNE; // velocity held when no aiding is available 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 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. 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. 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.