mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-23 09:08:30 -04:00
AP_NavEKF: Improve optical flow terrain height estimation
The two state auxiliary EKF has been replaced with a single state filter that only estimates terrain offset. The new filter fuses a optical flow line of sight rate scalar (length of the optical flow LOS rate vector) which provides a terrain offset estimate that is less affected by yaw errors. Estimation of focal length scale factor error in flight wasn't accurate enough and will be replaced with a pre-flight intrinsic sensor calibration procedure as the scale factor error does not change over time provided the lens assembly is not adjusted. AP_NavEKF: Remove unwanted printf
This commit is contained in:
priseborough
Andrew Tridgell
parent
d599fa588e
commit
8d1dae3ac1
@ -490,7 +490,7 @@ void NavEKF::ResetHeight(void)
|
||||
for (uint8_t i=0; i<=49; i++){
|
||||
storedStates[i].position.z = -hgtMea;
|
||||
}
|
||||
flowStates[1] = states[9] + 0.1f;
|
||||
terrainState = states[9] + 0.1f;
|
||||
}
|
||||
|
||||
// this function is used to initialise the filter whilst moving, using the AHRS DCM solution
|
||||
@ -900,9 +900,19 @@ void NavEKF::SelectMagFusion()
|
||||
// select fusion of optical flow measurements
|
||||
void NavEKF::SelectFlowFusion()
|
||||
{
|
||||
// Perform Data Checks
|
||||
// Check if the optical flow data is still valid
|
||||
flowDataValid = (imuSampleTime_ms - flowValidMeaTime_ms < 200);
|
||||
// if we don't have valid flow measurements and are not using GPS, dead reckon using current velocity vector unless we are in positon hold mode
|
||||
// check is the terrain offset estimate is still valid
|
||||
gndOffsetValid = (imuSampleTime_ms - gndHgtValidTime_ms < 5000);
|
||||
// Perform tilt check
|
||||
bool tiltOK = (Tnb_flow.c.z > DCM33FlowMin);
|
||||
// if we have waited too long, set a timeout flag which will force fusion to occur regardless of load spreading
|
||||
bool timeout = ((imuSampleTime_ms - prevFlowFusionTime_ms) >= flowIntervalMax_ms);
|
||||
// check if fusion should be delayed to spread load. Setting fuseMeNow to true disables this load spreading feature.
|
||||
bool delayFusion = ((covPredStep || magFusePerformed) && !(timeout || inhibitLoadLeveling));
|
||||
|
||||
// if we don't have valid flow measurements and are not using GPS, dead reckon using current velocity vector unless we are in position hold mode
|
||||
if (!flowDataValid && !constPosMode && PV_AidingMode == AID_RELATIVE) {
|
||||
constVelMode = true;
|
||||
constPosMode = false; // always clear constant position mode if constant velocity mode is active
|
||||
@ -911,12 +921,28 @@ void NavEKF::SelectFlowFusion()
|
||||
constVelMode = false;
|
||||
lastConstVelMode = false;
|
||||
}
|
||||
// Apply tilt check
|
||||
bool tiltOK = (Tnb_flow.c.z > DCM33FlowMin);
|
||||
// if we have waited too long, set a timeout flag which will force fusion to occur regardless of load spreading
|
||||
bool timeout = ((imuSampleTime_ms - prevFlowFusionTime_ms) >= flowIntervalMax_ms);
|
||||
// check if fusion should be delayed to spread load. Setting fuseMeNow to true disables this load spreading feature.
|
||||
bool delayFusion = ((covPredStep || magFusePerformed) && !(timeout || inhibitLoadLeveling));
|
||||
|
||||
// Fuse data into a 1-state EKF to estimate terrain height
|
||||
if ((newDataFlow || newDataRng) && tiltOK) {
|
||||
// fuse range data into the terrain estimator if available
|
||||
fuseRngData = newDataRng;
|
||||
// fuse optical flow data into the terrain estimator if available and if there is no range data (range data is better)
|
||||
fuseOptFlowData = (newDataFlow && !fuseRngData);
|
||||
// Estimate the terrain offset (runs a one state EKF)
|
||||
EstimateTerrainOffset();
|
||||
// Indicate we have used the range data
|
||||
newDataRng = false;
|
||||
// we don't do subsequent fusion of optical flow data into the main filter if GPS is good or terrain offset data is invalid
|
||||
if (!gpsNotAvailable || !gndOffsetValid) {
|
||||
// turn of fusion permissions
|
||||
// reset the measurement axis index
|
||||
flow_state.obsIndex = 0;
|
||||
// reset the flags to indicate that no new range finder or flow data is available for fusion
|
||||
newDataFlow = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Fuse optical flow data into the main filter
|
||||
// if the filter is initialised, we have data to fuse and the vehicle is not excessively tilted, then perform optical flow fusion
|
||||
if (flowDataValid && newDataFlow && tiltOK && !delayFusion && !constPosMode)
|
||||
{
|
||||
@ -925,10 +951,11 @@ void NavEKF::SelectFlowFusion()
|
||||
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
|
||||
// set the measurement axis index to fuse the X axis data
|
||||
flow_state.obsIndex = 0;
|
||||
// Fuse the optical flow X axis data into the main filter
|
||||
FuseOptFlow();
|
||||
// increment the index to fuse the Y axis data on the next prediction cycle
|
||||
// increment the measurement axis index to fuse the Y axis data on the next prediction cycle
|
||||
flow_state.obsIndex = 1;
|
||||
// reset flag to indicate that no new flow data is available for fusion
|
||||
newDataFlow = false;
|
||||
@ -936,34 +963,15 @@ void NavEKF::SelectFlowFusion()
|
||||
flowFusePerformed = true;
|
||||
// update the time stamp
|
||||
prevFlowFusionTime_ms = imuSampleTime_ms;
|
||||
|
||||
} else if (flowDataValid && flow_state.obsIndex == 1 && !delayFusion && !constPosMode && tiltOK){
|
||||
} else if (flowDataValid && flow_state.obsIndex == 1 && !delayFusion && !constPosMode && tiltOK) {
|
||||
// Fuse the optical flow Y axis data into the main filter
|
||||
FuseOptFlow();
|
||||
// increment the index to fuse the X and Y data using the 2-state EKF on the next prediction cycle
|
||||
flow_state.obsIndex = 2;
|
||||
// Reset the measurement axis index to prevent further fusion of this data
|
||||
flow_state.obsIndex = 0;
|
||||
// indicate that flow fusion has been performed. This is used for load spreading.
|
||||
flowFusePerformed = true;
|
||||
} else if (((flowDataValid && flow_state.obsIndex == 2) || newDataRng) && !constPosMode && tiltOK) {
|
||||
// enable fusion of range data if available and permitted
|
||||
if(newDataRng && useRngFinder()) {
|
||||
fuseRngData = true;
|
||||
} else {
|
||||
fuseRngData = false;
|
||||
}
|
||||
// the fact that we have got this far means we do have optical flow data
|
||||
fuseOptFlowData = true;
|
||||
// Estimate the focal length scale factor and terrain offset (runs a two state EKF)
|
||||
RunAuxiliaryEKF();
|
||||
// turn of fusion permissions
|
||||
// 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) {
|
||||
flowUpdateCount ++;
|
||||
@ -2542,252 +2550,177 @@ 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
|
||||
Estimation of terrain offset using a single state EKF
|
||||
The filter can fuse motion compensated optiocal flow rates and range finder measurements
|
||||
*/
|
||||
void NavEKF::RunAuxiliaryEKF()
|
||||
void NavEKF::EstimateTerrainOffset()
|
||||
{
|
||||
// start performance timer
|
||||
perf_begin(_perf_OpticalFlowEKF);
|
||||
|
||||
// calculate a predicted LOS rate squared
|
||||
float losRateSq = (sq(state.velocity.x) + sq(state.velocity.y)) / sq(flowStates[1] - state.position[2]);
|
||||
float velHorizSq = sq(state.velocity.x) + sq(state.velocity.y);
|
||||
float losRateSq = velHorizSq / sq(terrainState - state.position[2]);
|
||||
|
||||
// don't update terrain offset state if there is no range finder and not generating enough LOS rate, or without GPS, as it is poorly observable
|
||||
// record the time we last updated the terrain offset state
|
||||
if ((losRateSq < 0.01f || PV_AidingMode == AID_RELATIVE) && !fuseRngData) {
|
||||
if (!fuseRngData && (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f || onGround)) {
|
||||
inhibitGndState = true;
|
||||
} else {
|
||||
inhibitGndState = false;
|
||||
// record the time we last updated the terrain offset state
|
||||
gndHgtValidTime_ms = imuSampleTime_ms;
|
||||
}
|
||||
// Don't update focal length offset if there is no range finder or not using GPS, or we are not flying fast enough to generate a useful LOS rate
|
||||
if (!fuseRngData || PV_AidingMode == AID_RELATIVE || losRateSq < 0.01f || (flowStates[1] - state.position[2]) < 3.0f) {
|
||||
fScaleInhibit = true;
|
||||
} else {
|
||||
fScaleInhibit = false;
|
||||
}
|
||||
|
||||
// propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
|
||||
// limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
|
||||
if (!inhibitGndState) {
|
||||
float distanceTravelledSq;
|
||||
if (fuseRngData) {
|
||||
distanceTravelledSq = sq(statesAtRngTime.position[0] - prevPosN) + sq(statesAtRngTime.position[1] - prevPosE);
|
||||
prevPosN = statesAtRngTime.position[0];
|
||||
prevPosE = statesAtRngTime.position[1];
|
||||
} else if (fuseOptFlowData) {
|
||||
distanceTravelledSq = sq(statesAtFlowTime.position[0] - prevPosN) + sq(statesAtFlowTime.position[1] - prevPosE);
|
||||
prevPosN = statesAtFlowTime.position[0];
|
||||
prevPosE = statesAtFlowTime.position[1];
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
// propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
|
||||
// limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
|
||||
float distanceTravelledSq = sq(statesAtRngTime.position[0] - prevPosN) + sq(statesAtRngTime.position[1] - prevPosE);
|
||||
distanceTravelledSq = min(distanceTravelledSq, 100.0f);
|
||||
prevPosN = statesAtRngTime.position[0];
|
||||
prevPosE = statesAtRngTime.position[1];
|
||||
|
||||
// in addition to a terrain gradient error model, we also have a time based error growth that is scaled using the gradient parameter
|
||||
float timeLapsed = min(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
|
||||
Popt[1][1] += (distanceTravelledSq * sq(0.01f*float(_gndGradientSigma))) + sq(2.0f * timeLapsed);
|
||||
float Pincrement = (distanceTravelledSq * sq(0.01f*float(_gndGradientSigma))) + sq(float(_gndGradientSigma) * timeLapsed);
|
||||
Popt += Pincrement;
|
||||
timeAtLastAuxEKF_ms = imuSampleTime_ms;
|
||||
}
|
||||
|
||||
// fuse range finder data
|
||||
if (fuseRngData) {
|
||||
float range; // range from camera to centre of image
|
||||
float q0; // quaternion at optical flow measurement time
|
||||
float q1; // quaternion at optical flow measurement time
|
||||
float q2; // quaternion at optical flow measurement time
|
||||
float q3; // quaternion at optical flow measurement time
|
||||
float R_RNG = 0.5; // range measurement variance (m^2) TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
|
||||
// fuse range finder data
|
||||
if (fuseRngData) {
|
||||
// predict range
|
||||
float predRngMeas = max((terrainState - statesAtRngTime.position[2]),0.1f) / Tnb_flow.c.z;
|
||||
|
||||
// Copy required states to local variable names
|
||||
q0 = statesAtRngTime.quat[0];
|
||||
q1 = statesAtRngTime.quat[1];
|
||||
q2 = statesAtRngTime.quat[2];
|
||||
q3 = statesAtRngTime.quat[3];
|
||||
// Copy required states to local variable names
|
||||
float q0 = statesAtRngTime.quat[0]; // quaternion at optical flow measurement time
|
||||
float q1 = statesAtRngTime.quat[1]; // quaternion at optical flow measurement time
|
||||
float q2 = statesAtRngTime.quat[2]; // quaternion at optical flow measurement time
|
||||
float q3 = statesAtRngTime.quat[3]; // quaternion at optical flow measurement time
|
||||
|
||||
// calculate Kalman gains
|
||||
float SK_RNG[3];
|
||||
SK_RNG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
SK_RNG[1] = 1/(R_RNG + Popt[1][1]/sq(SK_RNG[0]));
|
||||
SK_RNG[2] = 1/SK_RNG[0];
|
||||
float K_RNG[2];
|
||||
if (!fScaleInhibit) {
|
||||
K_RNG[0] = Popt[0][1]*SK_RNG[1]*SK_RNG[2];
|
||||
} else {
|
||||
K_RNG[0] = 0.0f;
|
||||
}
|
||||
if (!inhibitGndState) {
|
||||
K_RNG[1] = Popt[1][1]*SK_RNG[1]*SK_RNG[2];
|
||||
} else {
|
||||
K_RNG[1] = 0.0f;
|
||||
}
|
||||
// Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
|
||||
float R_RNG = 0.5;
|
||||
|
||||
// Calculate the innovation variance for data logging
|
||||
varInnovRng = 1.0f/SK_RNG[1];
|
||||
// calculate Kalman gain
|
||||
float SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
float K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
|
||||
|
||||
// constrain terrain height to be below the vehicle
|
||||
flowStates[1] = max(flowStates[1], statesAtRngTime.position[2] + 0.1f);
|
||||
// Calculate the innovation variance for data logging
|
||||
varInnovRng = (R_RNG + Popt/sq(SK_RNG));
|
||||
|
||||
// estimate range to centre of image
|
||||
range = (flowStates[1] - statesAtRngTime.position[2]) * SK_RNG[2];
|
||||
// constrain terrain height to be below the vehicle
|
||||
terrainState = max(terrainState, statesAtRngTime.position[2] + 0.1f);
|
||||
|
||||
// Calculate the measurement innovation
|
||||
innovRng = range - rngMea;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxRngTestRatio = sq(innovRng) / (sq(_rngInnovGate) * varInnovRng);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if > 5Sigma
|
||||
if ((sq(innovRng)*SK_RNG[1]) < 25.0f)
|
||||
{
|
||||
// correct the state
|
||||
for (uint8_t i = 0; i < 2 ; i++) {
|
||||
flowStates[i] -= K_RNG[i] * innovRng;
|
||||
}
|
||||
// constrain the states
|
||||
flowStates[0] = constrain_float(flowStates[0], 0.8f, 1.25f);
|
||||
flowStates[1] = max(flowStates[1], statesAtRngTime.position[2] + 0.1f);
|
||||
|
||||
// correct the covariance matrix
|
||||
float nextPopt[2][2];
|
||||
nextPopt[0][0] = Popt[0][0] - (Popt[0][1]*Popt[1][0]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2];
|
||||
nextPopt[0][1] = Popt[0][1] - (Popt[0][1]*Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2];
|
||||
nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
|
||||
nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
|
||||
// prevent the state variances from becoming negative and maintain symmetry
|
||||
Popt[0][0] = max(nextPopt[0][0],0.0f);
|
||||
Popt[1][1] = max(nextPopt[1][1],0.0f);
|
||||
Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
|
||||
Popt[1][0] = Popt[0][1];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (fuseOptFlowData) {
|
||||
Vector3f vel; // velocity of sensor relative to ground in NED axes
|
||||
Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes
|
||||
float losPred[2]; // predicted optical flow angular rate measurements
|
||||
float range; // range from camera to centre of image
|
||||
float q0; // quaternion at optical flow measurement time
|
||||
float q1; // quaternion at optical flow measurement time
|
||||
float q2; // quaternion at optical flow measurement time
|
||||
float q3; // quaternion at optical flow measurement time
|
||||
float HP[2];
|
||||
float SH_OPT[6];
|
||||
float SK_OPT[3];
|
||||
float K_OPT[2][2];
|
||||
float H_OPT[2][2];
|
||||
float nextPopt[2][2];
|
||||
float SH015;
|
||||
float SH025;
|
||||
float SH014;
|
||||
float SH024;
|
||||
|
||||
// propagate scale factor state noise
|
||||
if (!fScaleInhibit) {
|
||||
Popt[0][0] += 1e-8f;
|
||||
} else {
|
||||
Popt[0][0] = 0.0f;
|
||||
}
|
||||
|
||||
// Copy required states to local variable names
|
||||
q0 = statesAtFlowTime.quat[0];
|
||||
q1 = statesAtFlowTime.quat[1];
|
||||
q2 = statesAtFlowTime.quat[2];
|
||||
q3 = statesAtFlowTime.quat[3];
|
||||
// Correct velocities for GPS glitch recovery offset
|
||||
vel.x = statesAtFlowTime.velocity[0] - gpsVelGlitchOffset.x;
|
||||
vel.y = statesAtFlowTime.velocity[1] - gpsVelGlitchOffset.y;
|
||||
vel.z = statesAtFlowTime.velocity[2];
|
||||
|
||||
// constrain terrain height to be below the vehicle
|
||||
flowStates[1] = max(flowStates[1], statesAtFlowTime.position[2] + 0.1f);
|
||||
|
||||
// estimate range to centre of image
|
||||
range = (flowStates[1] - statesAtFlowTime.position[2]) / Tnb_flow.c.z;
|
||||
|
||||
// calculate relative velocity in sensor frame
|
||||
relVelSensor = Tnb_flow*vel;
|
||||
|
||||
// divide velocity by range, subtract body rates and apply scale factor to
|
||||
// get predicted sensed angular optical rates relative to X and Y sensor axes
|
||||
losPred[0] = flowStates[0]*( relVelSensor.y/range) - omegaAcrossFlowTime.x;
|
||||
losPred[1] = flowStates[0]*(-relVelSensor.x/range) - omegaAcrossFlowTime.y;
|
||||
|
||||
// calculate innovations
|
||||
auxFlowObsInnov[0] = losPred[0] - flowRadXY[0];
|
||||
auxFlowObsInnov[1] = losPred[1] - flowRadXY[1];
|
||||
|
||||
// calculate Kalman gains
|
||||
SH_OPT[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
SH_OPT[1] = vel.x*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + vel.y*(2*q0*q3 + 2*q1*q2) - vel.z*(2*q0*q2 - 2*q1*q3);
|
||||
SH_OPT[2] = vel.y*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) - vel.x*(2*q0*q3 - 2*q1*q2) + vel.z*(2*q0*q1 + 2*q2*q3);
|
||||
SH_OPT[3] = statesAtFlowTime.position[2] - flowStates[1];
|
||||
SH_OPT[4] = 1.0f/sq(SH_OPT[3]);
|
||||
SH_OPT[5] = 1.0f/SH_OPT[3];
|
||||
SH015 = SH_OPT[0]*SH_OPT[1]*SH_OPT[5];
|
||||
SH025 = SH_OPT[0]*SH_OPT[2]*SH_OPT[5];
|
||||
SH014 = SH_OPT[0]*SH_OPT[1]*SH_OPT[4];
|
||||
SH024 = SH_OPT[0]*SH_OPT[2]*SH_OPT[4];
|
||||
SK_OPT[0] = 1.0f/(R_LOS + SH015*(Popt[0][0]*SH015 + Popt[1][0]*flowStates[0]*SH014) + flowStates[0]*SH014*(Popt[0][1]*SH015 + Popt[1][1]*flowStates[0]*SH014));
|
||||
SK_OPT[1] = 1.0f/(R_LOS + SH025*(Popt[0][0]*SH025 + Popt[1][0]*flowStates[0]*SH024) + flowStates[0]*SH024*(Popt[0][1]*SH025 + Popt[1][1]*flowStates[0]*SH024));
|
||||
SK_OPT[2] = SH_OPT[0];
|
||||
if (!fScaleInhibit) {
|
||||
K_OPT[0][0] = -SK_OPT[1]*(Popt[0][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]);
|
||||
K_OPT[0][1] = SK_OPT[0]*(Popt[0][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]);
|
||||
} else {
|
||||
K_OPT[0][0] = 0.0f;
|
||||
K_OPT[0][1] = 0.0f;
|
||||
}
|
||||
if (!inhibitGndState) {
|
||||
K_OPT[1][0] = -SK_OPT[1]*(Popt[1][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]);
|
||||
K_OPT[1][1] = SK_OPT[0]*(Popt[1][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]);
|
||||
} else {
|
||||
K_OPT[1][0] = 0.0f;
|
||||
K_OPT[1][1] = 0.0f;
|
||||
}
|
||||
|
||||
// calculate observations jacobians
|
||||
H_OPT[0][0] = -SH025;
|
||||
H_OPT[0][1] = -flowStates[0]*SH024;
|
||||
H_OPT[1][0] = SH015;
|
||||
H_OPT[1][1] = flowStates[0]*SH014;
|
||||
|
||||
// calculate innovation variances
|
||||
auxFlowObsInnovVar[0] = 1.0f/SK_OPT[1];
|
||||
auxFlowObsInnovVar[1] = 1.0f/SK_OPT[0];
|
||||
|
||||
// Check the innovation for consistency and don't fuse if > threshold
|
||||
for (uint8_t obsIndex = 0; obsIndex < 2; obsIndex++) {
|
||||
// Calculate the measurement innovation
|
||||
innovRng = predRngMeas - rngMea;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxFlowTestRatio[obsIndex] = sq(auxFlowObsInnov[obsIndex]) / (sq(_flowInnovGate) * auxFlowObsInnovVar[obsIndex]);
|
||||
auxRngTestRatio = sq(innovRng) / (sq(_rngInnovGate) * varInnovRng);
|
||||
|
||||
if ((auxFlowTestRatio[obsIndex] < 1.0f) && (flowRadXY[obsIndex] < _maxFlowRate)) {
|
||||
// Check the innovation for consistency and don't fuse if > 5Sigma
|
||||
if ((sq(innovRng)*SK_RNG) < 25.0f)
|
||||
{
|
||||
// correct the state
|
||||
for (uint8_t i = 0; i < 2 ; i++) {
|
||||
flowStates[i] -= K_OPT[i][obsIndex] * auxFlowObsInnov[obsIndex];
|
||||
}
|
||||
// constrain the states
|
||||
flowStates[0] = constrain_float(flowStates[0], 0.8f, 1.25f);
|
||||
flowStates[1] = max(flowStates[1], statesAtFlowTime.position[2] + 0.1f);
|
||||
terrainState -= K_RNG * innovRng;
|
||||
|
||||
// correct the covariance matrix
|
||||
for (uint8_t i = 0; i < 2 ; i++) {
|
||||
HP[i] = 0.0f;
|
||||
for (uint8_t j = 0; j < 2 ; j++) {
|
||||
HP[i] += H_OPT[obsIndex][j] * P[j][i];
|
||||
}
|
||||
}
|
||||
for (uint8_t i = 0; i < 2 ; i++) {
|
||||
for (uint8_t j = 0; j < 2 ; j++) {
|
||||
nextPopt[i][j] = P[i][j] - K_OPT[i][obsIndex] * HP[j];
|
||||
}
|
||||
}
|
||||
// constrain the state
|
||||
terrainState = max(terrainState, statesAtRngTime.position[2] + 0.1f);
|
||||
|
||||
// prevent the state variances from becoming negative and maintain symmetry
|
||||
Popt[0][0] = max(nextPopt[0][0],0.0f);
|
||||
Popt[1][1] = max(nextPopt[1][1],0.0f);
|
||||
Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
|
||||
Popt[1][0] = Popt[0][1];
|
||||
// correct the covariance
|
||||
Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
|
||||
|
||||
// prevent the state variance from becoming negative
|
||||
Popt = max(Popt,0.0f);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
if (fuseOptFlowData) {
|
||||
|
||||
Vector3f vel; // velocity of sensor relative to ground in NED axes
|
||||
Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes
|
||||
float losPred; // predicted optical flow angular rate measurement
|
||||
float q0 = statesAtFlowTime.quat[0]; // quaternion at optical flow measurement time
|
||||
float q1 = statesAtFlowTime.quat[1]; // quaternion at optical flow measurement time
|
||||
float q2 = statesAtFlowTime.quat[2]; // quaternion at optical flow measurement time
|
||||
float q3 = statesAtFlowTime.quat[3]; // quaternion at optical flow measurement time
|
||||
float K_OPT;
|
||||
float H_OPT;
|
||||
|
||||
// Correct velocities for GPS glitch recovery offset
|
||||
vel.x = statesAtFlowTime.velocity[0] - gpsVelGlitchOffset.x;
|
||||
vel.y = statesAtFlowTime.velocity[1] - gpsVelGlitchOffset.y;
|
||||
vel.z = statesAtFlowTime.velocity[2];
|
||||
|
||||
// predict range to centre of image
|
||||
float flowRngPred = max((terrainState - statesAtFlowTime.position[2]),0.1f) / Tnb_flow.c.z;
|
||||
|
||||
// constrain terrain height to be below the vehicle
|
||||
terrainState = max(terrainState, statesAtFlowTime.position[2] + 0.1f);
|
||||
|
||||
// calculate relative velocity in sensor frame
|
||||
relVelSensor = Tnb_flow*vel;
|
||||
|
||||
// divide velocity by range, subtract body rates and apply scale factor to
|
||||
// get predicted sensed angular optical rates relative to X and Y sensor axes
|
||||
losPred = relVelSensor.length()/flowRngPred;
|
||||
|
||||
// calculate innovations
|
||||
auxFlowObsInnov = losPred - sqrtf(sq(flowRadXYcomp[0]) + sq(flowRadXYcomp[1]));
|
||||
|
||||
// calculate observation jacobian
|
||||
float t3 = sq(q0);
|
||||
float t4 = sq(q1);
|
||||
float t5 = sq(q2);
|
||||
float t6 = sq(q3);
|
||||
float t10 = q0*q3*2.0f;
|
||||
float t11 = q1*q2*2.0f;
|
||||
float t14 = t3+t4-t5-t6;
|
||||
float t15 = t14*vel.x;
|
||||
float t16 = t10+t11;
|
||||
float t17 = t16*vel.y;
|
||||
float t18 = q0*q2*2.0f;
|
||||
float t19 = q1*q3*2.0f;
|
||||
float t20 = t18-t19;
|
||||
float t21 = t20*vel.z;
|
||||
float t2 = t15+t17-t21;
|
||||
float t7 = t3-t4-t5+t6;
|
||||
float t8 = statesAtFlowTime.position[2]-terrainState;
|
||||
float t9 = 1.0f/sq(t8);
|
||||
float t24 = t3-t4+t5-t6;
|
||||
float t25 = t24*vel.y;
|
||||
float t26 = t10-t11;
|
||||
float t27 = t26*vel.x;
|
||||
float t28 = q0*q1*2.0;
|
||||
float t29 = q2*q3*2.0;
|
||||
float t30 = t28+t29;
|
||||
float t31 = t30*vel.z;
|
||||
float t12 = t25-t27+t31;
|
||||
float t13 = sq(t7);
|
||||
float t22 = sq(t2);
|
||||
float t23 = 1.0f/(t8*t8*t8);
|
||||
float t32 = sq(t12);
|
||||
H_OPT = 0.5f*(t13*t22*t23*2.0+t13*t23*t32*2.0)/sqrt(t9*t13*t22+t9*t13*t32);
|
||||
|
||||
// calculate innovation variances
|
||||
auxFlowObsInnovVar = H_OPT*Popt*H_OPT + R_LOS;
|
||||
|
||||
// calculate Kalman gain
|
||||
K_OPT = Popt*H_OPT/auxFlowObsInnovVar;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(_flowInnovGate) * auxFlowObsInnovVar);
|
||||
|
||||
// don't fuse if optical flow data is outside valid range
|
||||
if (max(flowRadXY[0],flowRadXY[1]) < _maxFlowRate) {
|
||||
|
||||
// correct the state
|
||||
terrainState -= K_OPT * auxFlowObsInnov;
|
||||
|
||||
// constrain the state
|
||||
terrainState = max(terrainState, statesAtFlowTime.position[2] + 0.1f);
|
||||
|
||||
// correct the covariance
|
||||
Popt = Popt - K_OPT * H_OPT * Popt;
|
||||
|
||||
// prevent the state variances from becoming negative
|
||||
Popt = max(Popt,0.0f);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -2834,8 +2767,8 @@ void NavEKF::FuseOptFlow()
|
||||
velNED_local.z = vd;
|
||||
|
||||
// constrain terrain to be below vehicle
|
||||
flowStates[1] = max(flowStates[1], pd + 0.1f);
|
||||
float heightAboveGndEst = flowStates[1] - pd;
|
||||
terrainState = max(terrainState, pd + 0.1f);
|
||||
float heightAboveGndEst = terrainState - pd;
|
||||
// Calculate observation jacobians and Kalman gains
|
||||
if (obsIndex == 0) {
|
||||
// calculate range from ground plain to centre of sensor fov assuming flat earth
|
||||
@ -2853,7 +2786,7 @@ void NavEKF::FuseOptFlow()
|
||||
SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2);
|
||||
SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2);
|
||||
SH_LOS[3] = 1/(pd - flowStates[1]);
|
||||
SH_LOS[3] = 1/(pd - terrainState);
|
||||
SH_LOS[4] = sq(SH_LOS[3]);
|
||||
|
||||
// Calculate common expressions for Kalman gains
|
||||
@ -3558,7 +3491,7 @@ bool NavEKF::getPosNED(Vector3f &pos) const
|
||||
pos.y = state.position.y;
|
||||
// If relying on optical flow, then output ground relative position so that the vehicle does terain following
|
||||
if (_fusionModeGPS == 3) {
|
||||
pos.z = state.position.z - flowStates[1];
|
||||
pos.z = state.position.z - terrainState;
|
||||
} else {
|
||||
pos.z = state.position.z;
|
||||
}
|
||||
@ -3613,9 +3546,9 @@ void NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScal
|
||||
{
|
||||
if (flowDataValid || PV_AidingMode == AID_RELATIVE) {
|
||||
// allow 1.0 rad/sec margin for angular motion
|
||||
ekfGndSpdLimit = max((_maxFlowRate - 1.0f), 0.0f) * max((flowStates[1] - state.position[2]), 0.1f);
|
||||
ekfGndSpdLimit = max((_maxFlowRate - 1.0f), 0.0f) * max((terrainState - state.position[2]), 0.1f);
|
||||
// use standard gains up to 5.0 metres height and reduce above that
|
||||
ekfNavVelGainScaler = 4.0f / max((flowStates[1] - state.position[2]),4.0f);
|
||||
ekfNavVelGainScaler = 4.0f / max((terrainState - state.position[2]),4.0f);
|
||||
} else {
|
||||
ekfGndSpdLimit = 400.0f; //return 80% of max filter speed
|
||||
ekfNavVelGainScaler = 1.0f;
|
||||
@ -3674,22 +3607,22 @@ bool NavEKF::getLLH(struct Location &loc) const
|
||||
// return the estimated height above ground level
|
||||
bool NavEKF::getHAGL(float &HAGL) const
|
||||
{
|
||||
HAGL = flowStates[1] - state.position.z;
|
||||
HAGL = terrainState - state.position.z;
|
||||
return !inhibitGndState;
|
||||
}
|
||||
|
||||
// return data for debugging optical flow fusion
|
||||
void NavEKF::getFlowDebug(float &scaleFactor, float &estHAGL, float &flowInnovX, float &flowInnovY, float &flowVarX, float &flowVarY, float &rngInnov, float &range, float &gndOffsetErr) const
|
||||
void NavEKF::getFlowDebug(float &dummy, float &estHAGL, float &flowInnovX, float &flowInnovY, float &flowVarX, float &flowVarY, float &rngInnov, float &range, float &gndOffsetErr) const
|
||||
{
|
||||
scaleFactor = flowStates[0];
|
||||
estHAGL = flowStates[1] - state.position.z;
|
||||
dummy = 0.0f;
|
||||
estHAGL = terrainState - state.position.z;
|
||||
flowInnovX = innovOptFlow[0];
|
||||
flowInnovY = innovOptFlow[1];
|
||||
flowVarX = flowTestRatio[0];
|
||||
flowVarY = flowTestRatio[1];
|
||||
auxInnovX = auxFlowObsInnov;
|
||||
auxInnovY = auxFlowObsInnov;
|
||||
rngInnov = innovRng;
|
||||
range = rngMea;
|
||||
gndOffsetErr = sqrtf(Popt[1][1]); // note Popt[1][1] is constrained to be non-negative in RunAuxiliaryEKF()
|
||||
gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset()
|
||||
}
|
||||
|
||||
// calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed
|
||||
@ -3826,10 +3759,8 @@ void NavEKF::CovarianceInit()
|
||||
P[20][20] = P[19][19];
|
||||
P[21][21] = P[19][19];
|
||||
|
||||
// optical flow focal length scale factor
|
||||
Popt[0][0] = 0.0f;
|
||||
// ground height
|
||||
Popt[0][0] = 0.25f;
|
||||
// optical flow ground height covariance
|
||||
Popt = 0.25f;
|
||||
|
||||
}
|
||||
|
||||
@ -4092,9 +4023,9 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
|
||||
// note correction for different axis and sign conventions used by the px4flow sensor
|
||||
flowRadXY[0] = - rawFlowRates.x; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
|
||||
flowRadXY[1] = - rawFlowRates.y; // raw (non motion compensated) optical flow angular rate about the Y axis (rad/sec)
|
||||
// write flow rate measurements corrected for focal length scale factor errors and body rates
|
||||
flowRadXYcomp[0] = flowStates[0]*flowRadXY[0] + omegaAcrossFlowTime.x;
|
||||
flowRadXYcomp[1] = flowStates[0]*flowRadXY[1] + omegaAcrossFlowTime.y;
|
||||
// write flow rate measurements corrected for body rates
|
||||
flowRadXYcomp[0] = flowRadXY[0] + omegaAcrossFlowTime.x;
|
||||
flowRadXYcomp[1] = flowRadXY[1] + omegaAcrossFlowTime.y;
|
||||
// set flag that will trigger observations
|
||||
newDataFlow = true;
|
||||
flowValidMeaTime_ms = imuSampleTime_ms;
|
||||
@ -4363,9 +4294,8 @@ void NavEKF::InitialiseVariables()
|
||||
newDataRng = false;
|
||||
flowFusePerformed = false;
|
||||
fuseOptFlowData = false;
|
||||
memset(&Popt[0][0], 0, sizeof(Popt));
|
||||
flowStates[0] = 1.0f;
|
||||
flowStates[1] = 0.0f;
|
||||
Popt = 0.0f;
|
||||
terrainState = 0.0f;
|
||||
prevPosN = gpsPosNE.x;
|
||||
prevPosE = gpsPosNE.y;
|
||||
fuseRngData = false;
|
||||
@ -4390,6 +4320,7 @@ void NavEKF::InitialiseVariables()
|
||||
yawAligned = false;
|
||||
inhibitWindStates = true;
|
||||
inhibitMagStates = true;
|
||||
gndOffsetValid = false;
|
||||
}
|
||||
|
||||
// return true if we should use the airspeed sensor
|
||||
@ -4531,7 +4462,6 @@ void NavEKF::getFilterStatus(uint8_t &status) const
|
||||
bool notDeadReckoning = !constVelMode && !constPosMode;
|
||||
bool someVertRefData = (!velTimeout && (_fusionModeGPS == 0)) || !hgtTimeout;
|
||||
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
|
||||
bool gndOffsetValid = (imuSampleTime_ms - gndHgtValidTime_ms < 5000);
|
||||
status = (!state.quat.is_nan()<<0 | // attitude valid (we need a better check)
|
||||
(someHorizRefData && notDeadReckoning)<<1 | // horizontal velocity estimate valid
|
||||
someVertRefData<<2 | // vertical velocity estimate valid
|
||||
|
||||
@ -377,8 +377,8 @@ private:
|
||||
// this is useful for motion compensation of optical flow measurements
|
||||
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 RunAuxiliaryEKF();
|
||||
// Estimate terrain offset using a single state EKF
|
||||
void EstimateTerrainOffset();
|
||||
|
||||
// fuse optical flow measurements into the main filter
|
||||
void FuseOptFlow();
|
||||
@ -600,8 +600,8 @@ private:
|
||||
bool flowDataValid; // true while optical flow data is still fresh
|
||||
state_elements statesAtFlowTime;// States at the middle of the optical flow sample period
|
||||
bool fuseOptFlowData; // this boolean causes the last optical flow measurement to be fused
|
||||
float auxFlowObsInnov[2]; // optical flow observation innovations from 2-state focal length scale factor and terrain offset estimator
|
||||
float auxFlowObsInnovVar[2]; // innovation variance for optical flow observations from 2-state focal length scale factor and terrain offset estimator
|
||||
float auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
|
||||
float auxFlowObsInnovVar; // innovation variance for optical flow observations from 1-state terrain offset estimator
|
||||
float flowRadXYcomp[2]; // motion compensated optical flow angular rates(rad/sec)
|
||||
float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec)
|
||||
uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
|
||||
@ -613,8 +613,8 @@ private:
|
||||
Matrix3f Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period
|
||||
float varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2
|
||||
float innovOptFlow[2]; // optical flow LOS innovations (rad/sec)
|
||||
float Popt[2][2]; // state covariance matrix
|
||||
float flowStates[2]; // flow states [scale factor, terrain position]
|
||||
float Popt; // Optical flow terrain height state covariance (m^2)
|
||||
float terrainState; // terrain position state (m)
|
||||
float prevPosN; // north position at last measurement
|
||||
float prevPosE; // east position at last measurement
|
||||
state_elements statesAtRngTime; // States at the range finder measurement time
|
||||
@ -624,9 +624,8 @@ private:
|
||||
float rngMea; // range finder measurement (m)
|
||||
bool inhibitGndState; // true when the terrain position state is to remain constant
|
||||
uint32_t prevFlowFusionTime_ms; // time the last flow measurement was fused
|
||||
bool fScaleInhibit; // true when the focal length scale factor is to remain constant
|
||||
float flowTestRatio[2]; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
|
||||
float auxFlowTestRatio[2]; // sum of squares of optical flow innovations divided by fail threshold used by aux filter
|
||||
float auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
|
||||
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
|
||||
@ -643,6 +642,7 @@ private:
|
||||
AID_RELATIVE=2 // only optical flow aiding is being used so position estimates will be relative
|
||||
};
|
||||
AidingMode PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS
|
||||
bool gndOffsetValid; // true when the ground offset state can still be considered valid
|
||||
|
||||
// states held by optical flow fusion across time steps
|
||||
// optical flow X,Y motion compensated rate measurements are fused across two time steps
|
||||
|
||||
Loading…
Reference in New Issue
Block a user