mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-19 14:23:57 -04:00
AP_NavEKF2: Reduce memory required by 6KB when running at 400Hz
Down-sample the IMU and output observer state data to 100Hz for storage in the buffer. This reduces storage requirements for Copter by 75% or 6KB It does not affect memory required by plane which already uses short buffers due to its 50Hz execution rate. This means that the EKF filter operations operate at a maximum rate of 100Hz. The output observer continues to operate at 400Hz and coning and sculling corrections are applied during the down-sampling so there is no loss of accuracy.
This commit is contained in:
Paul Riseborough
Andrew Tridgell
parent
5e2382ea09
commit
577670ccee
@ -434,7 +434,7 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
|
||||
gndEffectTimeout_ms(1000), // time in msec that baro ground effect compensation will timeout after initiation
|
||||
gndEffectBaroScaler(4.0f), // scaler applied to the barometer observation variance when operating in ground effect
|
||||
gndGradientSigma(2), // RMS terrain gradient percentage assumed by the terrain height estimation
|
||||
fusionTimeStep_ms(10) // The nominal number of msec between covariance prediction and fusion operations
|
||||
fusionTimeStep_ms(10) // The minimum number of msec between covariance prediction and fusion operations
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
@ -497,8 +497,20 @@ void NavEKF2::UpdateFilter(void)
|
||||
if (!core) {
|
||||
return;
|
||||
}
|
||||
|
||||
const AP_InertialSensor &ins = _ahrs->get_ins();
|
||||
|
||||
for (uint8_t i=0; i<num_cores; i++) {
|
||||
core[i].UpdateFilter();
|
||||
// if the previous core has only recently finished a new state prediction cycle, then
|
||||
// dont start a new cycle to allow time for fusion operations to complete if the update
|
||||
// rate is higher than 200Hz
|
||||
bool statePredictEnabled;
|
||||
if ((i > 0) && (core[i-1].getFramesSincePredict() < 2) && (ins.get_sample_rate() > 200)) {
|
||||
statePredictEnabled = false;
|
||||
} else {
|
||||
statePredictEnabled = true;
|
||||
}
|
||||
core[i].UpdateFilter(statePredictEnabled);
|
||||
}
|
||||
|
||||
// If the current core selected has a bad fault score or is unhealthy, switch to a healthy core with the lowest fault score
|
||||
|
||||
@ -337,7 +337,7 @@ private:
|
||||
const uint16_t gndEffectTimeout_ms; // time in msec that ground effect mode is active after being activated
|
||||
const float gndEffectBaroScaler; // scaler applied to the barometer observation variance when ground effect mode is active
|
||||
const uint8_t gndGradientSigma; // RMS terrain gradient percentage assumed by the terrain height estimation
|
||||
const uint8_t fusionTimeStep_ms; // The nominal time interval between covariance predictions and measurement fusions in msec
|
||||
const uint8_t fusionTimeStep_ms; // The minimum time interval between covariance predictions and measurement fusions in msec
|
||||
};
|
||||
|
||||
#endif //AP_NavEKF2
|
||||
|
||||
@ -215,8 +215,6 @@ void NavEKF2_core::SelectTasFusion()
|
||||
tasDataWaiting = (statesInitialised && !inhibitWindStates && newDataTas);
|
||||
if (tasDataWaiting)
|
||||
{
|
||||
// ensure that the covariance prediction is up to date before fusing data
|
||||
if (!covPredStep) CovariancePrediction();
|
||||
FuseAirspeed();
|
||||
prevTasStep_ms = imuSampleTime_ms;
|
||||
tasDataWaiting = false;
|
||||
@ -285,8 +283,6 @@ void NavEKF2_core::SelectBetaFusion()
|
||||
bool f_feasible = (assume_zero_sideslip() && !inhibitWindStates);
|
||||
// use synthetic sideslip fusion if feasible, required and enough time has lapsed since the last fusion
|
||||
if (f_feasible && f_required && f_timeTrigger) {
|
||||
// ensure that the covariance prediction is up to date before fusing data
|
||||
if (!covPredStep) CovariancePrediction();
|
||||
FuseSideslip();
|
||||
prevBetaStep_ms = imuSampleTime_ms;
|
||||
}
|
||||
|
||||
@ -164,7 +164,7 @@ void NavEKF2_core::setAidingMode()
|
||||
void NavEKF2_core::checkAttitudeAlignmentStatus()
|
||||
{
|
||||
// Check for tilt convergence - used during initial alignment
|
||||
float alpha = 1.0f*dtIMUavg;
|
||||
float alpha = 1.0f*imuDataDelayed.delAngDT;
|
||||
float temp=tiltErrVec.length();
|
||||
tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt;
|
||||
if (tiltErrFilt < 0.005f && !tiltAlignComplete) {
|
||||
|
||||
@ -152,8 +152,6 @@ void NavEKF2_core::SelectMagFusion()
|
||||
// wait until the EKF time horizon catches up with the measurement
|
||||
bool dataReady = (newMagDataAvailable && statesInitialised && use_compass() && yawAlignComplete);
|
||||
if (dataReady) {
|
||||
// ensure that the covariance prediction is up to date before fusing data
|
||||
if (!covPredStep) CovariancePrediction();
|
||||
// If we haven't performed the first airborne magnetic field update or have inhibited magnetic field learning, then we use the simple method of declination to maintain heading
|
||||
if(inhibitMagStates) {
|
||||
fuseCompass();
|
||||
@ -261,7 +259,7 @@ void NavEKF2_core::FuseMagnetometer()
|
||||
}
|
||||
|
||||
// scale magnetometer observation error with total angular rate to allow for timing errors
|
||||
R_MAG = sq(constrain_float(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*imuDataDelayed.delAng.length() / dtIMUavg);
|
||||
R_MAG = sq(constrain_float(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*imuDataDelayed.delAng.length() / imuDataDelayed.delAngDT);
|
||||
|
||||
// calculate common expressions used to calculate observation jacobians an innovation variance for each component
|
||||
SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
||||
|
||||
@ -112,9 +112,8 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
|
||||
flowValidMeaTime_ms = imuSampleTime_ms;
|
||||
// estimate sample time of the measurement
|
||||
ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
|
||||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||||
ofDataNew.time_ms = roundToNearest(ofDataNew.time_ms, frontend->fusionTimeStep_ms);
|
||||
// Correct for the average intersampling delay due to the filter updaterate
|
||||
ofDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||||
// Prevent time delay exceeding age of oldest IMU data in the buffer
|
||||
ofDataNew.time_ms = max(ofDataNew.time_ms,imuDataDelayed.time_ms);
|
||||
// Save data to buffer
|
||||
@ -224,9 +223,8 @@ void NavEKF2_core::readMagData()
|
||||
// estimate of time magnetometer measurement was taken, allowing for delays
|
||||
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
|
||||
|
||||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||||
magDataNew.time_ms = roundToNearest(magDataNew.time_ms, frontend->fusionTimeStep_ms);
|
||||
// Correct for the average intersampling delay due to the filter updaterate
|
||||
magDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||||
|
||||
// read compass data and scale to improve numerical conditioning
|
||||
magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f;
|
||||
@ -285,7 +283,13 @@ bool NavEKF2_core::RecallMag()
|
||||
* Inertial Measurements *
|
||||
********************************************************/
|
||||
|
||||
// update IMU delta angle and delta velocity measurements
|
||||
/*
|
||||
* Read IMU delta angle and delta velocity measurements and downsample to 100Hz
|
||||
* for storage in the data buffers used by the EKF. If the IMU data arrives at
|
||||
* lower rate than 100Hz, then no downsampling or upsampling will be performed.
|
||||
* Downsampling is done using a method that does not introduce coning or sculling
|
||||
* errors.
|
||||
*/
|
||||
void NavEKF2_core::readIMUData()
|
||||
{
|
||||
const AP_InertialSensor &ins = _ahrs->get_ins();
|
||||
@ -311,11 +315,65 @@ void NavEKF2_core::readIMUData()
|
||||
}
|
||||
imuDataNew.delAngDT = max(ins.get_delta_time(),1.0e-4f);
|
||||
|
||||
// get current time stamp
|
||||
// Get current time stamp
|
||||
imuDataNew.time_ms = imuSampleTime_ms;
|
||||
|
||||
// save data in the FIFO buffer
|
||||
StoreIMU();
|
||||
// remove gyro scale factor errors
|
||||
imuDataNew.delAng.x = imuDataNew.delAng.x * stateStruct.gyro_scale.x;
|
||||
imuDataNew.delAng.y = imuDataNew.delAng.y * stateStruct.gyro_scale.y;
|
||||
imuDataNew.delAng.z = imuDataNew.delAng.z * stateStruct.gyro_scale.z;
|
||||
|
||||
// remove sensor bias errors
|
||||
imuDataNew.delAng -= stateStruct.gyro_bias;
|
||||
imuDataNew.delVel.z -= stateStruct.accel_zbias;
|
||||
|
||||
// Accumulate the measurement time interval for the delta velocity and angle data
|
||||
imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;
|
||||
imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;
|
||||
|
||||
// Rotate quaternon atitude from previous to new and normalise.
|
||||
// Accumulation using quaternions prevents introduction of coning errors due to downsampling
|
||||
Quaternion deltaQuat;
|
||||
deltaQuat.rotate(imuDataNew.delAng);
|
||||
imuQuatDownSampleNew = imuQuatDownSampleNew*deltaQuat;
|
||||
imuQuatDownSampleNew.normalize();
|
||||
|
||||
// Rotate the accumulated delta velocity into the new frame of reference created by the latest delta angle
|
||||
// This prevents introduction of sculling errors due to downsampling
|
||||
Matrix3f deltaRotMat;
|
||||
deltaQuat.inverse().rotation_matrix(deltaRotMat);
|
||||
imuDataDownSampledNew.delVel = deltaRotMat*imuDataDownSampledNew.delVel;
|
||||
|
||||
// accumulate the latest delta velocity
|
||||
imuDataDownSampledNew.delVel += imuDataNew.delVel;
|
||||
|
||||
// Keep track of the number of IMU frames since the last state prediction
|
||||
framesSincePredict++;
|
||||
|
||||
// If 10msec has elapsed, and the frontend has allowed us to start a new predict cycle, then store the accumulated IMU data
|
||||
// to be used by the state prediction, ignoring the frontend permission if more than 20msec has lapsed
|
||||
if ((dtIMUavg*(float)framesSincePredict >= 0.01f && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 0.02f)) {
|
||||
// convert the accumulated quaternion to an equivalent delta angle
|
||||
imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);
|
||||
// Time stamp the data
|
||||
imuDataDownSampledNew.time_ms = imuSampleTime_ms;
|
||||
// Write data to the FIFO IMU buffer
|
||||
StoreIMU();
|
||||
// zero the accumulated IMU data and quaternion
|
||||
imuDataDownSampledNew.delAng.zero();
|
||||
imuDataDownSampledNew.delVel.zero();
|
||||
imuDataDownSampledNew.delAngDT = 0.0f;
|
||||
imuDataDownSampledNew.delVelDT = 0.0f;
|
||||
imuQuatDownSampleNew[0] = 1.0f;
|
||||
imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f;
|
||||
// reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle
|
||||
framesSincePredict = 0;
|
||||
// set the flag to let the filter know it has new IMU data nad needs to run
|
||||
runUpdates = true;
|
||||
} else {
|
||||
// we don't have new IMU data in the buffer so don't run filter updates on this time step
|
||||
runUpdates = false;
|
||||
}
|
||||
|
||||
// extract the oldest available data from the FIFO buffer
|
||||
imuDataDelayed = storedIMU[fifoIndexDelayed];
|
||||
@ -330,10 +388,10 @@ void NavEKF2_core::StoreIMU()
|
||||
if (fifoIndexNow >= IMU_BUFFER_LENGTH) {
|
||||
fifoIndexNow = 0;
|
||||
}
|
||||
storedIMU[fifoIndexNow] = imuDataNew;
|
||||
storedIMU[fifoIndexNow] = imuDataDownSampledNew;
|
||||
// set the index required to access the oldest data, applying an offset to the fusion time horizon that is used to
|
||||
// prevent the same fusion operation being performed on the same frame across multiple EKF's
|
||||
fifoIndexDelayed = fifoIndexNow + 1 + fusionHorizonOffset;
|
||||
fifoIndexDelayed = fifoIndexNow + 1;
|
||||
if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) {
|
||||
fifoIndexDelayed = 0;
|
||||
}
|
||||
@ -359,7 +417,7 @@ void NavEKF2_core::RecallIMU()
|
||||
imuDataDelayed = storedIMU[fifoIndexDelayed];
|
||||
// make sure that the delta time used for the delta angles and velocities are is no less than 10% of dtIMUavg to prevent
|
||||
// divide by zero problems when converting to rates or acceleration
|
||||
float minDT = 0.1f*dtIMUavg;
|
||||
float minDT = 0.1f*dtEkfAvg;
|
||||
imuDataDelayed.delAngDT = max(imuDataDelayed.delAngDT,minDT);
|
||||
imuDataDelayed.delVelDT = max(imuDataDelayed.delVelDT,minDT);
|
||||
}
|
||||
@ -401,9 +459,8 @@ void NavEKF2_core::readGpsData()
|
||||
// ideally we should be using a timing signal from the GPS receiver to set this time
|
||||
gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend->_gpsDelay_ms;
|
||||
|
||||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||||
gpsDataNew.time_ms = roundToNearest(gpsDataNew.time_ms, frontend->fusionTimeStep_ms);
|
||||
// Correct for the average intersampling delay due to the filter updaterate
|
||||
gpsDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||||
|
||||
// Prevent time delay exceeding age of oldest IMU data in the buffer
|
||||
gpsDataNew.time_ms = max(gpsDataNew.time_ms,imuDataDelayed.time_ms);
|
||||
@ -637,9 +694,8 @@ void NavEKF2_core::readHgtData()
|
||||
// estimate of time height measurement was taken, allowing for delays
|
||||
baroDataNew.time_ms = lastHgtReceived_ms - frontend->_hgtDelay_ms;
|
||||
|
||||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||||
baroDataNew.time_ms = roundToNearest(baroDataNew.time_ms, frontend->fusionTimeStep_ms);
|
||||
// Correct for the average intersampling delay due to the filter updaterate
|
||||
baroDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||||
|
||||
// Prevent time delay exceeding age of oldest IMU data in the buffer
|
||||
baroDataNew.time_ms = max(baroDataNew.time_ms,imuDataDelayed.time_ms);
|
||||
@ -708,9 +764,8 @@ void NavEKF2_core::readAirSpdData()
|
||||
tasDataNew.tas = aspeed->get_airspeed() * aspeed->get_EAS2TAS();
|
||||
timeTasReceived_ms = aspeed->last_update_ms();
|
||||
tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
|
||||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||||
tasDataNew.time_ms = roundToNearest(tasDataNew.time_ms, frontend->fusionTimeStep_ms);
|
||||
// Correct for the average intersampling delay due to the filter update rate
|
||||
tasDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||||
newDataTas = true;
|
||||
StoreTAS();
|
||||
RecallTAS();
|
||||
@ -719,10 +774,4 @@ void NavEKF2_core::readAirSpdData()
|
||||
}
|
||||
}
|
||||
|
||||
// Round to the nearest multiple of a integer
|
||||
uint32_t NavEKF2_core::roundToNearest(uint32_t dividend, uint32_t divisor )
|
||||
{
|
||||
return ((uint32_t)round((float)dividend/float(divisor)))*divisor;
|
||||
}
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
|
||||
@ -84,8 +84,6 @@ void NavEKF2_core::SelectFlowFusion()
|
||||
{
|
||||
// Set the flow noise used by the fusion processes
|
||||
R_LOS = sq(max(frontend->_flowNoise, 0.05f));
|
||||
// ensure that the covariance prediction is up to date before fusing data
|
||||
if (!covPredStep) CovariancePrediction();
|
||||
// Fuse the optical flow X and Y axis data into the main filter sequentially
|
||||
FuseOptFlow();
|
||||
// reset flag to indicate that no new flow data is available for fusion
|
||||
|
||||
@ -104,11 +104,11 @@ void NavEKF2_core::getEulerAngles(Vector3f &euler) const
|
||||
// return body axis gyro bias estimates in rad/sec
|
||||
void NavEKF2_core::getGyroBias(Vector3f &gyroBias) const
|
||||
{
|
||||
if (dtIMUavg < 1e-6f) {
|
||||
if (dtEkfAvg < 1e-6f) {
|
||||
gyroBias.zero();
|
||||
return;
|
||||
}
|
||||
gyroBias = stateStruct.gyro_bias / dtIMUavg;
|
||||
gyroBias = stateStruct.gyro_bias / dtEkfAvg;
|
||||
}
|
||||
|
||||
// return body axis gyro scale factor error as a percentage
|
||||
@ -198,8 +198,8 @@ void NavEKF2_core::getAccelNED(Vector3f &accelNED) const {
|
||||
|
||||
// return the Z-accel bias estimate in m/s^2
|
||||
void NavEKF2_core::getAccelZBias(float &zbias) const {
|
||||
if (dtIMUavg > 0) {
|
||||
zbias = stateStruct.accel_zbias / dtIMUavg;
|
||||
if (dtEkfAvg > 0) {
|
||||
zbias = stateStruct.accel_zbias / dtEkfAvg;
|
||||
} else {
|
||||
zbias = 0;
|
||||
}
|
||||
@ -544,4 +544,11 @@ const char *NavEKF2_core::prearm_failure_reason(void) const
|
||||
}
|
||||
|
||||
|
||||
// report the number of frames lapsed since the last state prediction
|
||||
// this is used by other instances to level load
|
||||
uint8_t NavEKF2_core::getFramesSincePredict(void) const
|
||||
{
|
||||
return framesSincePredict;
|
||||
}
|
||||
|
||||
#endif // HAL_CPU_CLASS
|
||||
|
||||
@ -190,8 +190,6 @@ void NavEKF2_core::SelectVelPosFusion()
|
||||
|
||||
// perform fusion
|
||||
if (fuseVelData || fusePosData || fuseHgtData) {
|
||||
// ensure that the covariance prediction is up to date before fusing data
|
||||
if (!covPredStep) CovariancePrediction();
|
||||
FuseVelPosNED();
|
||||
// clear the flags to prevent repeated fusion of the same data
|
||||
fuseVelData = false;
|
||||
@ -233,7 +231,6 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
// so we might as well take advantage of the computational efficiencies
|
||||
// associated with sequential fusion
|
||||
if (fuseVelData || fusePosData || fuseHgtData) {
|
||||
|
||||
// set the GPS data timeout depending on whether airspeed data is present
|
||||
uint32_t gpsRetryTime;
|
||||
if (useAirspeed()) gpsRetryTime = frontend->gpsRetryTimeUseTAS_ms;
|
||||
@ -401,7 +398,6 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
if (fuseHgtData) {
|
||||
// calculate height innovations
|
||||
innovVelPos[5] = stateStruct.position.z - observation[5];
|
||||
|
||||
varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
|
||||
// calculate the innovation consistency test ratio
|
||||
hgtTestRatio = sq(innovVelPos[5]) / (sq(frontend->_hgtInnovGate) * varInnovVelPos[5]);
|
||||
@ -516,7 +512,6 @@ void NavEKF2_core::FuseVelPosNED()
|
||||
stateStruct.angErr.zero();
|
||||
|
||||
// calculate state corrections and re-normalise the quaternions for states predicted using the blended IMU data
|
||||
// Don't apply corrections to Z bias state as this has been done already as part of the single IMU calculations
|
||||
for (uint8_t i = 0; i<=stateIndexLim; i++) {
|
||||
statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex];
|
||||
}
|
||||
|
||||
@ -71,10 +71,11 @@ void NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _c
|
||||
// Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary.
|
||||
void NavEKF2_core::InitialiseVariables()
|
||||
{
|
||||
// Offset the fusion horizon if necessary to prevent frame over-runs
|
||||
if (dtIMUavg < 0.005) {
|
||||
fusionHorizonOffset = 2*core_index;
|
||||
}
|
||||
// calculate the nominal filter update rate
|
||||
const AP_InertialSensor &ins = _ahrs->get_ins();
|
||||
localFilterTimeStep_ms = (uint8_t)(1000.0f/(float)ins.get_sample_rate());
|
||||
localFilterTimeStep_ms = max(localFilterTimeStep_ms,10);
|
||||
|
||||
// initialise time stamps
|
||||
imuSampleTime_ms = hal.scheduler->millis();
|
||||
lastHealthyMagTime_ms = imuSampleTime_ms;
|
||||
@ -113,10 +114,9 @@ void NavEKF2_core::InitialiseVariables()
|
||||
badIMUdata = false;
|
||||
firstMagYawInit = false;
|
||||
dtIMUavg = 0.0025f;
|
||||
dtEkfAvg = 0.01f;
|
||||
dt = 0;
|
||||
velDotNEDfilt.zero();
|
||||
summedDelAng.zero();
|
||||
summedDelVel.zero();
|
||||
lastKnownPositionNE.zero();
|
||||
prevTnb.zero();
|
||||
memset(&P[0][0], 0, sizeof(P));
|
||||
@ -204,6 +204,12 @@ void NavEKF2_core::InitialiseVariables()
|
||||
velResetNE.zero();
|
||||
hgtInnovFiltState = 0.0f;
|
||||
magSelectIndex = _ahrs->get_compass()->get_primary();
|
||||
imuDataDownSampledNew.delAng.zero();
|
||||
imuDataDownSampledNew.delVel.zero();
|
||||
imuDataDownSampledNew.delAngDT = 0.0f;
|
||||
imuDataDownSampledNew.delVelDT = 0.0f;
|
||||
runUpdates = false;
|
||||
framesSincePredict = 0;
|
||||
}
|
||||
|
||||
// Initialise the states from accelerometer and magnetometer data (if present)
|
||||
@ -221,6 +227,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
|
||||
|
||||
// Initialise IMU data
|
||||
dtIMUavg = 1.0f/_ahrs->get_ins().get_sample_rate();
|
||||
dtEkfAvg = min(0.01f,dtIMUavg);
|
||||
readIMUData();
|
||||
StoreIMU_reset();
|
||||
|
||||
@ -311,7 +318,7 @@ void NavEKF2_core::CovarianceInit()
|
||||
P[7][7] = P[6][6];
|
||||
P[8][8] = sq(frontend->_baroAltNoise);
|
||||
// gyro delta angle biases
|
||||
P[9][9] = sq(radians(InitialGyroBiasUncertainty() * dtIMUavg));
|
||||
P[9][9] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg));
|
||||
P[10][10] = P[9][9];
|
||||
P[11][11] = P[9][9];
|
||||
// gyro scale factor biases
|
||||
@ -319,7 +326,7 @@ void NavEKF2_core::CovarianceInit()
|
||||
P[13][13] = P[12][12];
|
||||
P[14][14] = P[12][12];
|
||||
// Z delta velocity bias
|
||||
P[15][15] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtIMUavg);
|
||||
P[15][15] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtEkfAvg);
|
||||
// earth magnetic field
|
||||
P[16][16] = 0.0f;
|
||||
P[17][17] = P[16][16];
|
||||
@ -342,8 +349,11 @@ void NavEKF2_core::CovarianceInit()
|
||||
* UPDATE FUNCTIONS *
|
||||
********************************************************/
|
||||
// Update Filter States - this should be called whenever new IMU data is available
|
||||
void NavEKF2_core::UpdateFilter()
|
||||
void NavEKF2_core::UpdateFilter(bool predict)
|
||||
{
|
||||
// Set the flag to indicate to the filter that the front-end has given permission for a new state prediction cycle to be started
|
||||
startPredictEnabled = predict;
|
||||
|
||||
// zero the delta quaternion used by the strapdown navigation because it is published
|
||||
// and we need to return a zero rotation of the INS fails to update it
|
||||
correctedDelAngQuat.initialise();
|
||||
@ -370,41 +380,33 @@ void NavEKF2_core::UpdateFilter()
|
||||
// read IMU data as delta angles and velocities
|
||||
readIMUData();
|
||||
|
||||
// State Prediction Step
|
||||
// Run the strapdown INS equations to predict kinematic states forward to the fusion time horizon using buffered IMU data
|
||||
UpdateStrapdownEquationsNED();
|
||||
// Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the buffer
|
||||
if (runUpdates) {
|
||||
// Predict states using IMU data from the delayed time horizon
|
||||
UpdateStrapdownEquationsNED();
|
||||
|
||||
// sum delta angles and time used by covariance prediction
|
||||
summedDelAng = summedDelAng + correctedDelAng;
|
||||
summedDelVel = summedDelVel + correctedDelVel;
|
||||
dt += imuDataDelayed.delAngDT;
|
||||
|
||||
// perform a covariance prediction if the total delta angle has exceeded the limit
|
||||
// or the time limit will be exceeded at the next IMU update
|
||||
if (((dt >= (frontend->covTimeStepMax - dtIMUavg)) || (summedDelAng.length() > frontend->covDelAngMax))) {
|
||||
// Predict the covariance growth
|
||||
CovariancePrediction();
|
||||
} else {
|
||||
covPredStep = false;
|
||||
|
||||
// Read range finder data which is used by both position and optical flow fusion
|
||||
readRangeFinder();
|
||||
|
||||
// Update states using magnetometer data
|
||||
SelectMagFusion();
|
||||
|
||||
// Update states using GPS and altimeter data
|
||||
SelectVelPosFusion();
|
||||
|
||||
// Update states using optical flow data
|
||||
SelectFlowFusion();
|
||||
|
||||
// Update states using airspeed data
|
||||
SelectTasFusion();
|
||||
|
||||
// Update states using sideslip constraint assumption for fly-forward vehicles
|
||||
SelectBetaFusion();
|
||||
}
|
||||
|
||||
// Read range finder data which is used by both position and optical flow fusion
|
||||
readRangeFinder();
|
||||
|
||||
// Update states using magnetometer data
|
||||
SelectMagFusion();
|
||||
|
||||
// Update states using GPS and altimeter data
|
||||
SelectVelPosFusion();
|
||||
|
||||
// Update states using optical flow data
|
||||
SelectFlowFusion();
|
||||
|
||||
// Update states using airspeed data
|
||||
SelectTasFusion();
|
||||
|
||||
// Update states using sideslip constraint assumption for fly-forward vehicles
|
||||
SelectBetaFusion();
|
||||
|
||||
// Wind output forward from the fusion to output time horizon
|
||||
calcOutputStatesFast();
|
||||
|
||||
@ -424,21 +426,9 @@ void NavEKF2_core::UpdateFilter()
|
||||
*/
|
||||
void NavEKF2_core::UpdateStrapdownEquationsNED()
|
||||
{
|
||||
Vector3f delVelNav; // delta velocity vector
|
||||
|
||||
// remove gyro scale factor errors
|
||||
correctedDelAng.x = imuDataDelayed.delAng.x * stateStruct.gyro_scale.x;
|
||||
correctedDelAng.y = imuDataDelayed.delAng.y * stateStruct.gyro_scale.y;
|
||||
correctedDelAng.z = imuDataDelayed.delAng.z * stateStruct.gyro_scale.z;
|
||||
|
||||
// remove sensor bias errors
|
||||
correctedDelAng -= stateStruct.gyro_bias;
|
||||
correctedDelVel = imuDataDelayed.delVel;
|
||||
correctedDelVel.z -= stateStruct.accel_zbias;
|
||||
|
||||
// apply correction for earths rotation rate
|
||||
// % * - and + operators have been overloaded
|
||||
correctedDelAng = correctedDelAng - prevTnb * earthRateNED*imuDataDelayed.delAngDT;
|
||||
correctedDelAng = imuDataDelayed.delAng - prevTnb * earthRateNED*imuDataDelayed.delAngDT;
|
||||
|
||||
// convert the rotation vector to its equivalent quaternion
|
||||
correctedDelAngQuat.from_axis_angle(correctedDelAng);
|
||||
@ -455,7 +445,8 @@ void NavEKF2_core::UpdateStrapdownEquationsNED()
|
||||
|
||||
// transform body delta velocities to delta velocities in the nav frame
|
||||
// * and + operators have been overloaded
|
||||
delVelNav = Tbn_temp*correctedDelVel;
|
||||
Vector3f delVelNav; // delta velocity vector in earth axes
|
||||
delVelNav = Tbn_temp*imuDataDelayed.delVel;
|
||||
delVelNav.z += GRAVITY_MSS*imuDataDelayed.delVelDT;
|
||||
|
||||
// calculate the rate of change of velocity (used for launch detect and other functions)
|
||||
@ -479,8 +470,8 @@ void NavEKF2_core::UpdateStrapdownEquationsNED()
|
||||
stateStruct.position += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
|
||||
|
||||
// accumulate the bias delta angle and time since last reset by an OF measurement arrival
|
||||
delAngBodyOF += imuDataNew.delAng - stateStruct.gyro_bias;
|
||||
delTimeOF += imuDataNew.delAngDT;
|
||||
delAngBodyOF += imuDataDelayed.delAng - stateStruct.gyro_bias;
|
||||
delTimeOF += imuDataDelayed.delAngDT;
|
||||
|
||||
// limit states to protect against divergence
|
||||
ConstrainStates();
|
||||
@ -545,8 +536,10 @@ void NavEKF2_core::calcOutputStatesFast() {
|
||||
// apply a trapezoidal integration to velocities to calculate position, applying correction required to track EKF solution
|
||||
outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f) + velCorrection * imuDataNew.delVelDT;
|
||||
|
||||
// store the output in the FIFO buffer
|
||||
StoreOutput();
|
||||
// store the output in the FIFO buffer if this is a filter update step
|
||||
if (runUpdates) {
|
||||
StoreOutput();
|
||||
}
|
||||
|
||||
// extract data at the fusion time horizon from the FIFO buffer
|
||||
RecallOutput();
|
||||
@ -630,6 +623,7 @@ void NavEKF2_core::CovariancePrediction()
|
||||
// use filtered height rate to increase wind process noise when climbing or descending
|
||||
// this allows for wind gradient effects.
|
||||
// filter height rate using a 10 second time constant filter
|
||||
dt = imuDataDelayed.delAngDT;
|
||||
float alpha = 0.1f * dt;
|
||||
hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha;
|
||||
|
||||
@ -657,12 +651,12 @@ void NavEKF2_core::CovariancePrediction()
|
||||
for (uint8_t i= 0; i<=stateIndexLim; i++) processNoise[i] = sq(processNoise[i]);
|
||||
|
||||
// set variables used to calculate covariance growth
|
||||
dvx = summedDelVel.x;
|
||||
dvy = summedDelVel.y;
|
||||
dvz = summedDelVel.z;
|
||||
dax = summedDelAng.x;
|
||||
day = summedDelAng.y;
|
||||
daz = summedDelAng.z;
|
||||
dvx = imuDataDelayed.delVel.x;
|
||||
dvy = imuDataDelayed.delVel.y;
|
||||
dvz = imuDataDelayed.delVel.z;
|
||||
dax = imuDataDelayed.delAng.x;
|
||||
day = imuDataDelayed.delAng.y;
|
||||
daz = imuDataDelayed.delAng.z;
|
||||
q0 = stateStruct.quat[0];
|
||||
q1 = stateStruct.quat[1];
|
||||
q2 = stateStruct.quat[2];
|
||||
@ -1098,17 +1092,9 @@ void NavEKF2_core::CovariancePrediction()
|
||||
// constrain diagonals to prevent ill-conditioning
|
||||
ConstrainVariances();
|
||||
|
||||
// set the flag to indicate that covariance prediction has been performed and reset the increments used by the covariance prediction
|
||||
covPredStep = true;
|
||||
summedDelAng.zero();
|
||||
summedDelVel.zero();
|
||||
dt = 0.0f;
|
||||
|
||||
hal.util->perf_end(_perf_CovariancePrediction);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// zero specified range of rows in the state covariance matrix
|
||||
void NavEKF2_core::zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last)
|
||||
{
|
||||
@ -1218,9 +1204,9 @@ void NavEKF2_core::ConstrainVariances()
|
||||
for (uint8_t i=0; i<=2; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // attitude error
|
||||
for (uint8_t i=3; i<=5; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // velocities
|
||||
for (uint8_t i=6; i<=8; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f); // positions
|
||||
for (uint8_t i=9; i<=11; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtIMUavg)); // delta angle biases
|
||||
for (uint8_t i=9; i<=11; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtEkfAvg)); // delta angle biases
|
||||
for (uint8_t i=12; i<=14; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // delta angle scale factors
|
||||
P[15][15] = constrain_float(P[15][15],0.0f,sq(10.0f * dtIMUavg)); // delta velocity bias
|
||||
P[15][15] = constrain_float(P[15][15],0.0f,sq(10.0f * dtEkfAvg)); // delta velocity bias
|
||||
for (uint8_t i=16; i<=18; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // earth magnetic field
|
||||
for (uint8_t i=19; i<=21; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // body magnetic field
|
||||
for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // wind velocity
|
||||
@ -1238,11 +1224,11 @@ void NavEKF2_core::ConstrainStates()
|
||||
// height limit covers home alt on everest through to home alt at SL and ballon drop
|
||||
stateStruct.position.z = constrain_float(stateStruct.position.z,-4.0e4f,1.0e4f);
|
||||
// gyro bias limit (this needs to be set based on manufacturers specs)
|
||||
for (uint8_t i=9; i<=11; i++) statesArray[i] = constrain_float(statesArray[i],-GYRO_BIAS_LIMIT*dtIMUavg,GYRO_BIAS_LIMIT*dtIMUavg);
|
||||
for (uint8_t i=9; i<=11; i++) statesArray[i] = constrain_float(statesArray[i],-GYRO_BIAS_LIMIT*dtEkfAvg,GYRO_BIAS_LIMIT*dtEkfAvg);
|
||||
// gyro scale factor limit of +-5% (this needs to be set based on manufacturers specs)
|
||||
for (uint8_t i=12; i<=14; i++) statesArray[i] = constrain_float(statesArray[i],0.95f,1.05f);
|
||||
// Z accel bias limit 1.0 m/s^2 (this needs to be finalised from test data)
|
||||
stateStruct.accel_zbias = constrain_float(stateStruct.accel_zbias,-1.0f*dtIMUavg,1.0f*dtIMUavg);
|
||||
stateStruct.accel_zbias = constrain_float(stateStruct.accel_zbias,-1.0f*dtEkfAvg,1.0f*dtEkfAvg);
|
||||
// earth magnetic field limit
|
||||
for (uint8_t i=16; i<=18; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f);
|
||||
// body magnetic field limit
|
||||
|
||||
@ -50,7 +50,7 @@
|
||||
// Note that if using more than 2 instances of the EKF, as set by EK2_IMU_MASK, this delay should be increased by 2 samples
|
||||
// for each additional instance to allow for the need to offset the fusion time horizon for each instance to avoid simultaneous fusion
|
||||
// of measurements by each instance
|
||||
#define IMU_BUFFER_LENGTH 104 // maximum 260 msec delay at 400 Hz
|
||||
#define IMU_BUFFER_LENGTH 26 // maximum 260 msec delay at 100 Hz fusion rate
|
||||
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
|
||||
#define IMU_BUFFER_LENGTH 13 // maximum 260 msec delay at 50 Hz
|
||||
#elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
|
||||
@ -75,7 +75,8 @@ public:
|
||||
bool InitialiseFilterBootstrap(void);
|
||||
|
||||
// Update Filter States - this should be called whenever new IMU data is available
|
||||
void UpdateFilter(void);
|
||||
// The predict flag is set true when a new prediction cycle can be started
|
||||
void UpdateFilter(bool predict);
|
||||
|
||||
// Check basic filter health metrics and return a consolidated health status
|
||||
bool healthy(void) const;
|
||||
@ -272,6 +273,10 @@ public:
|
||||
// report any reason for why the backend is refusing to initialise
|
||||
const char *prearm_failure_reason(void) const;
|
||||
|
||||
// report the number of frames lapsed since the last state prediction
|
||||
// this is used by other instances to level load
|
||||
uint8_t getFramesSincePredict(void) const;
|
||||
|
||||
private:
|
||||
// Reference to the global EKF frontend for parameters
|
||||
NavEKF2 *frontend;
|
||||
@ -628,12 +633,8 @@ private:
|
||||
// using a simple observer
|
||||
void calcOutputStatesFast();
|
||||
|
||||
// Round to the nearest multiple of a integer
|
||||
uint32_t roundToNearest(uint32_t dividend, uint32_t divisor );
|
||||
|
||||
// Length of FIFO buffers used for non-IMU sensor data.
|
||||
// Must be larger than the maximum number of sensor samples that will arrive during the time period defined by IMU_BUFFER_LENGTH
|
||||
// OBS_BUFFER_LENGTH > IMU_BUFFER_LENGTH * dtIMUavg * 'max sensor rate'
|
||||
// Must be larger than the time period defined by IMU_BUFFER_LENGTH
|
||||
static const uint32_t OBS_BUFFER_LENGTH = 5;
|
||||
|
||||
// Variables
|
||||
@ -665,13 +666,12 @@ private:
|
||||
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
Quaternion correctedDelAngQuat; // quaternion representation of correctedDelAng
|
||||
Vector3f correctedDelVel; // delta velocities along the XYZ body axes for weighted average of IMU1 and IMU2 corrected for errors (m/s)
|
||||
Vector3f summedDelAng; // corrected & summed delta angles about the xyz body axes (rad)
|
||||
Vector3f summedDelVel; // corrected & summed delta velocities along the XYZ body axes (m/s)
|
||||
Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation
|
||||
ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
|
||||
ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2)
|
||||
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
|
||||
ftype dtIMUavg; // expected time between IMU measurements (sec)
|
||||
ftype dtEkfAvg; // expected time between EKF updates (sec)
|
||||
ftype dt; // time lapsed since the last covariance prediction (sec)
|
||||
ftype hgtRate; // state for rate of change of height filter
|
||||
bool onGround; // true when the flight vehicle is definitely on the ground
|
||||
@ -689,7 +689,6 @@ private:
|
||||
Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements
|
||||
ftype innovVtas; // innovation output from fusion of airspeed measurements
|
||||
ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements
|
||||
bool covPredStep; // boolean set to true when a covariance prediction step has been performed
|
||||
bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step
|
||||
bool magFuseRequired; // boolean set to true when magnetometer fusion will be perfomred in the next time step
|
||||
uint32_t prevTasStep_ms; // time stamp of last TAS fusion step
|
||||
@ -748,6 +747,8 @@ private:
|
||||
uint8_t stateIndexLim; // Max state index used during matrix and array operations
|
||||
imu_elements imuDataDelayed; // IMU data at the fusion time horizon
|
||||
imu_elements imuDataNew; // IMU data at the current time horizon
|
||||
imu_elements imuDataDownSampledNew; // IMU data at the current time horizon that has been downsampled to a 100Hz rate
|
||||
Quaternion imuQuatDownSampleNew; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame
|
||||
uint8_t fifoIndexNow; // Global index for inertial and output solution at current time horizon
|
||||
uint8_t fifoIndexDelayed; // Global index for inertial and output solution at delayed/fusion time horizon
|
||||
baro_elements baroDataNew; // Baro data at the current time horizon
|
||||
@ -786,9 +787,13 @@ private:
|
||||
uint32_t lastVelReset_ms; // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset
|
||||
float yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold
|
||||
Quaternion prevQuatMagReset; // Quaternion from the last time the magnetic field state reset condition test was performed
|
||||
uint8_t fusionHorizonOffset; // number of IMU samples that the fusion time horizon has been shifted forward to prevent multiple EKF instances fusing data at the same time
|
||||
uint8_t fusionHorizonOffset; // number of IMU samples that the fusion time horizon has been shifted to prevent multiple EKF instances fusing data at the same time
|
||||
float hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks
|
||||
uint8_t magSelectIndex; // Index of the magnetometer that is being used by the EKF
|
||||
bool runUpdates; // boolean true when the EKF updates can be run
|
||||
uint32_t framesSincePredict; // number of frames lapsed since EKF instance did a state prediction
|
||||
bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycele
|
||||
uint8_t localFilterTimeStep_ms; // average number of msec between filter updates
|
||||
|
||||
// variables used to calulate a vertical velocity that is kinematically consistent with the verical position
|
||||
float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.
|
||||
|
||||
Loading…
Reference in New Issue
Block a user