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

Bring declaration and initialization together

Browse Source
This commit is contained in:
kamilritz
2020-07-16 18:25:06 +02:00
committed by Mathieu Bresciani
parent 796afd5f98
commit e357376797
2 changed files with 12 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/optflow_fusion.cpp
+10 -13
View File
@@ -220,19 +220,17 @@ void Ekf::fuseOptFlow()
float t87 = t2*t22*t56;
float t92 = t2*t7*t69;
float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
float t78;
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
if (t77 >= R_LOS) {
t78 = 1.0f / t77;
_flow_innov_var(0) = t77;
} else {
// protect against a badly conditioned calculation
if (t77 < R_LOS) {
// we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
return;
}
float t78 = 1.0f / t77;
_flow_innov_var(0) = t77;
// calculate Kalman gains for X-axis observation
Kfusion[0][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);
Kfusion[1][0] = t78*(t31+P(1,0)*t2*t5-P(1,4)*t2*t7+P(1,6)*t2*t10+P(1,2)*t2*t19-P(1,3)*t2*t22+P(1,5)*t2*t27);
@@ -363,18 +361,17 @@ void Ekf::fuseOptFlow()
float t85 = t2*t19*t49;
float t94 = t2*t10*t76;
float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
float t78;
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
if (t77 >= R_LOS) {
t78 = 1.0f / t77;
_flow_innov_var(1) = t77;
} else {
// protect against a badly conditioned calculation
if (t77 < R_LOS) {
// we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
return;
}
float t78 = 1.0f / t77;
_flow_innov_var(1) = t77;
// calculate Kalman gains for Y-axis observation
Kfusion[0][1] = -t78*(t12+P(0,5)*t2*t8-P(0,6)*t2*t10+P(0,1)*t2*t16-P(0,2)*t2*t19+P(0,3)*t2*t22+P(0,4)*t2*t27);
Kfusion[1][1] = -t78*(t31+P(1,0)*t2*t5+P(1,5)*t2*t8-P(1,6)*t2*t10-P(1,2)*t2*t19+P(1,3)*t2*t22+P(1,4)*t2*t27);