Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Add range finder health and enable range finder fusion

Browse Source 
The 2-state auxiliary filter is also renamed.
This commit is contained in:
priseborough 2014-09-10 19:31:51 +10:00 committed by Andrew Tridgell
parent a4984a1e76
commit 235b3bebda
2 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -921,13 +921,15 @@ void NavEKF::SelectFlowFusion()
// indicate that flow fusion has been performed. This is used for load spreading.
flowFusePerformed = true;
} else if (flow_state.obsIndex == 2) {
// Estimate the focal length scale factor (runs a two state EKF)
OpticalFlowEKF();
// Estimate the focal length scale factor and terrain offset (runs a two state EKF)
RunAuxiliaryEKF();
// increment the index so that no further flow fusion is performed using this measurement
flow_state.obsIndex = 3;
// clear the flag indicating that flow fusion has been performed. The 2-state fusion is relatively computationally
// cheap and can be perfomred on the same prediction cycle with subsequent fusion steps
flowFusePerformed = false;
// reset the flag to indicate that no new range finder data is available for fusion
newDataRng = false;
}
// Apply corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output
if (flowUpdateCount < flowUpdateCountMax) {
@ -2503,7 +2505,7 @@ void NavEKF::FuseMagnetometer()
Estimation of optical flow sensor focal length scale factor and terrain offset using a two state EKF
This fiter requires optical flow rates that are not motion compensated
*/
void NavEKF::OpticalFlowEKF()
void NavEKF::RunAuxiliaryEKF()
{
// start performance timer
perf_begin(_perf_OpticalFlowEKF);
@ -3937,7 +3939,7 @@ void NavEKF::readAirSpdData()
// write the raw optical flow measurements
// this needs to be called externally.
void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, float &rawSonarRange, uint32_t &msecFlowMeas)
void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, uint8_t &rangeHealth, float &rawSonarRange)
{
// The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
// The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
@ -3974,6 +3976,13 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
} else {
newDataFlow = false;
}
// Use range finder if 3 or more consecutive good samples. This reduces likelihood of using bad data.
if (rangeHealth >= 3) {
rngMea = rawSonarRange;
newDataRng = true;
} else {
newDataRng = false;
}
}
// calculate the NED earth spin vector in rad/sec
@ -4216,6 +4225,7 @@ void NavEKF::ZeroVariables()
gpsPosGlitchOffsetNE.zero();
// optical flow variables
newDataFlow = false;
newDataRng = false;
flowFusePerformed = false;
fuseOptFlowData = false;
flowMeaTime_ms = imuSampleTime_ms;

5
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -160,7 +160,7 @@ public:
// rawGyroRates are the sensor phsyical rotation rates measured by the internal gyro
// rawSonarRange is the range in metres measured by the px4flow sensor
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, float &rawSonarRange, uint32_t &msecFlowMeas);
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, uint8_t &rangeHealth, float &rawSonarRange);
// return data for debugging optical flow fusion
void getFlowDebug(float &scaleFactor, float &obsX, float &obsY, float &innovX, float &innovY, float &gndPos, uint8_t &quality) const;
@ -347,7 +347,7 @@ private:
void RecallOmega(Vector3f &omegaAvg, uint32_t msecStart, uint32_t msecEnd);
// Estimate optical flow focal length scale factor and terrain offset using a 2-state EKF
void OpticalFlowEKF();
void RunAuxiliaryEKF();
// fuse optical flow measurements into the main filter
void FuseOptFlow();
@ -585,6 +585,7 @@ private:
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
bool newDataRng; // true when new valid range finder data has arrived.
// states held by optical flow fusion across time steps
// optical flow X,Y motion compensated rate measurements are fused across two time steps