aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: always calculate extNav innovations and variances

Browse Source
This commit is contained in:
Randy Mackay 2020-10-13 11:29:35 +09:00
parent 6b2b5c4ca0
commit e7e91b1c3b
2 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -426,6 +426,12 @@ void NavEKF3_core::SelectVelPosFusion()
extNavVelToFuse = storedExtNavVel.recall(extNavVelDelayed, imuDataDelayed.time_ms);
if (extNavVelToFuse) {
CorrectExtNavVelForSensorOffset(extNavVelDelayed);
// calculate innovations and variances for reporting purposes only
CalculateVelInnovationsAndVariances(extNavVelDelayed.vel, extNavVelDelayed.err, frontend->extNavVelVarAccScale, extNavVelInnov, extNavVelVarInnov);
// record time innovations were calculated (for timeout checks)
extNavVelInnovTime_ms = AP_HAL::millis();
}
// Read GPS data from the sensor

3
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1352,6 +1352,9 @@ private:
uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec)
bool extNavVelToFuse; // true when there is new external navigation velocity to fuse
bool useExtNavVel; // true if external nav velocity should be used
Vector3f extNavVelInnov; // external nav velocity innovations
Vector3f extNavVelVarInnov; // external nav velocity innovation variances
uint32_t extNavVelInnovTime_ms; // system time that external nav velocity innovations were recorded (to detect timeouts)
// flags indicating severe numerical errors in innovation variance calculation for different fusion operations
struct {