AP_NavEKF: Add methods to output and calculate vertical position derivative

This commit is contained in:
Paul Riseborough 2015-10-12 09:50:35 +11:00 committed by Andrew Tridgell
parent 9e3d9d15fc
commit dab658f6ed
2 changed files with 29 additions and 0 deletions

View File

@ -506,6 +506,7 @@ void NavEKF::ResetVelocity(void)
state.velocity.zero();
state.vel1.zero();
state.vel2.zero();
posDownDerivative = 0.0f;
} else if (!gpsNotAvailable) {
// reset horizontal velocity states, applying an offset to the GPS velocity to prevent the GPS position being rejected when the GPS position offset is being decayed to zero.
state.velocity.x = velNED.x + gpsVelGlitchOffset.x; // north velocity from blended accel data
@ -536,6 +537,8 @@ void NavEKF::ResetHeight(void)
storedStates[i].position.z = -hgtMea;
}
terrainState = state.position.z + rngOnGnd;
// reset the height state for the complementary filter used to provide a vertical position dervative
posDown = state.position.z;
}
// this function is used to initialise the filter whilst moving, using the AHRS DCM solution
@ -1218,6 +1221,13 @@ void NavEKF::UpdateStrapdownEquationsNED()
state.vel1 += delVelNav1;
state.vel2 += delVelNav2;
// update vertical velocity and position states used to provide a vertical position derivative output
// using a simple complementary filter
float lastPosDownDerivative = posDownDerivative;
posDownDerivative += delVelNav.z;
float posDownDerivativeCorrection = 0.2f * (state.position.z - posDown);
posDown += (posDownDerivative + lastPosDownDerivative) * (dtIMUactual*0.5f) + posDownDerivativeCorrection * dtIMUactual;
// apply a trapezoidal integration to velocities to calculate position
state.position += (state.velocity + lastVelocity) * (dtIMUactual*0.5f);
state.posD1 += (state.vel1.z + lastVel1.z) * (dtIMUactual*0.5f);
@ -3684,6 +3694,13 @@ void NavEKF::getVelNED(Vector3f &vel) const
vel = state.velocity;
}
// Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s
void NavEKF::getPosDownDerivative(float &ret) const
{
// return the value calculated from a complmentary filer applied to the EKF height and vertical acceleration
ret = posDownDerivative;
}
// Return the last calculated NED position relative to the reference point (m).
// if a calculated solution is not available, use the best available data and return false
bool NavEKF::getPosNED(Vector3f &pos) const
@ -4824,6 +4841,8 @@ void NavEKF::InitialiseVariables()
gpsVertVelFilt = 0.0f;
gpsHorizVelFilt = 0.0f;
memset(&gpsCheckStatus, 0, sizeof(gpsCheckStatus));
posDownDerivative = 0.0f;
posDown = 0.0f;
}
// return true if we should use the airspeed sensor

View File

@ -122,6 +122,11 @@ public:
// return NED velocity in m/s
void getVelNED(Vector3f &vel) const;
// Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s
// This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF
// but will always be kinematically consistent with the z component of the EKF position state
void getPosDownDerivative(float &ret) const;
// This returns the specific forces in the NED frame
void getAccelNED(Vector3f &accelNED) const;
@ -489,6 +494,9 @@ private:
// update inflight calculaton that determines if GPS data is good enough for reliable navigation
void calcGpsGoodForFlight(void);
// calculate the derivative of the down position using a complementary filter applied to vertical acceleration and EKF height
void calcPosDownDerivative();
// EKF Mavlink Tuneable Parameters
AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s
AP_Float _gpsVertVelNoise; // GPS vertical velocity measurement noise : m/s
@ -713,6 +721,8 @@ private:
float gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
bool gpsGoodToAlign; // true when GPS quality is good enough to set an EKF origin and commence GPS navigation
uint32_t lastConstPosFuseTime_ms; // last time in msec the constant position constraint was applied
float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.
float posDown; // Down position state used in calculation of posDownRate
// Used by smoothing of state corrections
Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement