fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 07:47:35 +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:43:15 +10:00
parent 1ad042da23
commit 6e992a18f6
1 changed files with 2 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/optflow_fusion.cpp
+2 -4
View File
@@ -175,14 +175,12 @@ void Ekf::fuseOptFlow()
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation // calculate innovation variance for X axis observation and protect against a badly conditioned calculation
_flow_innov_var(0) = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS); _flow_innov_var(0) = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
float HK50; if (_flow_innov_var(0) < R_LOS) {
if (_flow_innov_var(0) > R_LOS) {
HK50 = HK4/_flow_innov_var(0);
} 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 HK50 = HK4/_flow_innov_var(0);
const float HK51 = Tbs(0,1)*q1; const float HK51 = Tbs(0,1)*q1;
const float HK52 = Tbs(0,2)*q0; const float HK52 = Tbs(0,2)*q0;