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

Remove repeated division by same value

Browse Source
This commit is contained in:
kamilritz
2020-06-21 14:10:24 +02:00
committed by Mathieu Bresciani
parent d16b43a2da
commit d9afc2f1e8
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
+5 -5
View File
@@ -222,10 +222,10 @@ void Ekf::fuseFlowForTerrain()
// constrain terrain to minimum allowed value and predict height above ground // constrain terrain to minimum allowed value and predict height above ground
_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2)); _terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
const float pred_hagl = _terrain_vpos - _state.pos(2); const float pred_hagl_inv = 1.f / (_terrain_vpos - _state.pos(2));
// Calculate observation matrix for flow around the vehicle x axis // Calculate observation matrix for flow around the vehicle x axis
const float Hx = vel_body(1) * t0 / (pred_hagl * pred_hagl); const float Hx = vel_body(1) * t0 * pred_hagl_inv * pred_hagl_inv;
// Constrain terrain variance to be non-negative // Constrain terrain variance to be non-negative
_terrain_var = fmaxf(_terrain_var, 0.0f); _terrain_var = fmaxf(_terrain_var, 0.0f);
@@ -237,7 +237,7 @@ void Ekf::fuseFlowForTerrain()
const float Kx = _terrain_var * Hx / _flow_innov_var[0]; const float Kx = _terrain_var * Hx / _flow_innov_var[0];
// calculate prediced optical flow about x axis // calculate prediced optical flow about x axis
const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) / pred_hagl; const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv;
// calculate flow innovation (x axis) // calculate flow innovation (x axis)
_flow_innov[0] = pred_flow_x - opt_flow_rate(0); _flow_innov[0] = pred_flow_x - opt_flow_rate(0);
@@ -258,7 +258,7 @@ void Ekf::fuseFlowForTerrain()
} }
// Calculate observation matrix for flow around the vehicle y axis // Calculate observation matrix for flow around the vehicle y axis
const float Hy = -vel_body(0) * t0 / (pred_hagl * pred_hagl); const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv;
// Calculuate innovation variance // Calculuate innovation variance
_flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS; _flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS;
@@ -267,7 +267,7 @@ void Ekf::fuseFlowForTerrain()
const float Ky = _terrain_var * Hy / _flow_innov_var[1]; const float Ky = _terrain_var * Hy / _flow_innov_var[1];
// calculate prediced optical flow about y axis // calculate prediced optical flow about y axis
const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) / pred_hagl; const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv;
// calculate flow innovation (y axis) // calculate flow innovation (y axis)
_flow_innov[1] = pred_flow_y - opt_flow_rate(1); _flow_innov[1] = pred_flow_y - opt_flow_rate(1);