diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 2f6358f9eb..545892f48e 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -74,6 +74,7 @@ set(msg_files EstimatorAidSource2d.msg EstimatorAidSource3d.msg EstimatorBias.msg + EstimatorBias3d.msg EstimatorEventFlags.msg EstimatorGpsStatus.msg EstimatorInnovations.msg diff --git a/msg/EstimatorBias.msg b/msg/EstimatorBias.msg index 3dbadb94e1..10dbca87bd 100644 --- a/msg/EstimatorBias.msg +++ b/msg/EstimatorBias.msg @@ -9,4 +9,4 @@ float32 innov # innovation of the last measurement fusion (m) float32 innov_var # innovation variance of the last measurement fusion (m^2) float32 innov_test_ratio # normalized innovation squared test ratio -# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias estimator_ev_hgt_bias +# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias diff --git a/msg/EstimatorBias3d.msg b/msg/EstimatorBias3d.msg new file mode 100644 index 0000000000..16b2937291 --- /dev/null +++ b/msg/EstimatorBias3d.msg @@ -0,0 +1,14 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32[3] bias # estimated barometric altitude bias (m) +float32[3] bias_var # estimated barometric altitude bias variance (m^2) + +float32[3] innov # innovation of the last measurement fusion (m) +float32[3] innov_var # innovation variance of the last measurement fusion (m^2) +float32[3] innov_test_ratio # normalized innovation squared test ratio + +# TOPICS estimator_bias3d +# TOPICS estimator_ev_pos_bias diff --git a/msg/VehicleOdometry.msg b/msg/VehicleOdometry.msg index 4bd8c5ca30..fbdd1920e1 100644 --- a/msg/VehicleOdometry.msg +++ b/msg/VehicleOdometry.msg @@ -28,4 +28,4 @@ uint8 reset_counter int8 quality # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry -# TOPICS estimator_odometry estimator_visual_odometry_aligned +# TOPICS estimator_odometry diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index dc29cac2f1..19ce16f50d 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -91,8 +91,11 @@ px4_add_module( EKF/ekf_helper.cpp EKF/EKFGSF_yaw.cpp EKF/estimator_interface.cpp + EKF/ev_control.cpp EKF/ev_height_control.cpp + EKF/ev_pos_control.cpp EKF/ev_vel_control.cpp + EKF/ev_yaw_control.cpp EKF/fake_height_control.cpp EKF/fake_pos_control.cpp EKF/gnss_height_control.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 1594e01fce..e6d668dc1e 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -42,8 +42,11 @@ add_library(ecl_EKF ekf_helper.cpp EKFGSF_yaw.cpp estimator_interface.cpp + ev_control.cpp ev_height_control.cpp + ev_pos_control.cpp ev_vel_control.cpp + ev_yaw_control.cpp fake_height_control.cpp fake_pos_control.cpp gnss_height_control.cpp diff --git a/src/modules/ekf2/EKF/bias_estimator.hpp b/src/modules/ekf2/EKF/bias_estimator.hpp index 10448beb64..53f82dc03c 100644 --- a/src/modules/ekf2/EKF/bias_estimator.hpp +++ b/src/modules/ekf2/EKF/bias_estimator.hpp @@ -64,7 +64,9 @@ public: float innov_test_ratio{INFINITY}; }; + BiasEstimator() {} BiasEstimator(float state_init, float state_var_init): _state{state_init}, _state_var{state_var_init} {}; + virtual ~BiasEstimator() = default; void reset() diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 1beac4b6fa..b245b3e4ca 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -79,6 +79,11 @@ static constexpr float BADACC_BIAS_PNOISE = 4.9f; ///< The delta velocity proce // ground effect compensation static constexpr uint64_t GNDEFFECT_TIMEOUT = 10e6; ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec) +enum class PositionFrame : uint8_t { + LOCAL_FRAME_NED = 0, + LOCAL_FRAME_FRD = 1, +}; + enum class VelocityFrame : uint8_t { LOCAL_FRAME_NED = 0, LOCAL_FRAME_FRD = 1, @@ -115,6 +120,12 @@ enum HeightSensor : uint8_t { UNKNOWN = 4 }; +enum class PositionSensor : uint8_t { + UNKNOWN = 0, + GNSS = 1, + EV = 2, +}; + enum GnssCtrl : uint8_t { HPOS = (1<<0), VPOS = (1<<1), @@ -140,11 +151,11 @@ enum SensorFusionMask : uint16_t { DEPRECATED_USE_GPS = (1<<0), ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl) USE_OPT_FLOW = (1<<1), ///< set to true to use optical flow data INHIBIT_ACC_BIAS = (1<<2), ///< set to true to inhibit estimation of accelerometer delta velocity bias - USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data - USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw + DEPRECATED_USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data + DEPRECATED_USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw USE_DRAG = (1<<5), ///< set to true to use the multi-rotor drag model to estimate wind - ROTATE_EXT_VIS = (1<<6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used - DEPRECATED_USE_GPS_YAW = (1<<7),///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl) + DEPRECATED_ROTATE_EXT_VIS = (1<<6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used + DEPRECATED_USE_GPS_YAW = (1<<7), ///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl) DEPRECATED_USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data }; @@ -238,9 +249,10 @@ struct extVisionSample { Vector3f pos{}; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis Vector3f vel{}; ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis Quatf quat{}; ///< quaternion defining rotation from body to earth frame - Vector3f posVar{}; ///< XYZ position variances (m**2) + Vector3f position_var{}; ///< XYZ position variances (m**2) Vector3f velocity_var{}; ///< XYZ velocity variances ((m/sec)**2) Vector3f orientation_var{}; ///< orientation variance (rad**2) + PositionFrame pos_frame = PositionFrame::LOCAL_FRAME_FRD; VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD; uint8_t reset_counter{}; int8_t quality{}; ///< quality indicator between 0 and 100 @@ -283,6 +295,7 @@ struct parameters { // measurement source control int32_t fusion_mode{}; ///< bitmasked integer that selects some aiding sources int32_t height_sensor_ref{HeightSensor::BARO}; + int32_t position_sensor_ref{static_cast(PositionSensor::GNSS)}; int32_t baro_ctrl{1}; int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL}; int32_t rng_ctrl{RngCtrl::CONDITIONAL}; @@ -325,7 +338,7 @@ struct parameters { const float initial_wind_uncertainty{1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec) // position and velocity fusion - float gps_vel_noise{5.0e-1f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec) + float gps_vel_noise{0.5f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec) float gps_pos_noise{0.5f}; ///< minimum allowed observation noise for gps position fusion (m) float gps_hgt_bias_nsd{0.13f}; ///< process noise for gnss height bias estimation (m/s/sqrt(Hz)) float pos_noaid_noise{10.0f}; ///< observation noise for non-aiding position fusion (m) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index e1b91e0deb..6205ed270f 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -183,10 +183,6 @@ void Ekf::controlFusionModes() } } - if (_ext_vision_buffer) { - _ev_data_ready = _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed); - } - if (_airspeed_buffer) { _tas_data_ready = _airspeed_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed); } @@ -203,7 +199,7 @@ void Ekf::controlFusionModes() controlDragFusion(); controlHeightFusion(); - // Additional data odoemtery data from an external estimator can be fused. + // Additional data odometry data from an external estimator can be fused. controlExternalVisionFusion(); // Additional horizontal velocity data from an auxiliary sensor can be fused @@ -221,213 +217,6 @@ void Ekf::controlFusionModes() updateDeadReckoningStatus(); } -void Ekf::controlExternalVisionFusion() -{ - // Check for new external vision data - if (_ev_data_ready) { - - bool reset = false; - - if (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter) { - reset = true; - } - - if (_inhibit_ev_yaw_use) { - stopEvYawFusion(); - } - - // if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames - // needs to be calculated and the observations rotated into the EKF frame of reference - if ((_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) && ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS) - || (_params.ev_ctrl & static_cast(EvCtrl::VEL))) && !_control_status.flags.ev_yaw) { - - // rotate EV measurements into the EKF Navigation frame - calcExtVisRotMat(); - } - - // external vision aiding selection logic - if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) { - - // check for a external vision measurement that has fallen behind the fusion time horizon - if (isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) { - // turn on use of external vision measurements for position - if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS && !_control_status.flags.ev_pos) { - startEvPosFusion(); - } - } - } - - // external vision yaw aiding selection logic - if (!_inhibit_ev_yaw_use && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_YAW) && !_control_status.flags.ev_yaw - && _control_status.flags.tilt_align) { - - // don't start using EV data unless data is arriving frequently - if (isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) { - if (resetYawToEv()) { - _control_status.flags.yaw_align = true; - startEvYawFusion(); - } - } - } - - // determine if we should use the horizontal position observations - if (_control_status.flags.ev_pos) { - resetEstimatorAidStatus(_aid_src_ev_pos); - - if (reset && _control_status_prev.flags.ev_pos) { - if (!_fuse_hpos_as_odom) { - resetHorizontalPositionToVision(); - } - } - - Vector3f ev_pos_obs_var; - - // correct position and height for offset relative to IMU - const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; - const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - _ev_sample_delayed.pos -= pos_offset_earth; - - // Use an incremental position fusion method for EV position data if GPS is also used - if (_params.gnss_ctrl & GnssCtrl::HPOS) { - _fuse_hpos_as_odom = true; - - } else { - _fuse_hpos_as_odom = false; - } - - if (_fuse_hpos_as_odom) { - if (!_hpos_prev_available) { - // no previous observation available to calculate position change - _hpos_prev_available = true; - - } else { - // calculate the change in position since the last measurement - // rotate measurement into body frame is required when fusing with GPS - Vector3f ev_delta_pos = _R_ev_to_ekf * Vector3f(_ev_sample_delayed.pos - _ev_sample_delayed_prev.pos); - - // use the change in position since the last measurement - _aid_src_ev_pos.observation[0] = ev_delta_pos(0); - _aid_src_ev_pos.observation[1] = ev_delta_pos(1); - - _aid_src_ev_pos.innovation[0] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0); - _aid_src_ev_pos.innovation[1] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1); - - // observation 1-STD error, incremental pos observation is expected to have more uncertainty - Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar); - ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose(); - ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.5f)); - ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 1), sq(0.5f)); - - _aid_src_ev_pos.observation_variance[0] = ev_pos_obs_var(0); - _aid_src_ev_pos.observation_variance[1] = ev_pos_obs_var(1); - - _aid_src_ev_pos.innovation_variance[0] = P(7, 7) + _aid_src_ev_pos.observation_variance[0]; - _aid_src_ev_pos.innovation_variance[1] = P(8, 8) + _aid_src_ev_pos.observation_variance[1]; - } - } else { - // use the absolute position - Vector3f ev_pos_meas = _ev_sample_delayed.pos; - Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar); - - if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) { - ev_pos_meas = _R_ev_to_ekf * ev_pos_meas; - ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose(); - } - - _aid_src_ev_pos.observation[0] = ev_pos_meas(0); - _aid_src_ev_pos.observation[1] = ev_pos_meas(1); - - _aid_src_ev_pos.observation_variance[0] = fmaxf(ev_pos_var(0, 0), sq(0.01f)); - _aid_src_ev_pos.observation_variance[1] = fmaxf(ev_pos_var(1, 1), sq(0.01f)); - - _aid_src_ev_pos.innovation[0] = _state.pos(0) - _aid_src_ev_pos.observation[0]; - _aid_src_ev_pos.innovation[1] = _state.pos(1) - _aid_src_ev_pos.observation[1]; - - _aid_src_ev_pos.innovation_variance[0] = P(7, 7) + _aid_src_ev_pos.observation_variance[0]; - _aid_src_ev_pos.innovation_variance[1] = P(8, 8) + _aid_src_ev_pos.observation_variance[1]; - - // check if we have been deadreckoning too long - if (isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)) { - resetHorizontalPositionToVision(); - } - } - - // innovation gate size - const float ev_pos_innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.0f); - setEstimatorAidStatusTestRatio(_aid_src_ev_pos, ev_pos_innov_gate); - - _aid_src_ev_pos.timestamp_sample = _ev_sample_delayed.time_us; - _aid_src_ev_pos.fusion_enabled = true; - - fuseHorizontalPosition(_aid_src_ev_pos); - } - - // determine if we should use the yaw observation - resetEstimatorAidStatus(_aid_src_ev_yaw); - const float measured_hdg = getEulerYaw(_ev_sample_delayed.quat); - const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.orientation_var(2), 1.e-4f); - - if (PX4_ISFINITE(measured_hdg)) { - _aid_src_ev_yaw.timestamp_sample = _ev_sample_delayed.time_us; - _aid_src_ev_yaw.observation = measured_hdg; - _aid_src_ev_yaw.observation_variance = ev_yaw_obs_var; - _aid_src_ev_yaw.fusion_enabled = _control_status.flags.ev_yaw; - - if (_control_status.flags.ev_yaw) { - if (reset && _control_status_prev.flags.ev_yaw) { - resetYawToEv(); - } - - const float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg); - - fuseYaw(innovation, ev_yaw_obs_var, _aid_src_ev_yaw); - - } else { - // populate estimator_aid_src_ev_yaw with delta heading innovations for logging - // use the change in yaw since the last measurement - const float measured_hdg_prev = getEulerYaw(_ev_sample_delayed_prev.quat); - - // calculate the change in yaw since the last measurement - const float ev_delta_yaw = wrap_pi(measured_hdg - measured_hdg_prev); - - _aid_src_ev_yaw.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _yaw_pred_prev - ev_delta_yaw); - } - } - - - bool ev_reset = (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter); - - // determine if we should use the horizontal position observations - bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (_ev_sample_delayed.quality >= _params.ev_quality_minimum); - - const bool starting_conditions_passing = quality_sufficient - && ((_ev_sample_delayed.time_us - _ev_sample_delayed_prev.time_us) < EV_MAX_INTERVAL) - && ((_params.ev_quality_minimum <= 0) || (_ev_sample_delayed_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient - && ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient - && isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL); - - // EV velocity - controlEvVelFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel); - - // EV height - controlEvHeightFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt); - - - // record observation and estimate for use next time - _ev_sample_delayed_prev = _ev_sample_delayed; - _hpos_pred_prev = _state.pos.xy(); - _yaw_pred_prev = getEulerYaw(_state.quat_nominal); - - } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw || _control_status.flags.ev_hgt) - && !isRecent(_ev_sample_delayed.time_us, (uint64_t)_params.reset_timeout_max)) { - - // Turn off EV fusion mode if no data has been received - stopEvFusion(); - _warning_events.flags.vision_data_stopped = true; - ECL_WARN("vision data stopped"); - } -} - void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing) { if (!(_params.gnss_ctrl & GnssCtrl::YAW) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b3d2a832b4..83b7d8b108 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -48,6 +48,7 @@ #include "EKFGSF_yaw.h" #include "bias_estimator.hpp" #include "height_bias_estimator.hpp" +#include "position_bias_estimator.hpp" #include #include @@ -379,9 +380,6 @@ public: // return a bitmask integer that describes which state estimates can be used for flight control void get_ekf_soln_status(uint16_t *status) const; - // return the quaternion defining the rotation from the External Vision to the EKF reference frame - matrix::Quatf getVisionAlignmentQuaternion() const { return Quatf(_R_ev_to_ekf); }; - // use the latest IMU data at the current time horizon. Quatf calculate_quaternion() const; @@ -410,6 +408,8 @@ public: const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); } const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); } + const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); } + const auto &aid_src_airspeed() const { return _aid_src_airspeed; } const auto &aid_src_sideslip() const { return _aid_src_sideslip; } @@ -476,23 +476,18 @@ private: bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised - // variables used when position data is being fused using a relative position odometry model - bool _fuse_hpos_as_odom{false}; ///< true when the NE position data is being fused using an odometry assumption - Vector2f _hpos_pred_prev{}; ///< previous value of NE position state used by odometry fusion (m) - float _yaw_pred_prev{}; ///< previous value of yaw state used by odometry fusion (m) - bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use - Dcmf _R_ev_to_ekf; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity + float _ev_yaw_pred_prev{}; ///< previous value of yaw state used by odometry fusion (m) bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North) // booleans true when fresh sensor data is available at the fusion time horizon bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused bool _rng_data_ready{false}; bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon - bool _ev_data_ready{false}; ///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused bool _tas_data_ready{false}; ///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator uint64_t _time_prev_gps_us{0}; ///< time stamp of previous GPS data retrieved from the buffer (uSec) + uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) uint64_t _time_last_v_pos_aiding{0}; uint64_t _time_last_v_vel_aiding{0}; @@ -508,7 +503,9 @@ private: uint64_t _time_last_healthy_rng_data{0}; uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source + uint8_t _nb_ev_pos_reset_available{0}; uint8_t _nb_ev_vel_reset_available{0}; + uint8_t _nb_ev_yaw_reset_available{0}; Vector3f _last_known_pos{}; ///< last known local position vector (m) @@ -708,7 +705,6 @@ private: void resetHorizontalVelocityToZero(); void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var); - void resetHorizontalPositionToVision(); void resetHorizontalPositionToLastKnown(); void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var); @@ -776,19 +772,12 @@ private: // return true if successful bool resetMagHeading(); - // reset the heading using the external vision measurements - // return true if successful - bool resetYawToEv(); - // Return the magnetic declination in radians to be used by the alignment and fusion processing float getMagDeclination(); // modify output filter to match the the EKF state at the fusion time horizon void alignOutputFilter(); - // update the rotation matrix which transforms EV navigation frame measurements into NED - void calcExtVisRotMat(); - bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation) { for (unsigned i = 0; i < 3; i++) { @@ -851,8 +840,16 @@ private: // control fusion of external vision observations void controlExternalVisionFusion(); + void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s& aid_src); + + void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s& aid_src); + void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src); + void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src); + void stopEvPosFusion(); + void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s& aid_src); + void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s& aid_src); // control fusion of optical flow observations void controlOpticalFlowFusion(); @@ -1008,11 +1005,6 @@ private: void startGpsYawFusion(const gpsSample &gps_sample); void stopGpsYawFusion(); - void startEvPosFusion(); - void startEvYawFusion(); - - void stopEvFusion(); - void stopEvPosFusion(); void stopEvVelFusion(); void stopEvYawFusion(); @@ -1036,11 +1028,13 @@ private: EKFGSF_yaw _yawEstimator{}; uint8_t _height_sensor_ref{HeightSensor::UNKNOWN}; + uint8_t _position_sensor_ref{static_cast(PositionSensor::GNSS)}; HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref}; HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref}; HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref}; HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref}; + PositionBiasEstimator _ev_pos_b_est{static_cast(PositionSensor::EV), _position_sensor_ref}; void runYawEKFGSF(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 900a334776..b1c797cdf2 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -124,22 +124,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var) _time_last_ver_vel_fuse = _imu_sample_delayed.time_us; } -void Ekf::resetHorizontalPositionToVision() -{ - _information_events.flags.reset_pos_to_vision = true; - ECL_INFO("reset position to ev position"); - Vector3f _ev_pos = _ev_sample_delayed.pos; - - if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) { - _ev_pos = _R_ev_to_ekf * _ev_sample_delayed.pos; - } - - resetHorizontalPositionTo(Vector2f(_ev_pos), _ev_sample_delayed.posVar.slice<2, 1>(0, 0)); - - // let the next odometry update know that the previous value of states cannot be used to calculate the change in position - _hpos_prev_available = false; -} - void Ekf::resetHorizontalPositionToLastKnown() { _information_events.flags.reset_pos_to_last_known = true; @@ -178,6 +162,9 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f _state_reset_status.reset_count.posNE++; + _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change); + //_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change); + // Reset the timout timer _time_last_hor_pos_fuse = _imu_sample_delayed.time_us; } @@ -193,7 +180,6 @@ bool Ekf::isHeightResetRequired() const return (continuous_bad_accel_hgt || hgt_fusion_timeout); } - void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var) { const float old_vert_pos = _state.pos(2); @@ -275,8 +261,8 @@ void Ekf::alignOutputFilter() bool Ekf::resetMagHeading() { // prevent a reset being performed more than once on the same frame - if (_imu_sample_delayed.time_us == _flt_mag_align_start_time) { - return true; + if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) { + return false; } const Vector3f mag_init = _mag_lpf.getState(); @@ -320,17 +306,6 @@ bool Ekf::resetMagHeading() return false; } -bool Ekf::resetYawToEv() -{ - const float yaw_new = getEulerYaw(_ev_sample_delayed.quat); - const float yaw_new_variance = fmaxf(_ev_sample_delayed.orientation_var(2), sq(1.0e-2f)); - - resetQuatStateYaw(yaw_new, yaw_new_variance); - _R_ev_to_ekf.setIdentity(); - - return true; -} - // Return the magnetic declination in radians to be used by the alignment and fusion processing float Ekf::getMagDeclination() { @@ -755,6 +730,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f } else if (_control_status.flags.gps_yaw) { mag = sqrtf(_aid_src_gnss_yaw.test_ratio); + } else { mag = NAN; } @@ -1092,7 +1068,7 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) void Ekf::stopMagFusion() { if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { - ECL_INFO("stopping mag fusion"); + ECL_INFO("stopping all mag fusion"); stopMag3DFusion(); stopMagHdgFusion(); clearMagCov(); @@ -1166,14 +1142,6 @@ void Ekf::updateGroundEffect() } } -// update the rotation matrix which rotates EV measurements into the EKF's navigation frame -void Ekf::calcExtVisRotMat() -{ - // Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon. - const Quatf q_error((_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized()); - _R_ev_to_ekf = Dcmf(q_error); -} - // Increase the yaw error variance of the quaternions // Argument is additional yaw variance in rad**2 void Ekf::increaseQuatYawErrVariance(float yaw_variance) @@ -1320,52 +1288,6 @@ void Ekf::stopGpsYawFusion() } } -void Ekf::startEvPosFusion() -{ - _control_status.flags.ev_pos = true; - resetHorizontalPositionToVision(); - _information_events.flags.starting_vision_pos_fusion = true; - ECL_INFO("starting vision pos fusion"); -} - -void Ekf::startEvYawFusion() -{ - // turn on fusion of external vision yaw measurements and disable all magnetometer fusion - _control_status.flags.ev_yaw = true; - _control_status.flags.mag_dec = false; - - stopMagHdgFusion(); - stopMag3DFusion(); - - _information_events.flags.starting_vision_yaw_fusion = true; - ECL_INFO("starting vision yaw fusion"); -} - -void Ekf::stopEvFusion() -{ - stopEvPosFusion(); - stopEvVelFusion(); - stopEvYawFusion(); - stopEvHgtFusion(); -} - -void Ekf::stopEvPosFusion() -{ - if (_control_status.flags.ev_pos) { - ECL_INFO("stopping EV pos fusion"); - _control_status.flags.ev_pos = false; - resetEstimatorAidStatus(_aid_src_ev_pos); - } -} - -void Ekf::stopEvYawFusion() -{ - if (_control_status.flags.ev_yaw) { - ECL_INFO("stopping EV yaw fusion"); - _control_status.flags.ev_yaw = false; - } -} - void Ekf::stopAuxVelFusion() { ECL_INFO("stopping aux vel fusion"); diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 3954d2adcd..20d41830c7 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -542,7 +542,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms); } - if ((_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW)) || (_params.ev_ctrl > 0)) { + if (_params.ev_ctrl > 0) { max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms); } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 5dd181bde9..92a3034b15 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -258,7 +258,6 @@ public: const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); } - const extVisionSample &get_ev_sample_delayed() const { return _ev_sample_delayed; } const bool &global_origin_valid() const { return _NED_origin_initialised; } const MapProjection &global_origin() const { return _pos_ref; } @@ -305,8 +304,7 @@ protected: sensor::SensorRangeFinder _range_sensor{}; airspeedSample _airspeed_sample_delayed{}; flowSample _flow_sample_delayed{}; - extVisionSample _ev_sample_delayed{}; - extVisionSample _ev_sample_delayed_prev{}; + extVisionSample _ev_sample_prev{}; dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) RangeFinderConsistencyCheck _rng_consistency_check; diff --git a/src/modules/ekf2/EKF/ev_control.cpp b/src/modules/ekf2/EKF/ev_control.cpp new file mode 100644 index 0000000000..b5fbafb778 --- /dev/null +++ b/src/modules/ekf2/EKF/ev_control.cpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ev_control.cpp + * Control functions for ekf external vision control + */ + +#include "ekf.h" + +void Ekf::controlExternalVisionFusion() +{ + _ev_pos_b_est.predict(_dt_ekf_avg); + + // Check for new external vision data + extVisionSample ev_sample; + + if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &ev_sample)) { + + bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter); + + // determine if we should use the horizontal position observations + bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (ev_sample.quality >= _params.ev_quality_minimum); + + const bool starting_conditions_passing = quality_sufficient + && ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL) + && ((_params.ev_quality_minimum <= 0) || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient + && ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient + && isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL); + + controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw); + controlEvVelFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel); + controlEvPosFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos); + controlEvHeightFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt); + + if (quality_sufficient) { + _ev_sample_prev = ev_sample; + } + + // record corresponding yaw state for future EV delta heading innovation (logging only) + _ev_yaw_pred_prev = getEulerYaw(_state.quat_nominal); + + } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw + || _control_status.flags.ev_hgt) + && isTimedOut(_ev_sample_prev.time_us, 2 * EV_MAX_INTERVAL)) { + + // Turn off EV fusion mode if no data has been received + stopEvPosFusion(); + stopEvVelFusion(); + stopEvYawFusion(); + stopEvHgtFusion(); + + _warning_events.flags.vision_data_stopped = true; + ECL_WARN("vision data stopped"); + } +} diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index 1335d4f0cc..f03204e394 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -53,7 +53,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com // rotate measurement into correct earth frame if required Vector3f pos{ev_sample.pos}; - Matrix3f pos_cov{matrix::diag(ev_sample.posVar)}; + Matrix3f pos_cov{matrix::diag(ev_sample.position_var)}; // rotate EV to the EKF reference frame unless we're operating entirely in vision frame // TODO: only necessary if there's a roll/pitch offset between VIO and EKF @@ -65,7 +65,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com const Dcmf R_ev_to_ekf(q_error); pos = R_ev_to_ekf * ev_sample.pos; - pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.posVar) * R_ev_to_ekf.transpose(); + pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose(); // increase minimum variance to include EV orientation variance // TODO: do this properly @@ -98,9 +98,11 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9)); } - const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VPOS)) && measurement_valid; + const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VPOS)) + && measurement_valid; - const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing; + const bool starting_conditions_passing = common_starting_conditions_passing + && continuing_conditions_passing; if (_control_status.flags.ev_hgt) { aid_src.fusion_enabled = true; diff --git a/src/modules/ekf2/EKF/ev_pos_control.cpp b/src/modules/ekf2/EKF/ev_pos_control.cpp new file mode 100644 index 0000000000..bdbbe034ff --- /dev/null +++ b/src/modules/ekf2/EKF/ev_pos_control.cpp @@ -0,0 +1,304 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ev_pos_control.cpp + * Control functions for ekf external vision position fusion + */ + +#include "ekf.h" + +static constexpr const char *EV_AID_SRC_NAME = "EV position"; + + +void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, + const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src) +{ + const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw) + || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); + + // determine if we should use EV position aiding + bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::HPOS)) + && _control_status.flags.tilt_align + && PX4_ISFINITE(ev_sample.pos(0)) + && PX4_ISFINITE(ev_sample.pos(1)); + + // correct position for offset relative to IMU + const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; + const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + + const bool bias_fusion_was_active = _ev_pos_b_est.fusionActive(); + + // rotate measurement into correct earth frame if required + Vector3f pos{NAN, NAN, NAN}; + Matrix3f pos_cov{}; + + switch (ev_sample.pos_frame) { + case PositionFrame::LOCAL_FRAME_NED: + if (_control_status.flags.yaw_align) { + pos = ev_sample.pos - pos_offset_earth; + pos_cov = matrix::diag(ev_sample.position_var); + + if (_control_status.flags.gps) { + _ev_pos_b_est.setFusionActive(); + + } else { + _ev_pos_b_est.setFusionInactive(); + } + + } else { + continuing_conditions_passing = false; + _ev_pos_b_est.setFusionInactive(); + _ev_pos_b_est.reset(); + } + + break; + + case PositionFrame::LOCAL_FRAME_FRD: + if (_control_status.flags.ev_yaw) { + // using EV frame + pos = ev_sample.pos - pos_offset_earth; + pos_cov = matrix::diag(ev_sample.position_var); + + _ev_pos_b_est.setFusionInactive(); + _ev_pos_b_est.reset(); + + } else { + // rotate EV to the EKF reference frame + const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized()); + const Dcmf R_ev_to_ekf = Dcmf(q_error); + + pos = R_ev_to_ekf * ev_sample.pos - pos_offset_earth; + pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose(); + + // increase minimum variance to include EV orientation variance + // TODO: do this properly + const float orientation_var_max = ev_sample.orientation_var.max(); + + for (int i = 0; i < 2; i++) { + pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max); + } + + if (_control_status.flags.gps) { + _ev_pos_b_est.setFusionActive(); + + } else { + _ev_pos_b_est.setFusionInactive(); + } + } + + break; + + default: + continuing_conditions_passing = false; + _ev_pos_b_est.setFusionInactive(); + _ev_pos_b_est.reset(); + break; + } + + // increase minimum variance if GPS active (position reference) + if (_control_status.flags.gps) { + for (int i = 0; i < 2; i++) { + pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise)); + } + } + + const Vector2f measurement{pos(0), pos(1)}; + + const Vector2f measurement_var{ + math::max(pos_cov(0, 0), sq(_params.ev_pos_noise), sq(0.01f)), + math::max(pos_cov(1, 1), sq(_params.ev_pos_noise), sq(0.01f)) + }; + + const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite(); + + // bias fusion activated (GPS activated) + if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) { + if (quality_sufficient) { + // reset the bias estimator + _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + + } else if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.ev_pos)) { + // otherwise stop EV position, when quality is good again it will restart with reset bias + stopEvPosFusion(); + } + } + + updateHorizontalPositionAidSrcStatus(ev_sample.time_us, + measurement - _ev_pos_b_est.getBias(), // observation + measurement_var + _ev_pos_b_est.getBiasVar(), // observation variance + math::max(_params.ev_pos_innov_gate, 1.f), // innovation gate + aid_src); + + // update the bias estimator before updating the main filter but after + // using its current state to compute the vertical position innovation + if (measurement_valid && quality_sufficient) { + _ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1)))); + _ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO + _ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(P(7, 7), P(8, 8))); + } + + if (!measurement_valid) { + continuing_conditions_passing = false; + } + + const bool starting_conditions_passing = common_starting_conditions_passing + && continuing_conditions_passing; + + if (_control_status.flags.ev_pos) { + aid_src.fusion_enabled = true; + + if (continuing_conditions_passing) { + const bool bias_estimator_change = (bias_fusion_was_active != _ev_pos_b_est.fusionActive()); + const bool reset = ev_reset || yaw_alignment_changed || bias_estimator_change; + + updateEvPosFusion(measurement, measurement_var, quality_sufficient, reset, aid_src); + + } else { + // Stop fusion but do not declare it faulty + ECL_WARN("stopping %s fusion, continuing conditions failing", EV_AID_SRC_NAME); + stopEvPosFusion(); + } + + } else { + if (starting_conditions_passing) { + startEvPosFusion(measurement, measurement_var, aid_src); + } + } +} + +void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src) +{ + // activate fusion + // TODO: (_params.position_sensor_ref == PositionSensor::EV) + if (_control_status.flags.gps) { + ECL_INFO("starting %s fusion", EV_AID_SRC_NAME); + _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setFusionActive(); + + } else { + ECL_INFO("starting %s fusion, resetting state", EV_AID_SRC_NAME); + //_position_sensor_ref = PositionSensor::EV; + _information_events.flags.reset_pos_to_vision = true; + resetHorizontalPositionTo(measurement, measurement_var); + _ev_pos_b_est.reset(); + } + + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + + _nb_ev_pos_reset_available = 5; + _information_events.flags.starting_vision_pos_fusion = true; + _control_status.flags.ev_pos = true; +} + +void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src) +{ + if (reset) { + + if (quality_sufficient) { + + if (!_control_status.flags.gps) { + ECL_INFO("reset to %s", EV_AID_SRC_NAME); + _information_events.flags.reset_pos_to_vision = true; + resetHorizontalPositionTo(measurement, measurement_var); + _ev_pos_b_est.reset(); + + } else { + _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + } + + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + + } else { + // EV has reset, but quality isn't sufficient + // we have no choice but to stop EV and try to resume once quality is acceptable + stopEvPosFusion(); + return; + } + + } else if (quality_sufficient) { + fuseHorizontalPosition(aid_src); + + } else { + aid_src.innovation_rejected = true; + } + + const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.no_aid_timeout_max); // 1 second + + if (is_fusion_failing) { + bool pos_xy_fusion_failing = isTimedOut(_time_last_hor_pos_fuse, _params.no_aid_timeout_max); + + if ((_nb_ev_pos_reset_available > 0) && quality_sufficient) { + // Data seems good, attempt a reset + ECL_WARN("%s fusion failing, resetting", EV_AID_SRC_NAME); + + if (_control_status.flags.gps && !pos_xy_fusion_failing) { + // reset EV position bias + _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + + } else { + _information_events.flags.reset_pos_to_vision = true; + + if (_control_status.flags.gps) { + resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar()); + _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + + } else { + resetHorizontalPositionTo(measurement, measurement_var); + _ev_pos_b_est.reset(); + } + } + + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + + if (_control_status.flags.in_air) { + _nb_ev_pos_reset_available--; + } + + } else { + // A reset did not fix the issue but all the starting checks are not passing + // This could be a temporary issue, stop the fusion without declaring the sensor faulty + ECL_WARN("stopping %s, fusion failing", EV_AID_SRC_NAME); + stopEvPosFusion(); + } + } +} + +void Ekf::stopEvPosFusion() +{ + if (_control_status.flags.ev_pos) { + resetEstimatorAidStatus(_aid_src_ev_pos); + + _control_status.flags.ev_pos = false; + } +} diff --git a/src/modules/ekf2/EKF/ev_vel_control.cpp b/src/modules/ekf2/EKF/ev_vel_control.cpp index 9638b3228b..f1bddeb03a 100644 --- a/src/modules/ekf2/EKF/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/ev_vel_control.cpp @@ -134,7 +134,8 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common continuing_conditions_passing = false; } - const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing + const bool starting_conditions_passing = common_starting_conditions_passing + && continuing_conditions_passing && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive()); if (_control_status.flags.ev_vel) { diff --git a/src/modules/ekf2/EKF/ev_yaw_control.cpp b/src/modules/ekf2/EKF/ev_yaw_control.cpp new file mode 100644 index 0000000000..64e85cfedc --- /dev/null +++ b/src/modules/ekf2/EKF/ev_yaw_control.cpp @@ -0,0 +1,186 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ev_yaw_control.cpp + * Control functions for ekf external vision yaw fusion + */ + +#include "ekf.h" + +void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, + const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src) +{ + static constexpr const char *AID_SRC_NAME = "EV yaw"; + + resetEstimatorAidStatus(aid_src); + aid_src.timestamp_sample = ev_sample.time_us; + aid_src.observation = getEulerYaw(ev_sample.quat); + aid_src.observation_variance = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f)); + aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation); + + // determine if we should use EV yaw aiding + bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::YAW)) + && _control_status.flags.tilt_align + && !_inhibit_ev_yaw_use + && PX4_ISFINITE(aid_src.observation) + && PX4_ISFINITE(aid_src.observation_variance); + + // if GPS enabled don't allow EV yaw if EV isn't NED + if (_control_status.flags.gps && _control_status.flags.yaw_align + && (ev_sample.pos_frame != PositionFrame::LOCAL_FRAME_NED) + ) { + continuing_conditions_passing = false; + + // use delta yaw for innovation logging + aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _ev_yaw_pred_prev) + - wrap_pi(getEulerYaw(ev_sample.quat) - getEulerYaw(_ev_sample_prev.quat))); + } + + const bool starting_conditions_passing = common_starting_conditions_passing + && continuing_conditions_passing + && isTimedOut(aid_src.time_last_fuse, (uint32_t)1e6); + + if (_control_status.flags.ev_yaw) { + aid_src.fusion_enabled = true; + + if (continuing_conditions_passing) { + + if (ev_reset) { + + if (quality_sufficient) { + ECL_INFO("reset to %s", AID_SRC_NAME); + //_information_events.flags.reset_yaw_to_vision = true; // TODO + resetQuatStateYaw(aid_src.observation, aid_src.observation_variance); + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + + } else { + // EV has reset, but quality isn't sufficient + // we have no choice but to stop EV and try to resume once quality is acceptable + stopEvYawFusion(); + return; + } + + } else if (quality_sufficient) { + fuseYaw(aid_src.innovation, aid_src.observation_variance, aid_src); + + } else { + aid_src.innovation_rejected = true; + } + + const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.no_aid_timeout_max); + + if (is_fusion_failing) { + if ((_nb_ev_yaw_reset_available > 0) && quality_sufficient) { + // Data seems good, attempt a reset + //_information_events.flags.reset_yaw_to_vision = true; // TODO + ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); + resetQuatStateYaw(aid_src.innovation, aid_src.observation_variance); + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + + if (_control_status.flags.in_air) { + _nb_ev_yaw_reset_available--; + } + + } else if (starting_conditions_passing) { + // Data seems good, but previous reset did not fix the issue + // something else must be wrong, declare the sensor faulty and stop the fusion + //_control_status.flags.ev_yaw_fault = true; + ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME); + stopEvYawFusion(); + + } else { + // A reset did not fix the issue but all the starting checks are not passing + // This could be a temporary issue, stop the fusion without declaring the sensor faulty + ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); + stopEvYawFusion(); + } + } + + } else { + // Stop fusion but do not declare it faulty + ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME); + stopEvYawFusion(); + } + + } else { + if (starting_conditions_passing) { + // activate fusion + if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_NED) { + + if (_control_status.flags.yaw_align) { + ECL_INFO("starting %s fusion", AID_SRC_NAME); + + } else { + // reset yaw to EV and set yaw_align + ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME); + resetQuatStateYaw(aid_src.observation, aid_src.observation_variance); + _control_status.flags.yaw_align = true; + } + + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + _information_events.flags.starting_vision_yaw_fusion = true; + _control_status.flags.ev_yaw = true; + + } else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) { + // turn on fusion of external vision yaw measurements and disable all other heading fusion + stopMagFusion(); + stopGpsYawFusion(); + stopGpsFusion(); + + ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME); + + // reset yaw to EV + resetQuatStateYaw(aid_src.observation, aid_src.observation_variance); + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + + _information_events.flags.starting_vision_yaw_fusion = true; + _control_status.flags.yaw_align = false; + _control_status.flags.ev_yaw = true; + } + + if (_control_status.flags.ev_yaw) { + _nb_ev_yaw_reset_available = 5; + } + } + } +} + +void Ekf::stopEvYawFusion() +{ + if (_control_status.flags.ev_yaw) { + resetEstimatorAidStatus(_aid_src_ev_yaw); + + _control_status.flags.ev_yaw = false; + } +} diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/fake_pos_control.cpp index ed44b0d197..f6a013d086 100644 --- a/src/modules/ekf2/EKF/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/fake_pos_control.cpp @@ -95,7 +95,6 @@ void Ekf::controlFakePosFusion() if (starting_conditions_passing) { ECL_INFO("start fake position fusion"); _control_status.flags.fake_pos = true; - _fuse_hpos_as_odom = false; // TODO: needed? resetFakePosFusion(); if (_control_status.flags.tilt_align) { diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index be8102d02d..481319023c 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -57,16 +57,18 @@ void Ekf::controlGpsFusion() controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing); // GNSS velocity + const Vector3f velocity{gps_sample.vel}; const float vel_var = sq(math::max(gps_sample.sacc, _params.gps_vel_noise)); const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f)); updateVelocityAidSrcStatus(gps_sample.time_us, - gps_sample.vel, // observation + velocity, // observation vel_obs_var, // observation variance math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate _aid_src_gnss_vel); _aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL); // GNSS position + const Vector2f position{gps_sample.pos}; // relax the upper observation noise limit which prevents bad GPS perturbing the position estimate float pos_noise = math::max(gps_sample.hacc, _params.gps_pos_noise); @@ -81,12 +83,34 @@ void Ekf::controlGpsFusion() const float pos_var = sq(pos_noise); const Vector2f pos_obs_var(pos_var, pos_var); updateHorizontalPositionAidSrcStatus(gps_sample.time_us, - gps_sample.pos, // observation + position, // observation pos_obs_var, // observation variance math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate _aid_src_gnss_pos); _aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS); + // if GPS is otherwise ready to go, but yaw_align is blocked by EV give mag a chance to start + if (_control_status.flags.tilt_align && _NED_origin_initialised + && gps_checks_passing && !gps_checks_failing) { + + if (!_control_status.flags.yaw_align) { + if (_control_status.flags.ev_yaw && !_control_status.flags.yaw_align) { + + // give mag a chance to start and yaw align if currently blocked by EV yaw + const bool mag_enabled = (_params.mag_fusion_type <= MagFuseType::MAG_3D); + const bool mag_available = (_mag_counter != 0); + + if (mag_enabled && mag_available + && !_control_status.flags.mag_field_disturbed + && !_control_status.flags.mag_fault) { + + stopEvYawFusion(); + } + } + } + } + + // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently const bool mandatory_conditions_passing = ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL)) @@ -134,12 +158,6 @@ void Ekf::controlGpsFusion() stopGpsFusion(); _warning_events.flags.gps_quality_poor = true; ECL_WARN("GPS quality poor - stopping use"); - - // TODO: move this to EV control logic - // Reset position state to external vision if we are going to use absolute values - if (_control_status.flags.ev_pos && !(_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS)) { - resetHorizontalPositionToVision(); - } } } else { // mandatory conditions are not passing @@ -150,44 +168,48 @@ void Ekf::controlGpsFusion() if (starting_conditions_passing) { // Do not use external vision for yaw if using GPS because yaw needs to be // defined relative to an NED reference frame - if (_control_status.flags.ev_yaw - || _mag_inhibit_yaw_reset_req - || _mag_yaw_reset_req) { - - _mag_yaw_reset_req = true; - + if (_control_status.flags.ev_yaw) { // Stop the vision for yaw fusion and do not allow it to start again stopEvYawFusion(); _inhibit_ev_yaw_use = true; - - } else { - ECL_INFO("starting GPS fusion"); - _information_events.flags.starting_gps_fusion = true; - - // reset position - _information_events.flags.reset_pos_to_gps = true; - resetHorizontalPositionTo(gps_sample.pos, pos_obs_var); - - // when already using another velocity source velocity reset is not necessary - if (!isHorizontalAidingActive()) { - _information_events.flags.reset_vel_to_gps = true; - resetVelocityTo(gps_sample.vel, vel_obs_var); - } - - _control_status.flags.gps = true; } + ECL_INFO("starting GPS fusion"); + _information_events.flags.starting_gps_fusion = true; + + // when already using another velocity source velocity reset is not necessary + if (!isHorizontalAidingActive() + || isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) + || !_control_status_prev.flags.yaw_align + ) { + // reset velocity + _information_events.flags.reset_vel_to_gps = true; + resetVelocityTo(velocity, vel_obs_var); + _aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us; + } + + // reset position + _information_events.flags.reset_pos_to_gps = true; + resetHorizontalPositionTo(position, pos_obs_var); + _aid_src_gnss_pos.time_last_fuse = _imu_sample_delayed.time_us; + + _control_status.flags.gps = true; + } else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) { // If no mag is used, align using the yaw estimator (if available) if (resetYawToEKFGSF()) { _information_events.flags.yaw_aligned_to_imu_gps = true; - ECL_INFO("Yaw aligned using IMU and GPS"); + ECL_INFO("GPS yaw aligned using IMU, resetting vel and pos"); - ECL_INFO("reset velocity and position to GPS"); + // reset velocity _information_events.flags.reset_vel_to_gps = true; + resetVelocityTo(velocity, vel_obs_var); + _aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us; + + // reset position _information_events.flags.reset_pos_to_gps = true; - resetVelocityTo(gps_sample.vel, vel_obs_var); - resetHorizontalPositionTo(gps_sample.pos, pos_obs_var); + resetHorizontalPositionTo(position, pos_obs_var); + _aid_src_gnss_pos.time_last_fuse = _imu_sample_delayed.time_us; } } } diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 207b04ff8e..31018211a2 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -218,6 +218,12 @@ bool Ekf::canResetMagHeading() const void Ekf::runInAirYawReset() { + // prevent a reset being performed more than once on the same frame + if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us) + || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) { + return; + } + if (_mag_yaw_reset_req && !_is_yaw_fusion_inhibited) { bool has_realigned_yaw = false; @@ -252,6 +258,8 @@ void Ekf::runInAirYawReset() ); resetMagCov(); + + has_realigned_yaw = true; } if (!has_realigned_yaw && canResetMagHeading()) { @@ -296,14 +304,19 @@ void Ekf::check3DMagFusionSuitability() void Ekf::checkYawAngleObservability() { - // Check if there has been enough change in horizontal velocity to make yaw observable - // Apply hysteresis to check to avoid rapid toggling - _yaw_angle_observable = _yaw_angle_observable - ? _accel_lpf_NE.norm() > _params.mag_acc_gate - : _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate; + if (_control_status.flags.gps) { + // Check if there has been enough change in horizontal velocity to make yaw observable + // Apply hysteresis to check to avoid rapid toggling + if (_yaw_angle_observable) { + _yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate; - _yaw_angle_observable = _yaw_angle_observable - && (_control_status.flags.gps || _control_status.flags.ev_pos); // Do we have to add ev_vel here? + } else { + _yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate * 2.f; + } + + } else { + _yaw_angle_observable = false; + } } void Ekf::checkMagBiasObservability() @@ -366,9 +379,7 @@ bool Ekf::shouldInhibitMag() const // has explicitly stopped magnetometer use. const bool user_selected = (_params.mag_fusion_type == MagFuseType::INDOOR); - const bool heading_not_required_for_navigation = !_control_status.flags.gps - && !_control_status.flags.ev_pos - && !_control_status.flags.ev_vel; + const bool heading_not_required_for_navigation = !_control_status.flags.gps; return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed; } diff --git a/src/modules/ekf2/EKF/position_bias_estimator.hpp b/src/modules/ekf2/EKF/position_bias_estimator.hpp new file mode 100644 index 0000000000..ddcbe144ce --- /dev/null +++ b/src/modules/ekf2/EKF/position_bias_estimator.hpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file position_bias_estimator.hpp + */ + +#pragma once + +#include "bias_estimator.hpp" + +class PositionBiasEstimator +{ +public: + PositionBiasEstimator(uint8_t sensor, const uint8_t &sensor_ref): + _sensor(sensor), + _sensor_ref(sensor_ref) + {} + virtual ~PositionBiasEstimator() = default; + + bool fusionActive() const { return _is_sensor_fusion_active; } + + void setFusionActive() { _is_sensor_fusion_active = true; } + void setFusionInactive() { _is_sensor_fusion_active = false; } + + void predict(float dt) + { + if ((_sensor_ref != _sensor) && _is_sensor_fusion_active) { + _bias[0].predict(dt); + _bias[1].predict(dt); + } + } + + void fuseBias(Vector2f bias, Vector2f bias_var) + { + if ((_sensor_ref != _sensor) && _is_sensor_fusion_active) { + _bias[0].fuseBias(bias(0), bias_var(0)); + _bias[1].fuseBias(bias(1), bias_var(1)); + } + } + + void setBias(const Vector2f &bias) + { + _bias[0].setBias(bias(0)); + _bias[1].setBias(bias(1)); + } + + void setProcessNoiseSpectralDensity(float nsd) + { + _bias[0].setProcessNoiseSpectralDensity(nsd); + _bias[1].setProcessNoiseSpectralDensity(nsd); + } + // void setBiasStdDev(float state_noise) { _state_var = state_noise * state_noise; } + // void setInnovGate(float gate_size) { _gate_size = gate_size; } + + void setMaxStateNoise(const Vector2f &max_noise) + { + _bias[0].setMaxStateNoise(max_noise(0)); + _bias[1].setMaxStateNoise(max_noise(1)); + } + + Vector2f getBias() const { return Vector2f(_bias[0].getBias(), _bias[1].getBias()); } + float getBias(int index) const { return _bias[index].getBias(); } + + Vector2f getBiasVar() const { return Vector2f(_bias[0].getBiasVar(), _bias[1].getBiasVar()); } + float getBiasVar(int index) const { return _bias[index].getBiasVar(); } + + const BiasEstimator::status &getStatus(int index) const { return _bias[index].getStatus(); } + + void reset() + { + _bias[0].reset(); + _bias[1].reset(); + } + +private: + BiasEstimator _bias[2] {}; + + const uint8_t _sensor; + const uint8_t &_sensor_ref; + + bool _is_sensor_fusion_active{false}; // TODO: replace by const ref and remove setter when migrating _control_status.flags from union to bool +}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index bb794cb9f1..02b10ed5be 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -207,23 +207,67 @@ bool EKF2::multi_init(int imu, int mag) { // advertise all topics to ensure consistent uORB instance numbering _ekf2_timestamps_pub.advertise(); - _estimator_baro_bias_pub.advertise(); - _estimator_ev_hgt_bias_pub.advertise(); _estimator_event_flags_pub.advertise(); - _estimator_gnss_hgt_bias_pub.advertise(); - _estimator_gps_status_pub.advertise(); _estimator_innovation_test_ratios_pub.advertise(); _estimator_innovation_variances_pub.advertise(); _estimator_innovations_pub.advertise(); _estimator_optical_flow_vel_pub.advertise(); - _estimator_rng_hgt_bias_pub.advertise(); _estimator_sensor_bias_pub.advertise(); _estimator_states_pub.advertise(); _estimator_status_flags_pub.advertise(); _estimator_status_pub.advertise(); - _estimator_visual_odometry_aligned_pub.advertise(); _yaw_est_pub.advertise(); + // baro advertise + if (_param_ekf2_baro_ctrl.get()) { + _estimator_aid_src_baro_hgt_pub.advertise(); + _estimator_baro_bias_pub.advertise(); + } + + // EV advertise + if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::VPOS)) { + _estimator_aid_src_ev_hgt_pub.advertise(); + _estimator_ev_pos_bias_pub.advertise(); + } + + if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::HPOS)) { + _estimator_aid_src_ev_pos_pub.advertise(); + _estimator_ev_pos_bias_pub.advertise(); + } + + if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::VEL)) { + _estimator_aid_src_ev_vel_pub.advertise(); + } + + if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::YAW)) { + _estimator_aid_src_ev_yaw_pub.advertise(); + } + + // GNSS advertise + if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::VPOS)) { + _estimator_aid_src_gnss_hgt_pub.advertise(); + _estimator_gnss_hgt_bias_pub.advertise(); + } + + if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::HPOS)) { + _estimator_aid_src_gnss_pos_pub.advertise(); + _estimator_gps_status_pub.advertise(); + } + + if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::VEL)) { + _estimator_aid_src_gnss_vel_pub.advertise(); + } + + if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::YAW)) { + _estimator_aid_src_gnss_yaw_pub.advertise(); + } + + // RNG advertise + if (_param_ekf2_rng_ctrl.get()) { + _estimator_aid_src_rng_hgt_pub.advertise(); + _estimator_rng_hgt_bias_pub.advertise(); + } + _attitude_pub.advertise(); _local_position_pub.advertise(); _global_position_pub.advertise(); @@ -545,15 +589,13 @@ void EKF2::Run() UpdateAirspeedSample(ekf2_timestamps); UpdateAuxVelSample(ekf2_timestamps); UpdateBaroSample(ekf2_timestamps); + UpdateExtVisionSample(ekf2_timestamps); UpdateFlowSample(ekf2_timestamps); UpdateGpsSample(ekf2_timestamps); UpdateMagSample(ekf2_timestamps); UpdateRangeSample(ekf2_timestamps); UpdateSystemFlagsSample(ekf2_timestamps); - vehicle_odometry_s ev_odom; - const bool new_ev_odom = UpdateExtVisionSample(ekf2_timestamps, ev_odom); - // run the EKF update and output const hrt_abstime ekf_update_start = hrt_absolute_time(); @@ -569,7 +611,7 @@ void EKF2::Run() PublishBaroBias(now); PublishGnssHgtBias(now); PublishRngHgtBias(now); - PublishEvHgtBias(now); + PublishEvPosBias(now); PublishEventFlags(now); PublishGpsStatus(now); PublishInnovations(now); @@ -593,11 +635,6 @@ void EKF2::Run() perf_set_elapsed(_ecl_ekf_update_perf, hrt_elapsed_time(&ekf_update_start)); } - // publish external visual odometry after fixed frame alignment if new odometry is received - if (new_ev_odom) { - PublishOdometryAligned(now, ev_odom); - } - // publish ekf2_timestamps _ekf2_timestamps_pub.publish(ekf2_timestamps); } @@ -666,12 +703,32 @@ void EKF2::VerifyParams() } // EV EKF2_AID_MASK -> EKF2_EV_CTRL - if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)) { + if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL) + || (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_POS) + || (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_YAW) + ) { - _param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast(EvCtrl::VEL)); - _param_ekf2_ev_ctrl.commit(); + // EKF2_EV_CTRL set VEL bit + if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)) { + _param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast(EvCtrl::VEL)); + } + + // EKF2_EV_CTRL set HPOS/VPOS bits + if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_POS)) { + _param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() + | static_cast(EvCtrl::HPOS) | static_cast(EvCtrl::VPOS)); + } + + // EKF2_EV_CTRL set YAW bit + if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_YAW)) { + _param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast(EvCtrl::YAW)); + } _param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)); + _param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_POS)); + _param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_YAW)); + + _param_ekf2_ev_ctrl.commit(); _param_ekf2_aid_mask.commit(); mavlink_log_critical(&_mavlink_log_pub, "EKF2 EV use EKF2_EV_CTRL instead of EKF2_AID_MASK\n"); @@ -787,15 +844,35 @@ void EKF2::PublishRngHgtBias(const hrt_abstime ×tamp) } } -void EKF2::PublishEvHgtBias(const hrt_abstime ×tamp) +void EKF2::PublishEvPosBias(const hrt_abstime ×tamp) { - if (_ekf.get_ev_sample_delayed().time_us != 0) { - const BiasEstimator::status &status = _ekf.getEvHgtBiasEstimatorStatus(); + if (_ekf.aid_src_ev_hgt().timestamp_sample) { - if (fabsf(status.bias - _last_ev_hgt_bias_published) > 0.001f) { - _estimator_ev_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_ev_sample_delayed().time_us, timestamp)); + estimator_bias3d_s bias{}; - _last_ev_hgt_bias_published = status.bias; + // height + BiasEstimator::status bias_est_status[3]; + bias_est_status[0] = _ekf.getEvPosBiasEstimatorStatus(0); + bias_est_status[1] = _ekf.getEvPosBiasEstimatorStatus(1); + bias_est_status[2] = _ekf.getEvHgtBiasEstimatorStatus(); + + for (int i = 0; i < 3; i++) { + bias.bias[i] = bias_est_status[i].bias; + bias.bias_var[i] = bias_est_status[i].bias_var; + + bias.innov[i] = bias_est_status[i].innov; + bias.innov_var[i] = bias_est_status[i].innov_var; + bias.innov_test_ratio[i] = bias_est_status[i].innov_test_ratio; + } + + const Vector3f bias_vec{bias.bias}; + + if ((bias_vec - _last_ev_bias_published).longerThan(0.01f)) { + bias.timestamp_sample = _ekf.aid_src_ev_hgt().timestamp_sample; + bias.timestamp = hrt_absolute_time(); + _estimator_ev_pos_bias_pub.publish(bias); + + _last_ev_bias_published = Vector3f(bias.bias); } } } @@ -1210,43 +1287,6 @@ void EKF2::PublishOdometry(const hrt_abstime ×tamp) _odometry_pub.publish(odom); } -void EKF2::PublishOdometryAligned(const hrt_abstime ×tamp, const vehicle_odometry_s &ev_odom) -{ - const Quatf quat_ev2ekf = _ekf.getVisionAlignmentQuaternion(); // rotates from EV to EKF navigation frame - const Dcmf ev_rot_mat(quat_ev2ekf); - - vehicle_odometry_s aligned_ev_odom{ev_odom}; - - // Rotate external position and velocity into EKF navigation frame - const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.position); - aligned_pos.copyTo(aligned_ev_odom.position); - - switch (ev_odom.velocity_frame) { - case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD: { - const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.velocity); - aligned_vel.copyTo(aligned_ev_odom.velocity); - break; - } - - case vehicle_odometry_s::VELOCITY_FRAME_FRD: { - const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.velocity); - aligned_vel.copyTo(aligned_ev_odom.velocity); - break; - } - } - - aligned_ev_odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED; - - // Compute orientation in EKF navigation frame - Quatf ev_quat_aligned = quat_ev2ekf * Quatf(ev_odom.q); - ev_quat_aligned.normalize(); - - ev_quat_aligned.copyTo(aligned_ev_odom.q); - - aligned_ev_odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); - _estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom); -} - void EKF2::PublishSensorBias(const hrt_abstime ×tamp) { // estimator_sensor_bias @@ -1700,12 +1740,14 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) } } -bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom) +bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF external vision sample bool new_ev_odom = false; const unsigned last_generation = _ev_odom_sub.get_last_generation(); + vehicle_odometry_s ev_odom; + if (_ev_odom_sub.update(&ev_odom)) { if (_msg_missed_odometry_perf == nullptr) { _msg_missed_odometry_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_visual_odometry messages missed"); @@ -1764,38 +1806,38 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo } // check for valid position data - if (Vector3f(ev_odom.position).isAllFinite()) { - bool position_valid = true; + const Vector3f ev_odom_pos(ev_odom.position); + const Vector3f ev_odom_pos_var(ev_odom.position_variance); - // switch (ev_odom.pose_frame) { - // case vehicle_odometry_s::POSE_FRAME_NED: - // ev_data.pos_frame = PositionFrame::LOCAL_FRAME_NED; - // break; + if (ev_odom_pos.isAllFinite()) { + bool position_frame_valid = false; - // case vehicle_odometry_s::POSE_FRAME_FRD: - // ev_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD; - // break; + switch (ev_odom.pose_frame) { + case vehicle_odometry_s::POSE_FRAME_NED: + ev_data.pos_frame = PositionFrame::LOCAL_FRAME_NED; + position_frame_valid = true; + break; - // default: - // position_valid = false; - // break; - // } + case vehicle_odometry_s::POSE_FRAME_FRD: + ev_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD; + position_frame_valid = true; + break; + } - if (position_valid) { - ev_data.pos(0) = ev_odom.position[0]; - ev_data.pos(1) = ev_odom.position[1]; - ev_data.pos(2) = ev_odom.position[2]; + if (position_frame_valid) { + ev_data.pos = ev_odom_pos; const float evp_noise_var = sq(_param_ekf2_evp_noise.get()); // position measurement error from ev_data or parameters - if (!_param_ekf2_ev_noise_md.get() && Vector3f(ev_odom.position_variance).isAllFinite()) { - ev_data.posVar(0) = fmaxf(evp_noise_var, ev_odom.position_variance[0]); - ev_data.posVar(1) = fmaxf(evp_noise_var, ev_odom.position_variance[1]); - ev_data.posVar(2) = fmaxf(evp_noise_var, ev_odom.position_variance[2]); + if ((_param_ekf2_ev_noise_md.get() == 0) && ev_odom_pos_var.isAllFinite()) { + + ev_data.position_var(0) = fmaxf(evp_noise_var, ev_odom_pos_var(0)); + ev_data.position_var(1) = fmaxf(evp_noise_var, ev_odom_pos_var(1)); + ev_data.position_var(2) = fmaxf(evp_noise_var, ev_odom_pos_var(2)); } else { - ev_data.posVar.setAll(evp_noise_var); + ev_data.position_var.setAll(evp_noise_var); } new_ev_odom = true; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index fdfe92f12e..b4db8e8f40 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -142,7 +143,7 @@ private: void PublishBaroBias(const hrt_abstime ×tamp); void PublishGnssHgtBias(const hrt_abstime ×tamp); void PublishRngHgtBias(const hrt_abstime ×tamp); - void PublishEvHgtBias(const hrt_abstime ×tamp); + void PublishEvPosBias(const hrt_abstime ×tamp); estimator_bias_s fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us, uint64_t timestamp, uint32_t device_id = 0); void PublishEventFlags(const hrt_abstime ×tamp); @@ -165,7 +166,7 @@ private: void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps); - bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom); + bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps); bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps); @@ -291,7 +292,7 @@ private: float _last_baro_bias_published{}; float _last_gnss_hgt_bias_published{}; float _last_rng_hgt_bias_published{}; - float _last_ev_hgt_bias_published{}; + matrix::Vector3f _last_ev_bias_published{}; float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements hrt_abstime _airspeed_validated_timestamp_last{0}; @@ -340,7 +341,7 @@ private: uORB::PublicationMulti _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)}; uORB::PublicationMulti _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)}; uORB::PublicationMulti _estimator_rng_hgt_bias_pub{ORB_ID(estimator_rng_hgt_bias)}; - uORB::PublicationMulti _estimator_ev_hgt_bias_pub{ORB_ID(estimator_ev_hgt_bias)}; + uORB::PublicationMulti _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_bias)}; uORB::PublicationMultiData _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; uORB::PublicationMulti _estimator_gps_status_pub{ORB_ID(estimator_gps_status)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; @@ -350,7 +351,6 @@ private: uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; uORB::PublicationMulti _estimator_status_flags_pub{ORB_ID(estimator_status_flags)}; uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; - uORB::PublicationMulti _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)}; uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index e5d29b7361..571b7d1f26 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -603,10 +603,10 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f); * 0 : Deprecated, use EKF2_GPS_CTRL instead * 1 : Set to true to use optical flow data if available * 2 : Set to true to inhibit IMU delta velocity bias estimation - * 3 : Set to true to enable vision position fusion - * 4 : Set to true to enable vision yaw fusion. Cannot be used if bit position 7 is true. + * 3 : Deprecated, use EKF2_EV_CTRL instead + * 4 : Deprecated, use EKF2_EV_CTRL instead * 5 : Set to true to enable multi-rotor drag specific force fusion - * 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used + * 6 : Deprecated, use EKF2_EV_CTRL instead * 7 : Deprecated, use EKF2_GPS_CTRL instead * 3 : Deprecated, use EKF2_EV_CTRL instead * @@ -616,10 +616,10 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f); * @bit 0 unused * @bit 1 use optical flow * @bit 2 inhibit IMU bias estimation - * @bit 3 vision position fusion - * @bit 4 vision yaw fusion + * @bit 3 unused + * @bit 4 unused * @bit 5 multi-rotor drag fusion - * @bit 6 rotate external vision + * @bit 6 unused * @bit 7 unused * @bit 8 unused * @reboot_required true @@ -841,7 +841,7 @@ PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f); * @unit rad * @decimal 2 */ -PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.05f); +PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.1f); /** * Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 8f4ff8ae25..7f5a46fe80 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -137,12 +137,12 @@ void EkfWrapper::setFlowOffset(const Vector3f &offset) void EkfWrapper::enableExternalVisionPositionFusion() { - _ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_POS; + _ekf_params->ev_ctrl |= static_cast(EvCtrl::HPOS); } void EkfWrapper::disableExternalVisionPositionFusion() { - _ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_POS; + _ekf_params->ev_ctrl &= ~static_cast(EvCtrl::HPOS); } bool EkfWrapper::isIntendingExternalVisionPositionFusion() const @@ -167,12 +167,12 @@ bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const void EkfWrapper::enableExternalVisionHeadingFusion() { - _ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_YAW; + _ekf_params->ev_ctrl |= static_cast(EvCtrl::YAW); } void EkfWrapper::disableExternalVisionHeadingFusion() { - _ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_YAW; + _ekf_params->ev_ctrl &= ~static_cast(EvCtrl::YAW); } bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const @@ -180,16 +180,6 @@ bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const return _ekf->control_status_flags().ev_yaw; } -void EkfWrapper::enableExternalVisionAlignment() -{ - _ekf_params->fusion_mode |= SensorFusionMask::ROTATE_EXT_VIS; -} - -void EkfWrapper::disableExternalVisionAlignment() -{ - _ekf_params->fusion_mode &= ~SensorFusionMask::ROTATE_EXT_VIS; -} - bool EkfWrapper::isIntendingMagHeadingFusion() const { return _ekf->control_status_flags().mag_hdg; diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index 4072c4fd54..f13a772188 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -98,9 +98,6 @@ public: bool isIntendingMag3DFusion() const; void setMagFuseTypeNone(); - void enableExternalVisionAlignment(); - void disableExternalVisionAlignment(); - bool isWindVelocityEstimated() const; void enableTerrainRngFusion(); diff --git a/src/modules/ekf2/test/sensor_simulator/vio.cpp b/src/modules/ekf2/test/sensor_simulator/vio.cpp index ab26808ece..e6bc6203fa 100644 --- a/src/modules/ekf2/test/sensor_simulator/vio.cpp +++ b/src/modules/ekf2/test/sensor_simulator/vio.cpp @@ -8,6 +8,7 @@ namespace sensor Vio::Vio(std::shared_ptr ekf): Sensor(ekf) { _vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD; + _vio_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD; } Vio::~Vio() @@ -32,7 +33,7 @@ void Vio::setVelocityVariance(const Vector3f &velVar) void Vio::setPositionVariance(const Vector3f &posVar) { - _vio_data.posVar = posVar; + _vio_data.position_var = posVar; } void Vio::setAngularVariance(float angVar) @@ -70,16 +71,27 @@ void Vio::setVelocityFrameToLocalNED() _vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED; } +void Vio::setPositionFrameToLocalNED() +{ + _vio_data.pos_frame = PositionFrame::LOCAL_FRAME_NED; +} + +void Vio::setPositionFrameToLocalFRD() +{ + _vio_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD; +} + extVisionSample Vio::dataAtRest() { extVisionSample vio_data; - vio_data.pos = Vector3f{0.0f, 0.0f, 0.0f};; - vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};; + vio_data.pos = Vector3f{0.0f, 0.0f, 0.0f}; + vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f}; vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f}; - vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f}; + vio_data.position_var = Vector3f{0.1f, 0.1f, 0.1f}; vio_data.velocity_var = Vector3f{0.1f, 0.1f, 0.1f}; vio_data.orientation_var(2) = 0.05f; vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD; + vio_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD; return vio_data; } diff --git a/src/modules/ekf2/test/sensor_simulator/vio.h b/src/modules/ekf2/test/sensor_simulator/vio.h index 50b81e60c0..683341024b 100644 --- a/src/modules/ekf2/test/sensor_simulator/vio.h +++ b/src/modules/ekf2/test/sensor_simulator/vio.h @@ -63,6 +63,9 @@ public: void setVelocityFrameToLocalFRD(); void setVelocityFrameToBody(); + void setPositionFrameToLocalNED(); + void setPositionFrameToLocalFRD(); + extVisionSample dataAtRest(); private: diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index 50a52a9828..3be67e2da7 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/src/modules/ekf2/test/test_EKF_externalVision.cpp @@ -78,6 +78,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic) { _sensor_simulator.runSeconds(_tilt_align_time); // Let the tilt align _ekf_wrapper.enableExternalVisionPositionFusion(); + _sensor_simulator._vio.setPositionFrameToLocalNED(); _sensor_simulator.startExternalVision(); _sensor_simulator.runSeconds(2); @@ -147,7 +148,6 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment) // WHEN: Vision frame is rotate +90°. The reported heading is -90° Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, math::radians(-90.0f))); _sensor_simulator._vio.setOrientation(vision_to_ekf.inversed()); - _ekf_wrapper.enableExternalVisionAlignment(); const Vector3f simulated_velocity_in_vision_frame(0.3f, -1.0f, 0.4f); const Vector3f simulated_velocity_in_ekf_frame = @@ -179,6 +179,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset) const Vector3f simulated_position(8.3f, -1.0f, 0.0f); _sensor_simulator._vio.setPosition(simulated_position); + _sensor_simulator._vio.setPositionFrameToLocalNED(); _ekf_wrapper.enableExternalVisionPositionFusion(); _sensor_simulator.startExternalVision(); _sensor_simulator.runMicroseconds(2e5); @@ -197,7 +198,6 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment) // WHEN: Vision frame is rotate +90°. The reported heading is -90° Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, math::radians(-90.0f))); _sensor_simulator._vio.setOrientation(vision_to_ekf.inversed()); - _ekf_wrapper.enableExternalVisionAlignment(); const Vector3f simulated_position_in_vision_frame(8.3f, -1.0f, 0.0f); const Vector3f simulated_position_in_ekf_frame = @@ -236,7 +236,6 @@ TEST_F(EkfExternalVisionTest, visionAlignment) // WHEN: Vision frame is rotate +90°. The reported heading is -90° Quatf externalVisionFrameOffset(Eulerf(0.0f, 0.0f, math::radians(90.0f))); _sensor_simulator._vio.setOrientation(externalVisionFrameOffset.inversed()); - _ekf_wrapper.enableExternalVisionAlignment(); // Simulate high uncertainty on vision x axis which is in this case // the y EKF frame axis @@ -254,9 +253,9 @@ TEST_F(EkfExternalVisionTest, visionAlignment) EXPECT_TRUE(velVar_new(1) > velVar_new(0)); // THEN: the frame offset should be estimated correctly - Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion(); - EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(), - estimatedExternalVisionFrameOffset.canonical())); + // Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion(); + // EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(), + // estimatedExternalVisionFrameOffset.canonical())); } TEST_F(EkfExternalVisionTest, velocityFrameBody) @@ -340,7 +339,6 @@ TEST_F(EkfExternalVisionTest, positionFrameLocal) // WHEN: using EV yaw fusion and rotate EV is set Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, 0.f)); _sensor_simulator._vio.setOrientation(vision_to_ekf.inversed()); - _ekf_wrapper.enableExternalVisionAlignment(); // ROTATE_EV _ekf_wrapper.enableExternalVisionHeadingFusion(); // EV_YAW // AND WHEN: using EV position aiding diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 8b00a9ac34..f727cc8d43 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -149,7 +149,7 @@ void LoggedTopics::add_default_topics() add_topic("estimator_baro_bias", 500); add_topic("estimator_gnss_hgt_bias", 500); add_topic("estimator_rng_hgt_bias", 500); - add_topic("estimator_ev_hgt_bias", 500); + add_topic("estimator_ev_pos_bias", 500); add_topic("estimator_event_flags", 0); add_topic("estimator_gps_status", 1000); add_topic("estimator_innovation_test_ratios", 500); @@ -160,13 +160,12 @@ void LoggedTopics::add_default_topics() add_topic("estimator_states", 1000); add_topic("estimator_status", 200); add_topic("estimator_status_flags", 0); - add_topic("estimator_visual_odometry_aligned", 200); add_topic("yaw_estimator_status", 1000); add_optional_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_rng_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_ev_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_ev_pos_bias", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); @@ -177,7 +176,6 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_airspeed", 100, MAX_ESTIMATOR_INSTANCES); @@ -248,7 +246,7 @@ void LoggedTopics::add_default_topics() add_topic("estimator_baro_bias"); add_topic("estimator_gnss_hgt_bias"); add_topic("estimator_rng_hgt_bias"); - add_topic("estimator_ev_hgt_bias"); + add_topic("estimator_ev_pos_bias"); add_topic("estimator_event_flags"); add_topic("estimator_gps_status"); add_topic("estimator_innovation_test_ratios"); @@ -259,7 +257,6 @@ void LoggedTopics::add_default_topics() add_topic("estimator_states"); add_topic("estimator_status"); add_topic("estimator_status_flags"); - add_topic("estimator_visual_odometry_aligned"); add_topic("vehicle_attitude"); add_topic("vehicle_global_position"); add_topic("vehicle_local_position"); @@ -281,7 +278,6 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES); 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_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_sideslip", 0, MAX_ESTIMATOR_INSTANCES); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index a3938f3408..85c0361de3 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -502,7 +502,6 @@ public: bool hash_check_enabled() const { return _param_mav_hash_chk_en.get(); } bool forward_heartbeats_enabled() const { return _param_mav_hb_forw_en.get(); } - bool odometry_loopback_enabled() const { return _param_mav_odom_lp.get(); } bool failure_injection_enabled() const { return _param_sys_failure_injection_enabled.get(); } @@ -675,7 +674,6 @@ private: (ParamBool) _param_mav_fwdextsp, (ParamBool) _param_mav_hash_chk_en, (ParamBool) _param_mav_hb_forw_en, - (ParamBool) _param_mav_odom_lp, (ParamInt) _param_mav_radio_timeout, (ParamInt) _param_sys_hitl, (ParamBool) _param_sys_failure_injection_enabled diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index 59078c9525..8a793f5e5f 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/src/modules/mavlink/mavlink_params.c @@ -142,17 +142,6 @@ PARAM_DEFINE_INT32(MAV_HASH_CHK_EN, 1); */ PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1); -/** - * Activate ODOMETRY loopback. - * - * If set, it gets the data from 'vehicle_visual_odometry' instead of 'vehicle_odometry' - * serving as a loopback of the received ODOMETRY messages on the Mavlink receiver. - * - * @boolean - * @group MAVLink - */ -PARAM_DEFINE_INT32(MAV_ODOM_LP, 0); - /** * Timeout in seconds for the RADIO_STATUS reports coming in * diff --git a/src/modules/mavlink/streams/ODOMETRY.hpp b/src/modules/mavlink/streams/ODOMETRY.hpp index f423b16bfb..60b8564bc2 100644 --- a/src/modules/mavlink/streams/ODOMETRY.hpp +++ b/src/modules/mavlink/streams/ODOMETRY.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -49,42 +49,20 @@ public: unsigned get_size() override { - if (_mavlink->odometry_loopback_enabled()) { - return _vodom_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; - - } else { - return _odom_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; - } + return _vehicle_odometry_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: explicit MavlinkStreamOdometry(Mavlink *mavlink) : MavlinkStream(mavlink) {} - uORB::Subscription _odom_sub{ORB_ID(vehicle_odometry)}; - uORB::Subscription _vodom_sub{ORB_ID(vehicle_visual_odometry)}; + uORB::Subscription _vehicle_odometry_sub{ORB_ID(vehicle_odometry)}; bool send() override { vehicle_odometry_s odom; - // check if it is to send visual odometry loopback or not - bool odom_updated = false; - mavlink_odometry_t msg{}; - - if (_mavlink->odometry_loopback_enabled()) { - odom_updated = _vodom_sub.update(&odom); - - // source: external vision system - msg.estimator_type = MAV_ESTIMATOR_TYPE_VISION; - - } else { - odom_updated = _odom_sub.update(&odom); - - // source: PX4 estimator - msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT; - } - - if (odom_updated) { + if (_vehicle_odometry_sub.update(&odom)) { + mavlink_odometry_t msg{}; msg.time_usec = odom.timestamp_sample; // set the frame_id according to the local frame of the data