Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Fix bug in optical flow fusion smoothing

This commit is contained in:
priseborough 2014-11-05 20:05:07 +11:00 committed by Andrew Tridgell
parent 7d27a22f77
commit 0bf991eef9
2 changed files with 16 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -397,6 +397,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
_msecMagAvg = 100; // average number of msec between magnetometer measurements
_msecBetaAvg = 100; // average number of msec between synthetic sideslip measurements
_msecBetaMax = 200; // maximum number of msec between synthetic sideslip measurements
_msecFlowAvg = 100; // average number of msec between optical flow measurements
dtVelPos = 0.02; // number of seconds between position and velocity corrections. This should be a multiple of the imu update interval.
// Misc initial conditions
@ -545,7 +546,7 @@ void NavEKF::InitialiseFilterDynamic(void)
dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f);
dtIMUinv = 1.0f / dtIMU;
// If we have a high rate update step of >100 Hz, then there may not enough time to all the fusion steps at once, so load levelling is used
// If we have a high rate update step of >100 Hz, then there may not be enough time to do all the fusion steps at once, so load levelling is used
if (dtIMU < 0.009f) {
inhibitLoadLeveling = false;
} else {
@ -558,6 +559,8 @@ void NavEKF::InitialiseFilterDynamic(void)
hgtUpdateCountMax = uint8_t(1.0f/hgtUpdateCountMaxInv);
magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecMagAvg);
magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv);
flowUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecFlowAvg);
flowUpdateCountMax = uint8_t(1.0f/flowUpdateCountMaxInv);
// calculate initial orientation and earth magnetic field states
state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
@ -931,6 +934,9 @@ void NavEKF::SelectFlowFusion()
// if the filter is initialised, we have data to fuse and the vehicle is not excessively tilted, then perform optical flow fusion
if (useOptFlow() && newDataFlow && tiltOK && !delayFusion && !staticMode)
{
// reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
memset(&flowIncrStateDelta[0], 0, sizeof(flowIncrStateDelta));
flowUpdateCount = 0;
// Set the flow noise used by the fusion processes
R_LOS = sq(max(_flowNoise, 0.05f));
// Fuse the optical flow X axis data into the main filter
@ -944,6 +950,7 @@ void NavEKF::SelectFlowFusion()
flowFusePerformed = true;
// update the time stamp
prevFlowFusionTime_ms = imuSampleTime_ms;
} else if (flow_state.obsIndex == 1 && !delayFusion){
// Fuse the optical flow Y axis data into the main filter
FuseOptFlow();
@ -3020,10 +3027,10 @@ void NavEKF::FuseOptFlow()
}
// Attitude, velocity and position corrections are averaged across multiple prediction cycles between now and the anticipated time for the next measurement.
// Don't do averaging of quaternion state corrections if total angle change across predicted interval is going to exceed 0.1 rad
bool highRates = ((magUpdateCountMax * correctedDelAng.length()) > 0.1f);
// Calculate the number of averaging frames left to go. This is required becasue magnetometer fusion is applied across three consecutive prediction cycles
bool highRates = ((flowUpdateCountMax * correctedDelAng.length()) > 0.1f);
// Calculate the number of averaging frames left to go. This is required because flow fusion is applied across two consecutive prediction cycles
// There is no point averaging if the number of cycles left is less than 2
float minorFramesToGo = float(magUpdateCountMax) - float(magUpdateCount);
float minorFramesToGo = float(flowUpdateCountMax) - float(flowUpdateCount);
for (uint8_t i = 0; i<=21; i++) {
if ((i <= 3 && highRates) || i >= 10 || minorFramesToGo < 1.5f) {
states[i] = states[i] - Kfusion[i] * innovOptFlow[obsIndex];
@ -4292,6 +4299,7 @@ void NavEKF::ZeroVariables()
memset(&gpsIncrStateDelta[0], 0, sizeof(gpsIncrStateDelta));
memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta));
memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta));
memset(&flowIncrStateDelta[0], 0, sizeof(flowIncrStateDelta));
gpsPosGlitchOffsetNE.zero();
// optical flow variables
newDataFlow = false;

5
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -545,7 +545,7 @@ private:
// Used by smoothing of state corrections
float gpsIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement
float hgtIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next height measurement
float flowIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next magnetometer measurement
float magIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next magnetometer measurement
uint8_t gpsUpdateCount; // count of the number of minor state corrections using GPS data
uint8_t gpsUpdateCountMax; // limit on the number of minor state corrections using GPS data
float gpsUpdateCountMaxInv; // floating point inverse of gpsFilterCountMax
@ -595,14 +595,15 @@ private:
float R_LOS; // variance of optical flow rate measurements (rad/sec)^2
float auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
Vector2f flowGyroBias; // bias error of optical flow sensor gyro output
float magIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next magnetometer measurement
uint8_t flowUpdateCount; // count of the number of minor state corrections using optical flow data
uint8_t flowUpdateCountMax; // limit on the number of minor state corrections using optical flow data
float flowUpdateCountMaxInv; // floating point inverse of flowUpdateCountMax
float flowIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next magnetometer measurement
bool newDataRng; // true when new valid range finder data has arrived.
bool velHoldMode; // true when holding velocity in optical flow mode when no flow measurements are available
bool lastHoldVelocity; // last value of holdVelocity
Vector2f heldVelNE; // velocity held when no aiding is available
uint16_t _msecFlowAvg; // average number of msec between synthetic sideslip measurements
// states held by optical flow fusion across time steps
// optical flow X,Y motion compensated rate measurements are fused across two time steps