From 41d9c3dd2ae3cc91257e74c6413d56f316c8094e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 26 Jul 2022 11:14:00 -0400 Subject: [PATCH] ekf2: add AUX velocity aid src status - also includes velocity and position helpers for using estimator aid source status messages that will later be used for GPS, EV, etc --- msg/estimator_aid_source_3d.msg | 3 +- src/modules/ekf2/EKF/control.cpp | 17 ++-- src/modules/ekf2/EKF/ekf.h | 35 ++++++-- src/modules/ekf2/EKF/ekf_helper.cpp | 14 ++-- src/modules/ekf2/EKF/estimator_interface.h | 1 - src/modules/ekf2/EKF/vel_pos_fusion.cpp | 96 ++++++++++++++++++++++ src/modules/ekf2/EKF2.cpp | 7 +- src/modules/ekf2/EKF2.hpp | 3 + 8 files changed, 146 insertions(+), 30 deletions(-) diff --git a/msg/estimator_aid_source_3d.msg b/msg/estimator_aid_source_3d.msg index 36d7485e4f..2d5bdc1ef5 100644 --- a/msg/estimator_aid_source_3d.msg +++ b/msg/estimator_aid_source_3d.msg @@ -19,4 +19,5 @@ bool[3] innovation_rejected # true if the observation has been rejected bool[3] fused # true if the sample was successfully fused # TOPICS estimator_aid_source_3d -# TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel estimator_aid_src_mag +# TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel +# TOPICS estimator_aid_src_mag estimator_aid_src_aux_vel diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 47697a79bf..c26b8e686d 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -1131,18 +1131,13 @@ void Ekf::controlAuxVelFusion() if (_auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &auxvel_sample_delayed)) { + updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel); + if (isHorizontalAidingActive()) { - - const float aux_vel_innov_gate = fmaxf(_params.auxvel_gate, 1.f); - - _aux_vel_innov = _state.vel - auxvel_sample_delayed.vel; - - fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, auxvel_sample_delayed.velVar, - _aux_vel_innov_var, _aux_vel_test_ratio); - - // Can be enabled after bit for this is added to EKF_AID_MASK - // fuseVerticalVelocity(_aux_vel_innov, aux_vel_innov_gate, auxvel_sample_delayed.velVar, - // _aux_vel_innov_var, _aux_vel_test_ratio); + _aid_src_aux_vel.fusion_enabled[0] = PX4_ISFINITE(auxvel_sample_delayed.vel(0)); + _aid_src_aux_vel.fusion_enabled[1] = PX4_ISFINITE(auxvel_sample_delayed.vel(1)); + _aid_src_aux_vel.fusion_enabled[2] = PX4_ISFINITE(auxvel_sample_delayed.vel(2)); + fuseVelocity(_aid_src_aux_vel); } } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 46f891f3fa..9532178b58 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -96,7 +96,7 @@ public: void getAuxVelInnov(float aux_vel_innov[2]) const; void getAuxVelInnovVar(float aux_vel_innov[2]) const; - void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = _aux_vel_test_ratio(0); } + void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = Vector3f(_aid_src_aux_vel.test_ratio).max(); } void getFlowInnov(float flow_innov[2]) const { _flow_innov.copyTo(flow_innov); } void getFlowInnovVar(float flow_innov_var[2]) const { _flow_innov_var.copyTo(flow_innov_var); } @@ -404,6 +404,8 @@ public: const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; } const auto &aid_src_mag() const { return _aid_src_mag; } + const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; } + private: // set the internal states and status to their default value @@ -512,9 +514,6 @@ private: Vector3f _ev_pos_innov{}; ///< external vision position innovations (m) Vector3f _ev_pos_innov_var{}; ///< external vision position innovation variances (m**2) - Vector3f _aux_vel_innov{}; ///< horizontal auxiliary velocity innovations: (m/sec) - Vector3f _aux_vel_innov_var{}; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2) - Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2) Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2) @@ -553,6 +552,8 @@ private: estimator_aid_source_1d_s _aid_src_mag_heading{}; estimator_aid_source_3d_s _aid_src_mag{}; + estimator_aid_source_3d_s _aid_src_aux_vel{}; + // output predictor states Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad) Vector3f _vel_err_integ{}; ///< integral of velocity tracking error (m) @@ -719,6 +720,12 @@ private: // fuse optical flow line of sight rate measurements void fuseOptFlow(); + void updateVelocityAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& velocity, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& vel_aid_src) const; + void updatePositionAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& position, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& pos_aid_src) const; + + void fuseVelocity(estimator_aid_source_3d_s& vel_aid_src); + void fusePosition(estimator_aid_source_3d_s& pos_aid_src); + bool fuseHorizontalVelocity(const Vector3f &innov, float innov_gate, const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio); @@ -1157,16 +1164,28 @@ private: void setEstimatorAidStatusTestRatio(estimator_aid_source_1d_s &status, float innovation_gate) const { - status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance); - status.innovation_rejected = (status.test_ratio > 1.f); + if (PX4_ISFINITE(status.innovation) && PX4_ISFINITE(status.innovation_variance)) { + status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance); + status.innovation_rejected = (status.test_ratio > 1.f); + + } else { + status.test_ratio = INFINITY; + status.innovation_rejected = true; + } } template void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const { for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) { - status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]); - status.innovation_rejected[i] = (status.test_ratio[i] > 1.f); + if (PX4_ISFINITE(status.innovation[i]) && PX4_ISFINITE(status.innovation_variance[i])) { + status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]); + status.innovation_rejected[i] = (status.test_ratio[i] > 1.f); + + } else { + status.test_ratio[i] = INFINITY; + status.innovation_rejected[i] = true; + } } } }; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 6669937034..81888fa2b0 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -657,14 +657,14 @@ void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vp void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const { - aux_vel_innov[0] = _aux_vel_innov(0); - aux_vel_innov[1] = _aux_vel_innov(1); + aux_vel_innov[0] = _aid_src_aux_vel.innovation[0]; + aux_vel_innov[1] = _aid_src_aux_vel.innovation[1]; } void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const { - aux_vel_innov_var[0] = _aux_vel_innov_var(0); - aux_vel_innov_var[1] = _aux_vel_innov_var(1); + aux_vel_innov_var[0] = _aid_src_aux_vel.innovation_variance[0]; + aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1]; } // get the state vector at the delayed time horizon @@ -1761,9 +1761,9 @@ void Ekf::stopEvYawFusion() void Ekf::stopAuxVelFusion() { - _aux_vel_innov.setZero(); - _aux_vel_innov_var.setZero(); - _aux_vel_test_ratio.setZero(); + ECL_INFO("stopping aux vel fusion"); + //_control_status.flags.aux_vel = false; + resetEstimatorAidStatus(_aid_src_aux_vel); } void Ekf::stopFlowFusion() diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 02e61999c3..a4cc1507fb 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -331,7 +331,6 @@ protected: AlphaFilter _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state Vector2f _ev_vel_test_ratio{}; // EV velocity innovation consistency check ratios Vector2f _ev_pos_test_ratio{}; // EV position innovation consistency check ratios - Vector2f _aux_vel_test_ratio{}; // Auxiliary horizontal velocity innovation consistency check ratio float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio float _beta_test_ratio{}; // sideslip innovation consistency check ratio diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index 431fd6d82b..8a29487eb4 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -159,6 +159,102 @@ bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const } } +void Ekf::updateVelocityAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& velocity, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& vel_aid_src) const +{ + resetEstimatorAidStatus(vel_aid_src); + + for (int i = 0; i < 3; i++) { + vel_aid_src.observation[i] = velocity(i); + vel_aid_src.observation_variance[i] = obs_var(i); + + vel_aid_src.innovation[i] = _state.vel(i) - velocity(i); + vel_aid_src.innovation_variance[i] = P(4 + i, 4 + i) + obs_var(i); + } + + setEstimatorAidStatusTestRatio(vel_aid_src, innov_gate); + + // vz special case if there is bad vertical acceleration data, then don't reject measurement, + // but limit innovation to prevent spikes that could destabilise the filter + if (_fault_status.flags.bad_acc_vertical && vel_aid_src.innovation_rejected[2]) { + const float innov_limit = innov_gate * sqrtf(vel_aid_src.innovation_variance[2]); + vel_aid_src.innovation[2] = math::constrain(vel_aid_src.innovation[2], -innov_limit, innov_limit); + vel_aid_src.innovation_rejected[2] = false; + } + + vel_aid_src.timestamp_sample = sample_time_us; +} + +void Ekf::updatePositionAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& position, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& pos_aid_src) const +{ + resetEstimatorAidStatus(pos_aid_src); + + for (int i = 0; i < 3; i++) { + pos_aid_src.observation[i] = position(i); + pos_aid_src.observation_variance[i] = obs_var(i); + + pos_aid_src.innovation[i] = _state.pos(i) - position(i); + pos_aid_src.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i); + } + + setEstimatorAidStatusTestRatio(pos_aid_src, innov_gate); + + // z special case if there is bad vertical acceleration data, then don't reject measurement, + // but limit innovation to prevent spikes that could destabilise the filter + if (_fault_status.flags.bad_acc_vertical && pos_aid_src.innovation_rejected[2]) { + const float innov_limit = innov_gate * sqrtf(pos_aid_src.innovation_variance[2]); + pos_aid_src.innovation[2] = math::constrain(pos_aid_src.innovation[2], -innov_limit, innov_limit); + pos_aid_src.innovation_rejected[2] = false; + } + + pos_aid_src.timestamp_sample = sample_time_us; +} + +void Ekf::fuseVelocity(estimator_aid_source_3d_s& vel_aid_src) +{ + // vx & vy + if (vel_aid_src.fusion_enabled[0] && !vel_aid_src.innovation_rejected[0] + && vel_aid_src.fusion_enabled[1] && !vel_aid_src.innovation_rejected[1] + ) { + for (int i = 0; i < 2; i++) { + if (fuseVelPosHeight(vel_aid_src.innovation[i], vel_aid_src.innovation_variance[i], i)) { + vel_aid_src.fused[i] = true; + vel_aid_src.time_last_fuse[i] = _time_last_imu; + } + } + } + + // vz + if (vel_aid_src.fusion_enabled[2] && !vel_aid_src.innovation_rejected[2]) { + if (fuseVelPosHeight(vel_aid_src.innovation[2], vel_aid_src.innovation_variance[2], 2)) { + vel_aid_src.fused[2] = true; + vel_aid_src.time_last_fuse[2] = _time_last_imu; + } + } +} + +void Ekf::fusePosition(estimator_aid_source_3d_s& pos_aid_src) +{ + // x & y + if (pos_aid_src.fusion_enabled[0] && !pos_aid_src.innovation_rejected[0] + && pos_aid_src.fusion_enabled[1] && !pos_aid_src.innovation_rejected[1] + ) { + for (int i = 0; i < 2; i++) { + if (fuseVelPosHeight(pos_aid_src.innovation[i], pos_aid_src.innovation_variance[i], 3 + i)) { + pos_aid_src.fused[i] = true; + pos_aid_src.time_last_fuse[i] = _time_last_imu; + } + } + } + + // z + if (pos_aid_src.fusion_enabled[2] && !pos_aid_src.innovation_rejected[2]) { + if (fuseVelPosHeight(pos_aid_src.innovation[2], pos_aid_src.innovation_variance[2], 5)) { + pos_aid_src.fused[2] = true; + pos_aid_src.time_last_fuse[2] = _time_last_imu; + } + } +} + // Helper function that fuses a single velocity or position measurement bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index) { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 8730189609..c83b2ff3d3 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -663,6 +663,9 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) // mag 3d PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub); + + // aux velocity + PublishAidSourceStatus(_ekf.aid_src_aux_vel(), _status_aux_vel_pub_last, _estimator_aid_src_aux_vel_pub); } void EKF2::PublishAttitude(const hrt_abstime ×tamp) @@ -1563,8 +1566,8 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) // velocity of vehicle relative to target has opposite sign to target relative to vehicle auxVelSample auxvel_sample{ .time_us = landing_target_pose.timestamp, - .vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f}, - .velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f}, + .vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, NAN}, + .velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, NAN}, }; _ekf.setAuxVelData(auxvel_sample); } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index fd712747a5..59f6d66565 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -266,6 +266,8 @@ private: hrt_abstime _status_mag_pub_last{0}; hrt_abstime _status_mag_heading_pub_last{0}; + hrt_abstime _status_aux_vel_pub_last{0}; + float _last_baro_bias_published{}; float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements @@ -339,6 +341,7 @@ private: uORB::PublicationMulti _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)}; + uORB::PublicationMulti _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)}; // publications with topic dependent on multi-mode uORB::PublicationMulti _attitude_pub;