fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: optflow fusion cleanup

Browse Source 
Co-authored-by: kritz <kritz@ethz.ch>
This commit is contained in:
Paul Riseborough 2020-08-15 08:44:02 +10:00
parent 0e74f3ea9d
commit 7fa9a1963b
1 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
EKF/optflow_fusion.cpp
View File

@ -229,14 +229,13 @@ void Ekf::fuseOptFlow()
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
_flow_innov_var(1) = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
float HK95;
if (_flow_innov_var(1) >= R_LOS) {
HK95 = HK4/_flow_innov_var(1);
} else {
if (_flow_innov_var(1) < R_LOS) {
// we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
return;
}
const float HK95 = HK4/_flow_innov_var(1);
// run the innovation consistency check and record result
bool flow_fail = false;