fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 07:07:36 +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
Download Patch File Download Diff File Expand all files Collapse all files
EKF/optflow_fusion.cpp
+3 -4
View File
@@ -229,14 +229,13 @@ void Ekf::fuseOptFlow()
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation // 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); _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) {
if (_flow_innov_var(1) >= R_LOS) {
HK95 = HK4/_flow_innov_var(1);
} else {
// we need to reinitialise the covariance matrix and abort this fusion step // we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance(); initialiseCovariance();
return; return;
} }
const float HK95 = HK4/_flow_innov_var(1);
// run the innovation consistency check and record result // run the innovation consistency check and record result
bool flow_fail = false; bool flow_fail = false;