diff --git a/msg/EstimatorAidSource2d.msg b/msg/EstimatorAidSource2d.msg index a70c9acaf5..9fdba8fefa 100644 --- a/msg/EstimatorAidSource2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -19,4 +19,4 @@ bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos -# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow +# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow diff --git a/msg/EstimatorInnovations.msg b/msg/EstimatorInnovations.msg index 75327b3ab0..e8b430669e 100644 --- a/msg/EstimatorInnovations.msg +++ b/msg/EstimatorInnovations.msg @@ -23,6 +23,7 @@ float32 aux_vvel # vertical auxiliary velocity innovation from landing target # Optical flow float32[2] flow # flow innvoation (rad/sec) and innovation variance ((rad/sec)**2) +float32[2] terr_flow # flow innvoation (rad/sec) and innovation variance computed by the terrain estimator ((rad/sec)**2) # Various float32 heading # heading innovation (rad) and innovation variance (rad**2) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index be3d58a515..e79eea97e6 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -115,6 +115,10 @@ public: const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); } const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; } + void getTerrainFlowInnov(float flow_innov[2]) const; + void getTerrainFlowInnovVar(float flow_innov_var[2]) const; + void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); } + void getHeadingInnov(float &heading_innov) const { if (_control_status.flags.mag_hdg) { @@ -431,6 +435,7 @@ public: const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; } const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; } + const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; } private: @@ -570,6 +575,7 @@ private: estimator_aid_source2d_s _aid_src_aux_vel{}; estimator_aid_source2d_s _aid_src_optical_flow{}; + estimator_aid_source2d_s _aid_src_terrain_optical_flow{}; // variables used for the GPS quality checks Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec) @@ -745,7 +751,7 @@ private: void startHaglFlowFusion(); void resetHaglFlow(); void stopHaglFlowFusion(); - void fuseFlowForTerrain(); + void fuseFlowForTerrain(estimator_aid_source2d_s &flow); void controlHaglFakeFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 00a89966fc..8697e1e688 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -427,6 +427,18 @@ void Ekf::getFlowInnovVar(float flow_innov_var[2]) const flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1]; } +void Ekf::getTerrainFlowInnov(float flow_innov[2]) const +{ + flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0]; + flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1]; +} + +void Ekf::getTerrainFlowInnovVar(float flow_innov_var[2]) const +{ + flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0]; + flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1]; +} + // get the state vector at the delayed time horizon matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const { diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 7d720e6beb..56900214fe 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -254,7 +254,7 @@ void Ekf::controlHaglFlowFusion() } if (_flow_data_ready) { - updateOptFlow(_aid_src_optical_flow); + updateOptFlow(_aid_src_terrain_optical_flow); const bool continuing_conditions_passing = _control_status.flags.in_air && !_control_status.flags.opt_flow @@ -267,7 +267,7 @@ void Ekf::controlHaglFlowFusion() if (continuing_conditions_passing) { // TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon - fuseFlowForTerrain(); + fuseFlowForTerrain(_aid_src_terrain_optical_flow); _flow_data_ready = false; // TODO: do something when failing continuously the innovation check @@ -298,15 +298,15 @@ void Ekf::startHaglFlowFusion() { _hagl_sensor_status.flags.flow = true; // TODO: do a reset instead of trying to fuse the data? - fuseFlowForTerrain(); + fuseFlowForTerrain(_aid_src_terrain_optical_flow); _flow_data_ready = false; } void Ekf::stopHaglFlowFusion() { if (_hagl_sensor_status.flags.flow) { - _hagl_sensor_status.flags.flow = false; + resetEstimatorAidStatus(_aid_src_terrain_optical_flow); } } @@ -318,11 +318,11 @@ void Ekf::resetHaglFlow() _terrain_vpos_reset_counter++; } -void Ekf::fuseFlowForTerrain() +void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow) { - _aid_src_optical_flow.fusion_enabled = true; + flow.fusion_enabled = true; - const float R_LOS = _aid_src_optical_flow.observation_variance[0]; + const float R_LOS = flow.observation_variance[0]; // calculate the height above the ground of the optical flow camera. Since earth frame is NED // a positive offset in earth frame leads to a smaller height above the ground. @@ -332,10 +332,10 @@ void Ekf::fuseFlowForTerrain() Vector2f innov_var; float H; sym::TerrEstComputeFlowXyInnovVarAndHx(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &innov_var, &H); - innov_var.copyTo(_aid_src_optical_flow.innovation_variance); + innov_var.copyTo(flow.innovation_variance); - if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS) - || (_aid_src_optical_flow.innovation_variance[1] < R_LOS)) { + if ((flow.innovation_variance[0] < R_LOS) + || (flow.innovation_variance[1] < R_LOS)) { // we need to reinitialise the covariance matrix and abort this fusion step ECL_ERR("Opt flow error - covariance reset"); _terrain_var = 100.0f; @@ -343,13 +343,13 @@ void Ekf::fuseFlowForTerrain() } // run the innovation consistency check and record result - setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f)); + setEstimatorAidStatusTestRatio(flow, math::max(_params.flow_innov_gate, 1.f)); - _innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f); - _innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f); + _innov_check_fail_status.flags.reject_optflow_X = (flow.test_ratio[0] > 1.f); + _innov_check_fail_status.flags.reject_optflow_Y = (flow.test_ratio[1] > 1.f); // if either axis fails we abort the fusion - if (_aid_src_optical_flow.innovation_rejected) { + if (flow.innovation_rejected) { return; } @@ -360,14 +360,14 @@ void Ekf::fuseFlowForTerrain() } else if (index == 1) { // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) - sym::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H); + sym::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &flow.innovation_variance[1], &H); // recalculate the innovation using the updated state const Vector2f vel_body = predictFlowVelBody(); range = predictFlowRange(); - _aid_src_optical_flow.innovation[1] = (-vel_body(0) / range) - _aid_src_optical_flow.observation[1]; + flow.innovation[1] = (-vel_body(0) / range) - flow.observation[1]; - if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) { + if (flow.innovation_variance[1] < R_LOS) { // we need to reinitialise the covariance matrix and abort this fusion step ECL_ERR("Opt flow error - covariance reset"); _terrain_var = 100.0f; @@ -375,9 +375,9 @@ void Ekf::fuseFlowForTerrain() } } - float Kfusion = _terrain_var * H / _aid_src_optical_flow.innovation_variance[index]; + float Kfusion = _terrain_var * H / flow.innovation_variance[index]; - _terrain_vpos += Kfusion * _aid_src_optical_flow.innovation[0]; + _terrain_vpos += Kfusion * flow.innovation[0]; // constrain terrain to minimum allowed value and predict height above ground _terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2)); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 6860c88c90..7e90b56d93 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -787,6 +787,8 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) // optical flow PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub); + PublishAidSourceStatus(_ekf.aid_src_terrain_optical_flow(), _status_terrain_optical_flow_pub_last, + _estimator_aid_src_terrain_optical_flow_pub); } void EKF2::PublishAttitude(const hrt_abstime ×tamp) @@ -1073,6 +1075,7 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _ekf.getBetaInnov(innovations.beta); _ekf.getHaglInnov(innovations.hagl); _ekf.getHaglRateInnov(innovations.hagl_rate); + _ekf.getTerrainFlowInnov(innovations.terr_flow); // Not yet supported innovations.aux_vvel = NAN; @@ -1122,6 +1125,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) _ekf.getBetaInnovRatio(test_ratios.beta); _ekf.getHaglInnovRatio(test_ratios.hagl); _ekf.getHaglRateInnovRatio(test_ratios.hagl_rate); + _ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]); // Not yet supported test_ratios.aux_vvel = NAN; @@ -1147,6 +1151,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) _ekf.getBetaInnovVar(variances.beta); _ekf.getHaglInnovVar(variances.hagl); _ekf.getHaglRateInnovVar(variances.hagl_rate); + _ekf.getTerrainFlowInnovVar(variances.terr_flow); // Not yet supported variances.aux_vvel = NAN; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 222776f621..5dc81c51f0 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -288,6 +288,7 @@ private: hrt_abstime _status_aux_vel_pub_last{0}; hrt_abstime _status_optical_flow_pub_last{0}; + hrt_abstime _status_terrain_optical_flow_pub_last{0}; float _last_baro_bias_published{}; float _last_gnss_hgt_bias_published{}; @@ -378,6 +379,7 @@ private: uORB::PublicationMulti _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)}; uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; + uORB::PublicationMulti _estimator_aid_src_terrain_optical_flow_pub{ORB_ID(estimator_aid_src_terrain_optical_flow)}; // publications with topic dependent on multi-mode uORB::PublicationMulti _attitude_pub; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 2a0a2c957c..eecf97bc93 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -196,6 +196,7 @@ void LoggedTopics::add_default_topics() // add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); + // add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); // log all raw sensors at minimal rate (at least 1 Hz) @@ -283,6 +284,7 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_sideslip", 0, MAX_ESTIMATOR_INSTANCES); #endif /* CONFIG_ARCH_BOARD_PX4_SITL */