forked from Archive/PX4-Autopilot
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:
parent
672b29d555
commit
f5edff2647
|
@ -1296,6 +1296,10 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
||||||
_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
|
_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
|
||||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||||
_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_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
|
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||||
|
|
|
@ -85,7 +85,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_
|
||||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||||
|
|
||||||
if (_is_using_flow_aiding) {
|
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;
|
Vector2f flow_innov_lpf;
|
||||||
flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim);
|
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);
|
flow_innov_lpf(1) = _filter_flow_y_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim);
|
||||||
|
|
|
@ -77,6 +77,7 @@ public:
|
||||||
void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; }
|
void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; }
|
||||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||||
void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; }
|
void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; }
|
||||||
|
void setDistBottom(float dist_bottom) { _flow_dist_bottom = dist_bottom; }
|
||||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||||
void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
|
void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
|
||||||
void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }
|
void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }
|
||||||
|
@ -179,6 +180,7 @@ private:
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||||
bool _is_using_flow_aiding {};
|
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_x_innov; ///< Preflight low pass filter optical flow innovation (rad)
|
||||||
InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad)
|
InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad)
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue