mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-21 16:18:29 -04:00
AP_NavEKF: Add methods to output and calculate vertical position derivative
This commit is contained in:
parent
9e3d9d15fc
commit
dab658f6ed
@ -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
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user