Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18: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_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));
}

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 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;