Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 09:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Use default on ground range parameter from range finder object

Browse Source
This commit is contained in:
Paul Riseborough 2015-04-17 19:27:02 +10:00 committed by Randy Mackay
parent 10476333d8
commit 6c4c54c2ba
2 changed files with 17 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -114,9 +114,6 @@ extern const AP_HAL::HAL& hal;
#define INIT_GYRO_BIAS_UNCERTAINTY 0.1f #define INIT_GYRO_BIAS_UNCERTAINTY 0.1f
#define INIT_ACCEL_BIAS_UNCERTAINTY 0.3f #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 // Define tuning parameters
const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
@ -522,7 +519,7 @@ void NavEKF::ResetHeight(void)
for (uint8_t i=0; i<=49; i++){ for (uint8_t i=0; i<=49; i++){
storedStates[i].position.z = -hgtMea; 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 // 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); perf_begin(_perf_OpticalFlowEKF);
// constrain height above ground to be above range measured on ground // 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 // calculate a predicted LOS rate squared
float velHorizSq = sq(state.velocity.x) + sq(state.velocity.y); float velHorizSq = sq(state.velocity.x) + sq(state.velocity.y);
@ -2708,7 +2705,7 @@ void NavEKF::EstimateTerrainOffset()
// fuse range finder data // fuse range finder data
if (fuseRngData) { if (fuseRngData) {
// predict range // 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 // Copy required states to local variable names
float q0 = statesAtRngTime.quat[0]; // quaternion at optical flow measurement time 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)); varInnovRng = (R_RNG + Popt/sq(SK_RNG));
// constrain terrain height to be below the vehicle // 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 // Calculate the measurement innovation
innovRng = predRngMeas - rngMea; innovRng = predRngMeas - rngMea;
@ -2742,7 +2739,7 @@ void NavEKF::EstimateTerrainOffset()
terrainState -= K_RNG * innovRng; terrainState -= K_RNG * innovRng;
// constrain the state // constrain the state
terrainState = max(terrainState, statesAtRngTime.position[2] + RNG_MEAS_ON_GND); terrainState = max(terrainState, statesAtRngTime.position[2] + rngOnGnd);
// correct the covariance // correct the covariance
Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); 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]; vel.z = statesAtFlowTime.velocity[2];
// predict range to centre of image // 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 // 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 // calculate relative velocity in sensor frame
relVelSensor = Tnb_flow*vel; relVelSensor = Tnb_flow*vel;
@ -2836,7 +2833,7 @@ void NavEKF::EstimateTerrainOffset()
terrainState -= K_OPT * auxFlowObsInnov; terrainState -= K_OPT * auxFlowObsInnov;
// constrain the state // constrain the state
terrainState = max(terrainState, statesAtFlowTime.position[2] + RNG_MEAS_ON_GND); terrainState = max(terrainState, statesAtFlowTime.position[2] + rngOnGnd);
// correct the covariance // correct the covariance
Popt = Popt - K_OPT * H_OPT * Popt; Popt = Popt - K_OPT * H_OPT * Popt;
@ -2886,11 +2883,11 @@ void NavEKF::FuseOptFlow()
velNED_local.z = vd; velNED_local.z = vd;
// constrain height above ground to be above range measured on ground // 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 // Calculate observation jacobians and Kalman gains
if (obsIndex == 0) { if (obsIndex == 0) {
// calculate range from ground plain to centre of sensor fov assuming flat earth // 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 // calculate relative velocity in sensor frame
relVelSensor = Tnb_flow*velNED_local; relVelSensor = Tnb_flow*velNED_local;
@ -3677,7 +3674,7 @@ void NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScal
{ {
if (PV_AidingMode == AID_RELATIVE) { if (PV_AidingMode == AID_RELATIVE) {
// allow 1.0 rad/sec margin for angular motion // 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 // use standard gains up to 5.0 metres height and reduce above that
ekfNavVelGainScaler = 4.0f / max((terrainState - state.position[2]),4.0f); ekfNavVelGainScaler = 4.0f / max((terrainState - state.position[2]),4.0f);
} else { } else {
@ -3986,7 +3983,7 @@ void NavEKF::ConstrainStates()
// body magnetic field limit // body magnetic field limit
for (uint8_t i=19; i<=21; i++) states[i] = constrain_float(states[i],-0.5f,0.5f); for (uint8_t i=19; i<=21; i++) states[i] = constrain_float(states[i],-0.5f,0.5f);
// constrain the terrain offset state // 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 // update IMU delta angle and delta velocity measurements
@ -4138,7 +4135,7 @@ void NavEKF::readHgtData()
if (_fusionModeGPS == 3 && _altSource == 1) { if (_fusionModeGPS == 3 && _altSource == 1) {
if ((imuSampleTime_ms - rngValidMeaTime_ms) < 2000) { if ((imuSampleTime_ms - rngValidMeaTime_ms) < 2000) {
// adjust range finder measurement to allow for effect of vehicle tilt and height of sensor // 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 // get states that were stored at the time closest to the measurement time, taking measurement delay into account
statesAtHgtTime = statesAtFlowTime; statesAtHgtTime = statesAtFlowTime;
// calculate offset to baro data that enables baro to be used as a backup // 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; baroHgtOffset = 0.2f * (_baro.get_altitude() + state.position.z) + 0.8f * baroHgtOffset;
} else { } else {
// use baro measurement and correct for baro offset - failsafe use only as baro will drift // 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 // get states that were 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 {
// use baro measurement and correct for baro offset // 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 // get states that were stored at the time closest to the measurement time, taking measurement delay into account
RecallStates(statesAtHgtTime, (imuSampleTime_ms - msecHgtDelay)); RecallStates(statesAtHgtTime, (imuSampleTime_ms - msecHgtDelay));
} }

2
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -704,6 +704,8 @@ private:
bool gndOffsetValid; // true when the ground offset state can still be considered valid 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 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 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; bool haveDeltaAngles;