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

AP_NavEKF3: rename varInnovOptFlow to flowVarInnov 

also renamed innovOptFlow to flowInnov
This commit is contained in:
Randy Mackay 2021-08-16 22:00:52 +09:00
parent 85ade10e85
commit d1f2acd813
4 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -187,8 +187,8 @@ void NavEKF3_core::Log_Write_XKF5(uint64_t time_us) const
time_us : time_us,
core : DAL_CORE(core_index),
normInnov : (uint8_t)(MIN(100*MAX(flowTestRatio[0],flowTestRatio[1]),255)), // normalised innovation variance ratio for optical flow observations fused by the main nav filter
FIX : (int16_t)(1000*innovOptFlow[0]), // optical flow LOS rate vector innovations from the main nav filter
FIY : (int16_t)(1000*innovOptFlow[1]), // optical flow LOS rate vector innovations from the main nav filter
FIX : (int16_t)(1000*flowInnov[0]), // optical flow LOS rate vector innovations from the main nav filter
FIY : (int16_t)(1000*flowInnov[1]), // optical flow LOS rate vector innovations from the main nav filter
AFI : (int16_t)(1000*norm(auxFlowObsInnov.x,auxFlowObsInnov.y)), // optical flow LOS rate innovation from terrain offset estimator
HAGL : (int16_t)(100*(terrainState - stateStruct.position.z)), // height above ground level
offset : (int16_t)(100*terrainState), // filter ground offset state error

12
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -424,11 +424,11 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
faultStatus.bad_xflow = true;
return;
}
varInnovOptFlow[0] = t77;
flowVarInnov[0] = t77;
// calculate innovation for X axis observation
// flowInnovTime_ms will be updated when Y-axis innovations are calculated
innovOptFlow[0] = losPred[0] - ofDataDelayed.flowRadXYcomp.x;
flowInnov[0] = losPred[0] - ofDataDelayed.flowRadXYcomp.x;
// calculate Kalman gains for X-axis observation
Kfusion[0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27);
@ -601,10 +601,10 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
faultStatus.bad_yflow = true;
return;
}
varInnovOptFlow[1] = t77;
flowVarInnov[1] = t77;
// calculate innovation for Y observation
innovOptFlow[1] = losPred[1] - ofDataDelayed.flowRadXYcomp.y;
flowInnov[1] = losPred[1] - ofDataDelayed.flowRadXYcomp.y;
flowInnovTime_ms = AP_HAL::millis();
// calculate Kalman gains for the Y-axis observation
@ -664,7 +664,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
}
// calculate the innovation consistency test ratio
flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]);
flowTestRatio[obsIndex] = sq(flowInnov[obsIndex]) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * flowVarInnov[obsIndex]);
// Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
if (really_fuse && (flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {
@ -722,7 +722,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
// correct the state vector
for (uint8_t j= 0; j<=stateIndexLim; j++) {
statesArray[j] = statesArray[j] - Kfusion[j] * innovOptFlow[obsIndex];
statesArray[j] = statesArray[j] - Kfusion[j] * flowInnov[obsIndex];
}
stateStruct.quat.normalize();

8
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -482,11 +482,11 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour
if (AP_HAL::millis() - flowInnovTime_ms > 500) {
return false;
}
innovations.x = innovOptFlow[0];
innovations.y = innovOptFlow[1];
innovations.x = flowInnov[0];
innovations.y = flowInnov[1];
innovations.z = 0;
variances.x = varInnovOptFlow[0];
variances.y = varInnovOptFlow[1];
variances.x = flowVarInnov[0];
variances.y = flowVarInnov[1];
variances.z = 0;
return true;
default:

4
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1170,8 +1170,8 @@ private:
uint32_t rngValidMeaTime_ms; // time stamp from latest valid range measurement (msec)
uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec)
uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (msec)
Vector2 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2
Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec)
Vector2 flowVarInnov; // optical flow innovations variances (rad/sec)^2
Vector2 flowInnov; // optical flow LOS innovations (rad/sec)
uint32_t flowInnovTime_ms; // system time that optical flow innovations and variances were recorded (to detect timeouts)
ftype Popt; // Optical flow terrain height state covariance (m^2)
ftype terrainState; // terrain position state (m)