dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2 - preflt checks: scale flow innovation checks 

Opt flow raw innovations can be really large on ground due to the small
distance to the ground (vel = flow / dist). To make the pre-flight check
more meaningful, scale it with the current distance.
This commit is contained in:
bresch 2023-07-03 13:35:44 +02:00 committed by Daniel Agar
parent a07b8c08ab
commit eb23779c57
3 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF2.cpp
View File

@ -1296,6 +1296,10 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
// set dist bottom to scale flow innovation
const float dist_bottom = _ekf.getTerrainVertPos() - _ekf.getPosition()(2);
_preflt_checker.setDistBottom(dist_bottom);
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_EXTERNAL_VISION)

2
src/modules/ekf2/Utility/PreFlightChecker.cpp
View File

@ -85,7 +85,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
if (_is_using_flow_aiding) {
const Vector2f flow_innov = Vector2f(innov.flow);
const Vector2f flow_innov = Vector2f(innov.flow) * _flow_dist_bottom;
Vector2f flow_innov_lpf;
flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim);
flow_innov_lpf(1) = _filter_flow_y_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim);

2
src/modules/ekf2/Utility/PreFlightChecker.hpp
View File

@ -77,6 +77,7 @@ public:
void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; }
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; }
void setDistBottom(float dist_bottom) { _flow_dist_bottom = dist_bottom; }
#endif // CONFIG_EKF2_OPTICAL_FLOW
void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }
@ -179,6 +180,7 @@ private:
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
bool _is_using_flow_aiding {};
float _flow_dist_bottom {};
InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad)
InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad)