fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 09:00: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-03 22:17:13 +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/gps_yaw_fusion.cpp
+2 -3
View File
@@ -146,13 +146,10 @@ void Ekf::fuseGpsYaw()
_heading_innov_var += H_YAW[row] * PH[row];
}
float heading_innov_var_inv;
// check if the innovation variance calculation is badly conditioned
if (_heading_innov_var >= R_YAW) {
// the innovation variance contribution from the state covariances is not negative, no fault
_fault_status.flags.bad_hdg = false;
heading_innov_var_inv = 1.0f / _heading_innov_var;
} else {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
@@ -164,6 +161,8 @@ void Ekf::fuseGpsYaw()
return;
}
const float heading_innov_var_inv = 1.f / _heading_innov_var;
// calculate the Kalman gains
// only calculate gains for states we are using
Vector24f Kfusion;
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);