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

ekf2-terrain: make terrain validity based on uncertainty

Browse Source 
When using optical flow for terrain aiding, we cannot assume that the
terrain estimate is valid if flow is fused as is can only be observed
during motion. When no direct observation is available, the terrain is
assumed to be valid if its 1sigma uncertainty is < 10% of the hagl.
This commit is contained in:
bresch
2024-06-26 11:05:38 +02:00
committed by Silvan Fuhrer
parent a665764b0e
commit a50ef2eb5e
5 changed files with 54 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/terrain_control.cpp
+5 -10
View File
@@ -36,7 +36,7 @@
*/
#include "ekf.h"
#include "ekf_derivation/generated/state.h"
#include "ekf_derivation/generated/compute_hagl_innov_var.h"
#include <mathlib/mathlib.h>
@@ -86,23 +86,18 @@ void Ekf::controlTerrainFakeFusion()
bool Ekf::isTerrainEstimateValid() const
{
// Assume being valid when the uncertainty is small compared to the height above ground
bool valid = getTerrainVariance() < sq(0.1f * getHagl());
float hagl_var = INFINITY;
sym::ComputeHaglInnovVar(P, 0.f, &hagl_var);
bool valid = hagl_var < fmaxf(sq(0.1f * getHagl()), 0.2f);
#if defined(CONFIG_EKF2_RANGE_FINDER)
// Assume that the terrain estimate is always valid when direct observations are fused
if (_hagl_sensor_status.flags.range_finder && isRecent(_aid_src_rng_hgt.time_last_fuse, (uint64_t)5e6)) {
valid = true;
}
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
if (_hagl_sensor_status.flags.flow && isRecent(_aid_src_optical_flow.time_last_fuse, (uint64_t)5e6)) {
valid = true;
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
return valid;
}