diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 1daacdd645..5266d9a64e 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -114,9 +114,6 @@ extern const AP_HAL::HAL& hal; #define INIT_GYRO_BIAS_UNCERTAINTY 0.1f #define INIT_ACCEL_BIAS_UNCERTAINTY 0.3f -// altitude of OF and range finder when on ground -#define RNG_MEAS_ON_GND 0.1f - // Define tuning parameters const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { @@ -522,7 +519,7 @@ void NavEKF::ResetHeight(void) for (uint8_t i=0; i<=49; i++){ storedStates[i].position.z = -hgtMea; } - terrainState = state.position.z + RNG_MEAS_ON_GND; + terrainState = state.position.z + rngOnGnd; } // this function is used to initialise the filter whilst moving, using the AHRS DCM solution @@ -2678,7 +2675,7 @@ void NavEKF::EstimateTerrainOffset() perf_begin(_perf_OpticalFlowEKF); // constrain height above ground to be above range measured on ground - float heightAboveGndEst = max((terrainState - state.position.z), RNG_MEAS_ON_GND); + float heightAboveGndEst = max((terrainState - state.position.z), rngOnGnd); // calculate a predicted LOS rate squared float velHorizSq = sq(state.velocity.x) + sq(state.velocity.y); @@ -2708,7 +2705,7 @@ void NavEKF::EstimateTerrainOffset() // fuse range finder data if (fuseRngData) { // predict range - float predRngMeas = max((terrainState - statesAtRngTime.position[2]),RNG_MEAS_ON_GND) / Tnb_flow.c.z; + float predRngMeas = max((terrainState - statesAtRngTime.position[2]),rngOnGnd) / Tnb_flow.c.z; // Copy required states to local variable names float q0 = statesAtRngTime.quat[0]; // quaternion at optical flow measurement time @@ -2727,7 +2724,7 @@ void NavEKF::EstimateTerrainOffset() varInnovRng = (R_RNG + Popt/sq(SK_RNG)); // constrain terrain height to be below the vehicle - terrainState = max(terrainState, statesAtRngTime.position[2] + RNG_MEAS_ON_GND); + terrainState = max(terrainState, statesAtRngTime.position[2] + rngOnGnd); // Calculate the measurement innovation innovRng = predRngMeas - rngMea; @@ -2742,7 +2739,7 @@ void NavEKF::EstimateTerrainOffset() terrainState -= K_RNG * innovRng; // constrain the state - terrainState = max(terrainState, statesAtRngTime.position[2] + RNG_MEAS_ON_GND); + terrainState = max(terrainState, statesAtRngTime.position[2] + rngOnGnd); // correct the covariance Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); @@ -2771,10 +2768,10 @@ void NavEKF::EstimateTerrainOffset() vel.z = statesAtFlowTime.velocity[2]; // predict range to centre of image - float flowRngPred = max((terrainState - statesAtFlowTime.position[2]),RNG_MEAS_ON_GND) / Tnb_flow.c.z; + float flowRngPred = max((terrainState - statesAtFlowTime.position[2]),rngOnGnd) / Tnb_flow.c.z; // constrain terrain height to be below the vehicle - terrainState = max(terrainState, statesAtFlowTime.position[2] + RNG_MEAS_ON_GND); + terrainState = max(terrainState, statesAtFlowTime.position[2] + rngOnGnd); // calculate relative velocity in sensor frame relVelSensor = Tnb_flow*vel; @@ -2836,7 +2833,7 @@ void NavEKF::EstimateTerrainOffset() terrainState -= K_OPT * auxFlowObsInnov; // constrain the state - terrainState = max(terrainState, statesAtFlowTime.position[2] + RNG_MEAS_ON_GND); + terrainState = max(terrainState, statesAtFlowTime.position[2] + rngOnGnd); // correct the covariance Popt = Popt - K_OPT * H_OPT * Popt; @@ -2886,11 +2883,11 @@ void NavEKF::FuseOptFlow() velNED_local.z = vd; // constrain height above ground to be above range measured on ground - float heightAboveGndEst = max((terrainState - pd), RNG_MEAS_ON_GND); + float heightAboveGndEst = max((terrainState - pd), rngOnGnd); // Calculate observation jacobians and Kalman gains if (obsIndex == 0) { // calculate range from ground plain to centre of sensor fov assuming flat earth - float range = constrain_float((heightAboveGndEst/Tnb_flow.c.z),RNG_MEAS_ON_GND,1000.0f); + float range = constrain_float((heightAboveGndEst/Tnb_flow.c.z),rngOnGnd,1000.0f); // calculate relative velocity in sensor frame relVelSensor = Tnb_flow*velNED_local; @@ -3677,7 +3674,7 @@ void NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScal { if (PV_AidingMode == AID_RELATIVE) { // allow 1.0 rad/sec margin for angular motion - ekfGndSpdLimit = max((_maxFlowRate - 1.0f), 0.0f) * max((terrainState - state.position[2]), RNG_MEAS_ON_GND); + ekfGndSpdLimit = max((_maxFlowRate - 1.0f), 0.0f) * max((terrainState - state.position[2]), rngOnGnd); // use standard gains up to 5.0 metres height and reduce above that ekfNavVelGainScaler = 4.0f / max((terrainState - state.position[2]),4.0f); } else { @@ -3986,7 +3983,7 @@ void NavEKF::ConstrainStates() // body magnetic field limit for (uint8_t i=19; i<=21; i++) states[i] = constrain_float(states[i],-0.5f,0.5f); // constrain the terrain offset state - terrainState = max(terrainState, state.position.z + RNG_MEAS_ON_GND); + terrainState = max(terrainState, state.position.z + rngOnGnd); } // update IMU delta angle and delta velocity measurements @@ -4138,7 +4135,7 @@ void NavEKF::readHgtData() if (_fusionModeGPS == 3 && _altSource == 1) { if ((imuSampleTime_ms - rngValidMeaTime_ms) < 2000) { // adjust range finder measurement to allow for effect of vehicle tilt and height of sensor - hgtMea = max(rngMea * Tnb_flow.c.z, RNG_MEAS_ON_GND); + hgtMea = max(rngMea * Tnb_flow.c.z, rngOnGnd); // get states that were stored at the time closest to the measurement time, taking measurement delay into account statesAtHgtTime = statesAtFlowTime; // calculate offset to baro data that enables baro to be used as a backup @@ -4146,13 +4143,13 @@ void NavEKF::readHgtData() baroHgtOffset = 0.2f * (_baro.get_altitude() + state.position.z) + 0.8f * baroHgtOffset; } else { // use baro measurement and correct for baro offset - failsafe use only as baro will drift - hgtMea = max(_baro.get_altitude() - baroHgtOffset, RNG_MEAS_ON_GND); + hgtMea = max(_baro.get_altitude() - baroHgtOffset, rngOnGnd); // get states that were stored at the time closest to the measurement time, taking measurement delay into account RecallStates(statesAtHgtTime, (imuSampleTime_ms - msecHgtDelay)); } } else { // use baro measurement and correct for baro offset - hgtMea = _baro.get_altitude() - baroHgtOffset; + hgtMea = _baro.get_altitude(); // get states that were stored at the time closest to the measurement time, taking measurement delay into account RecallStates(statesAtHgtTime, (imuSampleTime_ms - msecHgtDelay)); } diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 0c3c3dba56..b55d335822 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -704,6 +704,8 @@ private: bool gndOffsetValid; // true when the ground offset state can still be considered valid bool flowXfailed; // true when the X optical flow measurement has failed the innovation consistency check float baroHgtOffset; // offset applied when baro height used as a backup height reference if range-finder fails + float rngOnGnd; // Expected range finder reading in metres when vehicle is on ground + bool haveDeltaAngles;