AP_NavEKF3: rename varInnovOptFlow to flowVarInnov
also renamed innovOptFlow to flowInnov
This commit is contained in:
parent
85ade10e85
commit
d1f2acd813
@ -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
|
||||
|
@ -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();
|
||||
|
||||
|
@ -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:
|
||||
|
@ -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)
|
||||
|
Loading…
Reference in New Issue
Block a user