From a397c09e59155ce05e55000a9d8a54d1759c9cac Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 20 Jul 2022 21:37:00 -0400 Subject: [PATCH] ekf2: use estimator_aid_src for all yaw sources (mag, gnss, ev) --- msg/estimator_aid_source_1d.msg | 1 + msg/estimator_aid_source_3d.msg | 2 +- msg/estimator_status_flags.msg | 3 - src/modules/ekf2/EKF/control.cpp | 40 ++++-- src/modules/ekf2/EKF/ekf.h | 84 ++++++++--- src/modules/ekf2/EKF/ekf_helper.cpp | 37 +++-- src/modules/ekf2/EKF/estimator_interface.h | 4 +- src/modules/ekf2/EKF/gps_control.cpp | 5 +- src/modules/ekf2/EKF/gps_fusion.cpp | 38 ++++- src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 52 ++++--- src/modules/ekf2/EKF/mag_control.cpp | 32 ++++- src/modules/ekf2/EKF/mag_fusion.cpp | 132 +++++++++--------- .../EKF/zero_innovation_heading_update.cpp | 9 +- src/modules/ekf2/EKF2.cpp | 14 +- src/modules/ekf2/EKF2.hpp | 13 ++ src/modules/logger/logged_topics.cpp | 23 +++ 16 files changed, 334 insertions(+), 155 deletions(-) diff --git a/msg/estimator_aid_source_1d.msg b/msg/estimator_aid_source_1d.msg index d08c2bb2f9..2a4f8a07a1 100644 --- a/msg/estimator_aid_source_1d.msg +++ b/msg/estimator_aid_source_1d.msg @@ -20,3 +20,4 @@ bool fused # true if the sample was successfully fused # TOPICS estimator_aid_source_1d # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt estimator_aid_src_airspeed +# TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw diff --git a/msg/estimator_aid_source_3d.msg b/msg/estimator_aid_source_3d.msg index b2d158487d..36d7485e4f 100644 --- a/msg/estimator_aid_source_3d.msg +++ b/msg/estimator_aid_source_3d.msg @@ -19,4 +19,4 @@ bool[3] innovation_rejected # true if the observation has been rejected bool[3] fused # true if the sample was successfully fused # TOPICS estimator_aid_source_3d -# TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel +# TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel estimator_aid_src_mag diff --git a/msg/estimator_status_flags.msg b/msg/estimator_status_flags.msg index f86f85c1a7..0e59adaa75 100644 --- a/msg/estimator_status_flags.msg +++ b/msg/estimator_status_flags.msg @@ -65,9 +65,6 @@ bool reject_hor_vel # 0 - true if horizontal velocity obs bool reject_ver_vel # 1 - true if vertical velocity observations have been rejected bool reject_hor_pos # 2 - true if horizontal position observations have been rejected bool reject_ver_pos # 3 - true if vertical position observations have been rejected -bool reject_mag_x # 4 - true if the X magnetometer observation has been rejected -bool reject_mag_y # 5 - true if the Y magnetometer observation has been rejected -bool reject_mag_z # 6 - true if the Z magnetometer observation has been rejected bool reject_yaw # 7 - true if the yaw observation has been rejected bool reject_airspeed # 8 - true if the airspeed observation has been rejected bool reject_sideslip # 9 - true if the synthetic sideslip observation has been rejected diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 80c354de9b..47697a79bf 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -361,21 +361,41 @@ void Ekf::controlExternalVisionFusion() } // determine if we should use the yaw observation - if (_control_status.flags.ev_yaw) { - if (reset && _control_status_prev.flags.ev_yaw) { - resetYawToEv(); + resetEstimatorAidStatus(_aid_src_ev_yaw); + const float measured_hdg = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed.quat) : getEuler312Yaw(_ev_sample_delayed.quat); + const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.angVar, 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 = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed_prev.quat) : getEuler312Yaw(_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); } - - float measured_hdg = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed.quat) : getEuler312Yaw(_ev_sample_delayed.quat); - - float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg); - float obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f); - fuseYaw(innovation, obs_var); } // 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) && isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) { @@ -584,7 +604,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) fuseGpsYaw(); - const bool is_fusion_failing = isTimedOut(_time_last_gps_yaw_fuse, _params.reset_timeout_max); + const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max); if (is_fusion_failing) { if (_nb_gps_yaw_reset_available > 0) { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 3264e85631..46f891f3fa 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -111,13 +111,54 @@ public: const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); } const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; } - void getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; } - void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; } - void getHeadingInnovRatio(float &heading_innov_ratio) const { heading_innov_ratio = _yaw_test_ratio; } + void getHeadingInnov(float &heading_innov) const { + if (_control_status.flags.mag_hdg) { + heading_innov = _aid_src_mag_heading.innovation; - void getMagInnov(float mag_innov[3]) const { _mag_innov.copyTo(mag_innov); } - void getMagInnovVar(float mag_innov_var[3]) const { _mag_innov_var.copyTo(mag_innov_var); } - void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = _mag_test_ratio.max(); } + } else if (_control_status.flags.mag_3D) { + heading_innov = Vector3f(_aid_src_mag.innovation).max(); + + } else if (_control_status.flags.gps_yaw) { + heading_innov = _aid_src_gnss_yaw.innovation; + + } else if (_control_status.flags.ev_yaw) { + heading_innov = _aid_src_ev_yaw.innovation; + } + } + + void getHeadingInnovVar(float &heading_innov_var) const { + if (_control_status.flags.mag_hdg) { + heading_innov_var = _aid_src_mag_heading.innovation_variance; + + } else if (_control_status.flags.mag_3D) { + heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max(); + + } else if (_control_status.flags.gps_yaw) { + heading_innov_var = _aid_src_gnss_yaw.innovation_variance; + + } else if (_control_status.flags.ev_yaw) { + heading_innov_var = _aid_src_ev_yaw.innovation_variance; + } + } + + void getHeadingInnovRatio(float &heading_innov_ratio) const { + if (_control_status.flags.mag_hdg) { + heading_innov_ratio = _aid_src_mag_heading.test_ratio; + + } else if (_control_status.flags.mag_3D) { + heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); + + } else if (_control_status.flags.gps_yaw) { + heading_innov_ratio = _aid_src_gnss_yaw.test_ratio; + + } else if (_control_status.flags.ev_yaw) { + heading_innov_ratio = _aid_src_ev_yaw.test_ratio; + } + } + + void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); } + void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); } + void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); } void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); } void getDragInnovVar(float drag_innov_var[2]) const { _drag_innov_var.copyTo(drag_innov_var); } @@ -354,9 +395,15 @@ public: const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; } + const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; } + + const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; } const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; } const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; } + const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; } + const auto &aid_src_mag() const { return _aid_src_mag; } + private: // set the internal states and status to their default value @@ -391,6 +438,7 @@ private: // 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 bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North) @@ -418,10 +466,7 @@ private: uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec) uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec) uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) - uint64_t _time_last_gps_yaw_fuse{0}; ///< time the last fusion of GPS yaw measurements were performed (uSec) uint64_t _time_last_gps_yaw_data{0}; ///< time the last GPS yaw measurement was available (uSec) - uint64_t _time_last_mag_heading_fuse{0}; - uint64_t _time_last_mag_3d_fuse{0}; 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 @@ -470,12 +515,6 @@ private: Vector3f _aux_vel_innov{}; ///< horizontal auxiliary velocity innovations: (m/sec) Vector3f _aux_vel_innov_var{}; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2) - float _heading_innov{0.0f}; ///< heading measurement innovation (rad) - float _heading_innov_var{0.0f}; ///< heading measurement innovation variance (rad**2) - - Vector3f _mag_innov{}; ///< earth magnetic field innovations (Gauss) - Vector3f _mag_innov_var{}; ///< earth magnetic field innovation variance (Gauss**2) - Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2) Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2) @@ -505,9 +544,15 @@ private: estimator_aid_source_2d_s _aid_src_fake_pos{}; + estimator_aid_source_1d_s _aid_src_ev_yaw{}; + + estimator_aid_source_1d_s _aid_src_gnss_yaw{}; estimator_aid_source_3d_s _aid_src_gnss_vel{}; estimator_aid_source_3d_s _aid_src_gnss_pos{}; + estimator_aid_source_1d_s _aid_src_mag_heading{}; + estimator_aid_source_3d_s _aid_src_mag{}; + // output predictor states Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad) Vector3f _vel_err_integ{}; ///< integral of velocity tracking error (m) @@ -606,12 +651,12 @@ private: void predictCovariance(); // ekf sequential fusion of magnetometer measurements - bool fuseMag(const Vector3f &mag, bool update_all_states = true); + bool fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states = true); // update quaternion states and covariances using an innovation, observation variance and Jacobian vector // innovation : prediction - measurement // variance : observaton variance - bool fuseYaw(const float innovation, const float variance); + bool fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s& aid_src_status); // fuse the yaw angle obtained from a dual antenna GPS unit void fuseGpsYaw(); @@ -686,10 +731,11 @@ private: bool fuseVerticalPosition(float innov, float innov_gate, float obs_var, float &innov_var, float &test_ratio); + void updateGpsYaw(const gpsSample &gps_sample); void updateGpsVel(const gpsSample &gps_sample); - void fuseGpsVel(); - void updateGpsPos(const gpsSample &gps_sample); + + void fuseGpsVel(); void fuseGpsPos(); // calculate optical flow body angular rate compensation diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index b6eb79aa9a..6669937034 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -931,7 +931,17 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f status = _innov_check_fail_status.value; // return the largest magnetometer innovation test ratio - mag = sqrtf(math::max(_yaw_test_ratio, _mag_test_ratio.max())); + if (_control_status.flags.mag_hdg) { + mag = sqrtf(_aid_src_mag_heading.test_ratio); + + } else if (_control_status.flags.mag_3D) { + mag = sqrtf(Vector3f(_aid_src_mag.test_ratio).max()); + + } else if (_control_status.flags.gps_yaw) { + mag = sqrtf(_aid_src_gnss_yaw.test_ratio); + } else { + mag = NAN; + } // return the largest velocity and position innovation test ratio vel = NAN; @@ -1002,9 +1012,23 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz; soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel; soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs; + + bool mag_innov_good = true; + + if (_control_status.flags.mag_hdg) { + if (_aid_src_mag_heading.test_ratio < 1.f) { + mag_innov_good = false; + } + + } else if (_control_status.flags.mag_3D) { + if (Vector3f(_aid_src_mag.test_ratio).max() < 1.f) { + mag_innov_good = false; + } + } + const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f; const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f; - const bool mag_innov_good = (_mag_test_ratio.max() < 1.0f) && (_yaw_test_ratio < 1.0f); + soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good; soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical; *status = soln_status.value; @@ -1260,10 +1284,6 @@ void Ekf::stopMag3DFusion() _control_status.flags.mag_3D = false; _control_status.flags.mag_dec = false; - _mag_innov.zero(); - _mag_innov_var.zero(); - _mag_test_ratio.zero(); - _fault_status.flags.bad_mag_x = false; _fault_status.flags.bad_mag_y = false; _fault_status.flags.bad_mag_z = false; @@ -1278,8 +1298,6 @@ void Ekf::stopMagHdgFusion() _control_status.flags.mag_hdg = false; _fault_status.flags.bad_hdg = false; - - _yaw_test_ratio = 0.f; } } @@ -1298,8 +1316,6 @@ void Ekf::startMag3DFusion() stopMagHdgFusion(); - _yaw_test_ratio = 0.0f; - zeroMagCov(); loadMagCovData(); _control_status.flags.mag_3D = true; @@ -1679,6 +1695,7 @@ void Ekf::stopGpsYawFusion() if (_control_status.flags.gps_yaw) { ECL_INFO("stopping GPS yaw fusion"); _control_status.flags.gps_yaw = false; + resetEstimatorAidStatus(_aid_src_gnss_yaw); } } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 15e46cbbbb..02e61999c3 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -328,9 +328,7 @@ protected: float _gps_yaw_offset{0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians). // innovation consistency check monitoring ratios - float _yaw_test_ratio{}; // yaw innovation consistency check ratio - AlphaFilter _yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state - Vector3f _mag_test_ratio{}; // magnetometer XYZ innovation consistency check ratios + AlphaFilter _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state Vector2f _ev_vel_test_ratio{}; // EV velocity innovation consistency check ratios Vector2f _ev_pos_test_ratio{}; // EV position innovation consistency check ratios Vector2f _aux_vel_test_ratio{}; // Auxiliary horizontal velocity innovation consistency check ratio diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 61a8bd7b6f..b9e278c203 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -49,10 +49,7 @@ void Ekf::controlGpsFusion() // Check for new GPS data that has fallen behind the fusion time horizon if (_gps_data_ready) { - // reset flags - resetEstimatorAidStatusFlags(_aid_src_gnss_vel); - resetEstimatorAidStatusFlags(_aid_src_gnss_pos); - + updateGpsYaw(_gps_sample_delayed); updateGpsVel(_gps_sample_delayed); updateGpsPos(_gps_sample_delayed); diff --git a/src/modules/ekf2/EKF/gps_fusion.cpp b/src/modules/ekf2/EKF/gps_fusion.cpp index 940ce3f151..19828df5dc 100644 --- a/src/modules/ekf2/EKF/gps_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4. All rights reserved. + * Copyright (c) 2021-2022 PX4. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,16 +39,43 @@ /* #include */ #include "ekf.h" +void Ekf::updateGpsYaw(const gpsSample &gps_sample) +{ + auto &gps_yaw = _aid_src_gnss_yaw; + resetEstimatorAidStatusFlags(gps_yaw); + + if (PX4_ISFINITE(gps_sample.yaw)) { + // initially populate for estimator_aid_src_gnss_yaw logging + + // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement + const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset); + + gps_yaw.observation = measured_hdg; + gps_yaw.observation_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f)); + + // define the predicted antenna array vector and rotate into earth frame + const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; + const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf; + const float predicted_hdg = atan2f(ant_vec_ef(1), ant_vec_ef(0)); + gps_yaw.innovation = wrap_pi(predicted_hdg - gps_yaw.observation); + + gps_yaw.fusion_enabled = _control_status.flags.gps_yaw; + + gps_yaw.timestamp_sample = gps_sample.time_us; + } +} + void Ekf::updateGpsVel(const gpsSample &gps_sample) { + auto &gps_vel = _aid_src_gnss_vel; + resetEstimatorAidStatus(gps_vel); + const float vel_var = sq(gps_sample.sacc); const Vector3f obs_var{vel_var, vel_var, vel_var * sq(1.5f)}; // innovation gate size const float innov_gate = fmaxf(_params.gps_vel_innov_gate, 1.f); - auto &gps_vel = _aid_src_gnss_vel; - for (int i = 0; i < 3; i++) { gps_vel.observation[i] = gps_sample.vel(i); gps_vel.observation_variance[i] = obs_var(i); @@ -72,6 +99,9 @@ void Ekf::updateGpsVel(const gpsSample &gps_sample) void Ekf::updateGpsPos(const gpsSample &gps_sample) { + auto &gps_pos = _aid_src_gnss_pos; + resetEstimatorAidStatus(gps_pos); + Vector3f position; position(0) = gps_sample.pos(0); position(1) = gps_sample.pos(1); @@ -100,8 +130,6 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample) // innovation gate size float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f); - auto &gps_pos = _aid_src_gnss_pos; - for (int i = 0; i < 3; i++) { gps_pos.observation[i] = position(i); gps_pos.observation_variance[i] = obs_var(i); diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 750a22094a..f363bca17a 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -60,18 +60,27 @@ void Ekf::fuseGpsYaw() const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf; - // check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading - if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) { - return; - } - // calculate predicted antenna yaw angle const float predicted_hdg = atan2f(ant_vec_ef(1), ant_vec_ef(0)); + // wrap the innovation to the interval between +-pi + const float heading_innov = wrap_pi(predicted_hdg - measured_hdg); + // using magnetic heading process noise // TODO extend interface to use yaw uncertainty provided by GPS if available const float R_YAW = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f)); + _aid_src_gnss_yaw.timestamp_sample = _gps_sample_delayed.time_us; + _aid_src_gnss_yaw.observation = measured_hdg; + _aid_src_gnss_yaw.observation_variance = R_YAW; + _aid_src_gnss_yaw.innovation = heading_innov; + _aid_src_gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw; + + // check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading + if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) { + return; + } + // calculate intermediate variables const float HK0 = sinf(_gps_yaw_offset); const float HK1 = q0*q3; @@ -121,9 +130,11 @@ void Ekf::fuseGpsYaw() // const float HK32 = HK18/(-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW); // check if the innovation variance calculation is badly conditioned - _heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW); + const float heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW); - if (_heading_innov_var < R_YAW) { + _aid_src_gnss_yaw.innovation_variance = heading_innov_var; + + if (heading_innov_var < R_YAW) { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned _fault_status.flags.bad_hdg = true; @@ -134,19 +145,15 @@ void Ekf::fuseGpsYaw() } _fault_status.flags.bad_hdg = false; - const float HK32 = HK18 / _heading_innov_var; + const float HK32 = HK18 / heading_innov_var; // calculate the innovation and define the innovation gate const float innov_gate = math::max(_params.heading_innov_gate, 1.0f); - _heading_innov = predicted_hdg - measured_hdg; - - // wrap the innovation to the interval between +-pi - _heading_innov = wrap_pi(_heading_innov); // innovation test ratio - _yaw_test_ratio = sq(_heading_innov) / (sq(innov_gate) * _heading_innov_var); + setEstimatorAidStatusTestRatio(_aid_src_gnss_yaw, innov_gate); - if (_yaw_test_ratio > 1.0f) { + if (_aid_src_gnss_yaw.innovation_rejected) { _innov_check_fail_status.flags.reject_yaw = true; return; @@ -154,10 +161,10 @@ void Ekf::fuseGpsYaw() _innov_check_fail_status.flags.reject_yaw = false; } - _yaw_signed_test_ratio_lpf.update(matrix::sign(_heading_innov) * _yaw_test_ratio); + _gnss_yaw_signed_test_ratio_lpf.update(matrix::sign(heading_innov) * _aid_src_gnss_yaw.test_ratio); - if ((fabsf(_yaw_signed_test_ratio_lpf.getState()) > 0.2f) - && !_control_status.flags.in_air && isTimedOut(_time_last_heading_fuse, (uint64_t)1e6)) { + if ((fabsf(_gnss_yaw_signed_test_ratio_lpf.getState()) > 0.2f) + && !_control_status.flags.in_air && isTimedOut(_aid_src_gnss_yaw.time_last_fuse, (uint64_t)1e6)) { // A constant large signed test ratio is a sign of wrong gyro bias // Reset the yaw gyro variance to converge faster and avoid @@ -184,12 +191,13 @@ void Ekf::fuseGpsYaw() Kfusion(row) = HK32*(-HK16*P(0,row) - HK24*P(1,row) - HK25*P(2,row) + HK26*P(3,row)); } - const bool is_fused = measurementUpdate(Kfusion, Hfusion, _heading_innov); + const bool is_fused = measurementUpdate(Kfusion, Hfusion, heading_innov); _fault_status.flags.bad_hdg = !is_fused; + _aid_src_gnss_yaw.fused = is_fused; if (is_fused) { - _time_last_gps_yaw_fuse = _time_last_imu; _time_last_heading_fuse = _time_last_imu; + _aid_src_gnss_yaw.time_last_fuse = _time_last_imu; } } @@ -207,11 +215,11 @@ bool Ekf::resetYawToGps() // GPS yaw measurement is alreday compensated for antenna offset in the driver const float measured_yaw = _gps_sample_delayed.yaw; - const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f)); + const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f)); resetQuatStateYaw(measured_yaw, yaw_variance, true); - _time_last_gps_yaw_fuse = _time_last_imu; - _yaw_signed_test_ratio_lpf.reset(0.f); + _aid_src_gnss_yaw.time_last_fuse = _time_last_imu; + _gnss_yaw_signed_test_ratio_lpf.reset(0.f); return true; } diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index c871801917..1119f12c03 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -66,6 +66,28 @@ void Ekf::controlMagFusion() } _control_status.flags.mag_field_disturbed = magFieldStrengthDisturbed(mag_sample.mag); + + + // compute mag heading innovation (for estimator_aid_src_mag_heading logging) + const Vector3f mag_observation = mag_sample.mag - _state.mag_B; + const Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? updateEuler321YawInRotMat(0.f, _R_to_earth) : updateEuler312YawInRotMat(0.f, _R_to_earth); + const Vector3f mag_earth_pred = R_to_earth * mag_observation; + + resetEstimatorAidStatus(_aid_src_mag_heading); + _aid_src_mag_heading.timestamp_sample = mag_sample.time_us; + _aid_src_mag_heading.observation = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();; + _aid_src_mag_heading.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _aid_src_mag_heading.observation); + + // compute magnetometer innovations (for estimator_aid_src_mag logging) + // rotate magnetometer earth field state into body frame + const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal); + const Vector3f mag_I_rot = R_to_body * _state.mag_I; + const Vector3f mag_innov = mag_I_rot - mag_observation; + + resetEstimatorAidStatus(_aid_src_mag); + _aid_src_mag.timestamp_sample = mag_sample.time_us; + mag_observation.copyTo(_aid_src_mag.observation); + mag_innov.copyTo(_aid_src_mag.innovation); } } @@ -344,9 +366,9 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag) float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg); float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f); - if (fuseYaw(innovation, obs_var)) { - _time_last_mag_heading_fuse = _time_last_imu; - } + _aid_src_mag_heading.fusion_enabled = _control_status.flags.mag_hdg; + + fuseYaw(innovation, obs_var, _aid_src_mag_heading); } } @@ -364,13 +386,13 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag) // states for the first few observations. fuseDeclination(0.02f); _mag_decl_cov_reset = true; - fuseMag(mag, update_all_states); + fuseMag(mag, _aid_src_mag, update_all_states); } else { // The normal sequence is to fuse the magnetometer data first before fusing // declination angle at a higher uncertainty to allow some learning of // declination angle over time. - fuseMag(mag, update_all_states); + fuseMag(mag, _aid_src_mag, update_all_states); if (_control_status.flags.mag_dec) { fuseDeclination(0.5f); diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 286fdf2f19..1b1ffe23e2 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -45,7 +45,7 @@ #include -bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) +bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states) { // assign intermediate variables const float q0 = _state.quat_nominal(0); @@ -87,9 +87,9 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) const float HKX22 = HKX10*P(1,17) - HKX11*P(1,18) + HKX12*P(1,1) + HKX13*P(0,1) - HKX14*P(1,2) + HKX15*P(1,3) + HKX6*P(1,16) + P(1,19); const float HKX23 = HKX10*P(17,19) - HKX11*P(18,19) + HKX12*P(1,19) + HKX13*P(0,19) - HKX14*P(2,19) + HKX15*P(3,19) + HKX6*P(16,19) + P(19,19); - _mag_innov_var(0) = HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG; + aid_src_mag.innovation_variance[0] = HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG; - if (_mag_innov_var(0) < R_MAG) { + if (aid_src_mag.innovation_variance[0] < R_MAG) { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned _fault_status.flags.bad_mag_x = true; @@ -101,7 +101,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) _fault_status.flags.bad_mag_x = false; - const float HKX24 = 1.0F/_mag_innov_var(0); + const float HKX24 = 1.0F/aid_src_mag.innovation_variance[0]; // intermediate variables for calculation of innovations variances for Y and Z axes // don't calculate all terms needed for observation jacobians and Kalman gains because @@ -126,12 +126,11 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) const float IV17 = 2*IV0 - 2*IV1; const float IV18 = IV10 - IV8 + IV9; - _mag_innov_var(1) = IV11*P(17,20) + IV11*(IV11*P(17,17) + IV2*P(17,18) - IV3*P(16,17) + IV4*P(2,17) + IV5*P(0,17) + IV6*P(1,17) - IV7*P(3,17) + P(17,20)) + IV2*P(18,20) + IV2*(IV11*P(17,18) + IV2*P(18,18) - IV3*P(16,18) + IV4*P(2,18) + IV5*P(0,18) + IV6*P(1,18) - IV7*P(3,18) + P(18,20)) - IV3*P(16,20) - IV3*(IV11*P(16,17) + IV2*P(16,18) - IV3*P(16,16) + IV4*P(2,16) + IV5*P(0,16) + IV6*P(1,16) - IV7*P(3,16) + P(16,20)) + IV4*P(2,20) + IV4*(IV11*P(2,17) - IV12 + IV2*P(2,18) - IV3*P(2,16) + IV4*P(2,2) + IV5*P(0,2) + IV6*P(1,2) + P(2,20)) + IV5*P(0,20) + IV5*(IV11*P(0,17) + IV14 + IV2*P(0,18) - IV3*P(0,16) + IV4*P(0,2) + IV5*P(0,0) - IV7*P(0,3) + P(0,20)) + IV6*P(1,20) + IV6*(IV11*P(1,17) + IV13 + IV2*P(1,18) - IV3*P(1,16) + IV4*P(1,2) + IV6*P(1,1) - IV7*P(1,3) + P(1,20)) - IV7*P(3,20) - IV7*(IV11*P(3,17) + IV15 + IV2*P(3,18) - IV3*P(3,16) + IV5*P(0,3) + IV6*P(1,3) - IV7*P(3,3) + P(3,20)) + P(20,20) + R_MAG; - _mag_innov_var(2) = IV16*P(16,21) + IV16*(IV16*P(16,16) - IV17*P(16,17) + IV18*P(16,18) + IV4*P(3,16) - IV5*P(1,16) + IV6*P(0,16) + IV7*P(2,16) + P(16,21)) - IV17*P(17,21) - IV17*(IV16*P(16,17) - IV17*P(17,17) + IV18*P(17,18) + IV4*P(3,17) - IV5*P(1,17) + IV6*P(0,17) + IV7*P(2,17) + P(17,21)) + IV18*P(18,21) + IV18*(IV16*P(16,18) - IV17*P(17,18) + IV18*P(18,18) + IV4*P(3,18) - IV5*P(1,18) + IV6*P(0,18) + IV7*P(2,18) + P(18,21)) + IV4*P(3,21) + IV4*(IV12 + IV16*P(3,16) - IV17*P(3,17) + IV18*P(3,18) + IV4*P(3,3) - IV5*P(1,3) + IV6*P(0,3) + P(3,21)) - IV5*P(1,21) - IV5*(IV14 + IV16*P(1,16) - IV17*P(1,17) + IV18*P(1,18) + IV4*P(1,3) - IV5*P(1,1) + IV7*P(1,2) + P(1,21)) + IV6*P(0,21) + IV6*(-IV13 + IV16*P(0,16) - IV17*P(0,17) + IV18*P(0,18) + IV4*P(0,3) + IV6*P(0,0) + IV7*P(0,2) + P(0,21)) + IV7*P(2,21) + IV7*(IV15 + IV16*P(2,16) - IV17*P(2,17) + IV18*P(2,18) - IV5*P(1,2) + IV6*P(0,2) + IV7*P(2,2) + P(2,21)) + P(21,21) + R_MAG; + aid_src_mag.innovation_variance[1] = IV11*P(17,20) + IV11*(IV11*P(17,17) + IV2*P(17,18) - IV3*P(16,17) + IV4*P(2,17) + IV5*P(0,17) + IV6*P(1,17) - IV7*P(3,17) + P(17,20)) + IV2*P(18,20) + IV2*(IV11*P(17,18) + IV2*P(18,18) - IV3*P(16,18) + IV4*P(2,18) + IV5*P(0,18) + IV6*P(1,18) - IV7*P(3,18) + P(18,20)) - IV3*P(16,20) - IV3*(IV11*P(16,17) + IV2*P(16,18) - IV3*P(16,16) + IV4*P(2,16) + IV5*P(0,16) + IV6*P(1,16) - IV7*P(3,16) + P(16,20)) + IV4*P(2,20) + IV4*(IV11*P(2,17) - IV12 + IV2*P(2,18) - IV3*P(2,16) + IV4*P(2,2) + IV5*P(0,2) + IV6*P(1,2) + P(2,20)) + IV5*P(0,20) + IV5*(IV11*P(0,17) + IV14 + IV2*P(0,18) - IV3*P(0,16) + IV4*P(0,2) + IV5*P(0,0) - IV7*P(0,3) + P(0,20)) + IV6*P(1,20) + IV6*(IV11*P(1,17) + IV13 + IV2*P(1,18) - IV3*P(1,16) + IV4*P(1,2) + IV6*P(1,1) - IV7*P(1,3) + P(1,20)) - IV7*P(3,20) - IV7*(IV11*P(3,17) + IV15 + IV2*P(3,18) - IV3*P(3,16) + IV5*P(0,3) + IV6*P(1,3) - IV7*P(3,3) + P(3,20)) + P(20,20) + R_MAG; + aid_src_mag.innovation_variance[2] = IV16*P(16,21) + IV16*(IV16*P(16,16) - IV17*P(16,17) + IV18*P(16,18) + IV4*P(3,16) - IV5*P(1,16) + IV6*P(0,16) + IV7*P(2,16) + P(16,21)) - IV17*P(17,21) - IV17*(IV16*P(16,17) - IV17*P(17,17) + IV18*P(17,18) + IV4*P(3,17) - IV5*P(1,17) + IV6*P(0,17) + IV7*P(2,17) + P(17,21)) + IV18*P(18,21) + IV18*(IV16*P(16,18) - IV17*P(17,18) + IV18*P(18,18) + IV4*P(3,18) - IV5*P(1,18) + IV6*P(0,18) + IV7*P(2,18) + P(18,21)) + IV4*P(3,21) + IV4*(IV12 + IV16*P(3,16) - IV17*P(3,17) + IV18*P(3,18) + IV4*P(3,3) - IV5*P(1,3) + IV6*P(0,3) + P(3,21)) - IV5*P(1,21) - IV5*(IV14 + IV16*P(1,16) - IV17*P(1,17) + IV18*P(1,18) + IV4*P(1,3) - IV5*P(1,1) + IV7*P(1,2) + P(1,21)) + IV6*P(0,21) + IV6*(-IV13 + IV16*P(0,16) - IV17*P(0,17) + IV18*P(0,18) + IV4*P(0,3) + IV6*P(0,0) + IV7*P(0,2) + P(0,21)) + IV7*P(2,21) + IV7*(IV15 + IV16*P(2,16) - IV17*P(2,17) + IV18*P(2,18) - IV5*P(1,2) + IV6*P(0,2) + IV7*P(2,2) + P(2,21)) + P(21,21) + R_MAG; - // chedk innovation variances for being badly conditioned - - if (_mag_innov_var(1) < R_MAG) { + // check innovation variances for being badly conditioned + if (aid_src_mag.innovation_variance[1] < R_MAG) { // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned _fault_status.flags.bad_mag_y = true; @@ -143,7 +142,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) _fault_status.flags.bad_mag_y = false; - if (_mag_innov_var(2) < R_MAG) { + if (aid_src_mag.innovation_variance[2] < R_MAG) { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned _fault_status.flags.bad_mag_z = true; @@ -157,46 +156,40 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) // rotate magnetometer earth field state into body frame const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal); - const Vector3f mag_I_rot = R_to_body * _state.mag_I; // compute magnetometer innovations - _mag_innov = mag_I_rot + _state.mag_B - mag; + const Vector3f mag_observation = mag - _state.mag_B; + Vector3f mag_innov = mag_I_rot - mag_observation; // do not use the synthesized measurement for the magnetomter Z component for 3D fusion if (_control_status.flags.synthetic_mag_z) { - _mag_innov(2) = 0.0f; + mag_innov(2) = 0.0f; } + for (int i = 0; i < 3; i++) { + aid_src_mag.observation[i] = mag_observation(i); + aid_src_mag.observation_variance[i] = R_MAG; + aid_src_mag.innovation[i] = mag_innov(i); + aid_src_mag.fusion_enabled[i] = _control_status.flags.mag_3D && update_all_states; + } + + // do not use the synthesized measurement for the magnetomter Z component for 3D fusion + if (_control_status.flags.synthetic_mag_z) { + aid_src_mag.innovation[2] = 0.0f; + aid_src_mag.innovation_rejected[2] = false; + } + + const float innov_gate = math::max(_params.mag_innov_gate, 1.f); + setEstimatorAidStatusTestRatio(aid_src_mag, innov_gate); + // Perform an innovation consistency check and report the result - bool all_innovation_checks_passed = true; - - for (uint8_t index = 0; index <= 2; index++) { - _mag_test_ratio(index) = sq(_mag_innov(index)) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var(index)); - - bool rejected = (_mag_test_ratio(index) > 1.f); - - switch (index) { - case 0: - _innov_check_fail_status.flags.reject_mag_x = rejected; - break; - - case 1: - _innov_check_fail_status.flags.reject_mag_y = rejected; - break; - - case 2: - _innov_check_fail_status.flags.reject_mag_z = rejected; - break; - } - - if (rejected) { - all_innovation_checks_passed = false; - } - } + _innov_check_fail_status.flags.reject_mag_x = aid_src_mag.innovation_rejected[0]; + _innov_check_fail_status.flags.reject_mag_y = aid_src_mag.innovation_rejected[1]; + _innov_check_fail_status.flags.reject_mag_z = aid_src_mag.innovation_rejected[2]; // if any axis fails, abort the mag fusion - if (!all_innovation_checks_passed) { + if (aid_src_mag.innovation_rejected[0] || aid_src_mag.innovation_rejected[1] || aid_src_mag.innovation_rejected[2]) { return false; } @@ -246,7 +239,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) } else if (index == 1) { - // recalculate innovation variance becasue states and covariances have changed due to previous fusion + // recalculate innovation variance because states and covariances have changed due to previous fusion const float HKY0 = magD*q1 + magE*q0 - magN*q3; const float HKY1 = magD*q0 - magE*q1 + magN*q2; const float HKY2 = magD*q3 + magE*q2 + magN*q1; @@ -272,9 +265,9 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) const float HKY22 = HKY10*P(2,18) - HKY11*P(2,16) + HKY12*P(2,2) + HKY13*P(0,2) + HKY14*P(1,2) - HKY15*P(2,3) + HKY8*P(2,17) + P(2,20); const float HKY23 = HKY10*P(18,20) - HKY11*P(16,20) + HKY12*P(2,20) + HKY13*P(0,20) + HKY14*P(1,20) - HKY15*P(3,20) + HKY8*P(17,20) + P(20,20); - _mag_innov_var(1) = (HKY10*HKY20 - HKY11*HKY18 + HKY12*HKY22 + HKY13*HKY16 + HKY14*HKY21 - HKY15*HKY19 + HKY17*HKY8 + HKY23 + R_MAG); + aid_src_mag.innovation_variance[1] = (HKY10*HKY20 - HKY11*HKY18 + HKY12*HKY22 + HKY13*HKY16 + HKY14*HKY21 - HKY15*HKY19 + HKY17*HKY8 + HKY23 + R_MAG); - if (_mag_innov_var(1) < R_MAG) { + if (aid_src_mag.innovation_variance[1] < R_MAG) { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned _fault_status.flags.bad_mag_y = true; @@ -283,7 +276,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) ECL_ERR("magY %s", numerical_error_covariance_reset_string); return false; } - const float HKY24 = 1.0F/_mag_innov_var(1); + const float HKY24 = 1.0F/aid_src_mag.innovation_variance[1]; // Calculate Y axis observation jacobians Hfusion.setZero(); @@ -352,9 +345,9 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) const float HKZ22 = HKZ10*P(2,16) - HKZ11*P(2,17) + HKZ12*P(2,3) + HKZ13*P(0,2) - HKZ14*P(1,2) + HKZ15*P(2,2) + HKZ9*P(2,18) + P(2,21); const float HKZ23 = HKZ10*P(16,21) - HKZ11*P(17,21) + HKZ12*P(3,21) + HKZ13*P(0,21) - HKZ14*P(1,21) + HKZ15*P(2,21) + HKZ9*P(18,21) + P(21,21); - _mag_innov_var(2) = (HKZ10*HKZ20 - HKZ11*HKZ18 + HKZ12*HKZ21 + HKZ13*HKZ16 - HKZ14*HKZ19 + HKZ15*HKZ22 + HKZ17*HKZ9 + HKZ23 + R_MAG); + aid_src_mag.innovation_variance[2] = (HKZ10*HKZ20 - HKZ11*HKZ18 + HKZ12*HKZ21 + HKZ13*HKZ16 - HKZ14*HKZ19 + HKZ15*HKZ22 + HKZ17*HKZ9 + HKZ23 + R_MAG); - if (_mag_innov_var(2) < R_MAG) { + if (aid_src_mag.innovation_variance[2] < R_MAG) { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned _fault_status.flags.bad_mag_z = true; @@ -364,7 +357,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) return false; } - const float HKZ24 = 1.0F/_mag_innov_var(2); + const float HKZ24 = 1.0F/aid_src_mag.innovation_variance[2]; // calculate Z axis observation jacobians Hfusion.setZero(); @@ -404,7 +397,16 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) Kfusion(21) = HKZ23*HKZ24; } - const bool is_fused = measurementUpdate(Kfusion, Hfusion, _mag_innov(index)); + const bool is_fused = measurementUpdate(Kfusion, Hfusion, aid_src_mag.innovation[index]); + + if (is_fused) { + aid_src_mag.fused[index] = true; + aid_src_mag.time_last_fuse[index] = _time_last_imu; + + } else { + aid_src_mag.fused[index] = false; + } + switch (index) { case 0: @@ -425,17 +427,14 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) } } - if (!_fault_status.flags.bad_mag_x && !_fault_status.flags.bad_mag_y && !_fault_status.flags.bad_mag_z) { - _time_last_mag_3d_fuse = _time_last_imu; - return true; - } - - return false; + return aid_src_mag.fused[0] && aid_src_mag.fused[1] && aid_src_mag.fused[2]; } // update quaternion states and covariances using the yaw innovation and yaw observation variance -bool Ekf::fuseYaw(const float innovation, const float variance) +bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s& aid_src_status) { + aid_src_status.innovation = innovation; + // assign intermediate state variables const float q0 = _state.quat_nominal(0); const float q1 = _state.quat_nominal(1); @@ -551,7 +550,7 @@ bool Ekf::fuseYaw(const float innovation, const float variance) // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero // calculate the innovation variance - _heading_innov_var = variance; + aid_src_status.innovation_variance = variance; for (unsigned row = 0; row <= 3; row++) { float tmp = 0.0f; @@ -560,16 +559,16 @@ bool Ekf::fuseYaw(const float innovation, const float variance) tmp += P(row, col) * H_YAW(col); } - _heading_innov_var += H_YAW(row) * tmp; + aid_src_status.innovation_variance += H_YAW(row) * tmp; } - float heading_innov_var_inv; + float heading_innov_var_inv = 0.f; // check if the innovation variance calculation is badly conditioned - if (_heading_innov_var >= variance) { + if (aid_src_status.innovation_variance >= variance) { // the innovation variance contribution from the state covariances is not negative, no fault _fault_status.flags.bad_hdg = false; - heading_innov_var_inv = 1.0f / _heading_innov_var; + heading_innov_var_inv = 1.f / aid_src_status.innovation_variance; } else { // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned @@ -607,10 +606,10 @@ bool Ekf::fuseYaw(const float innovation, const float variance) float gate_sigma = math::max(_params.heading_innov_gate, 1.f); // innovation test ratio - _yaw_test_ratio = sq(innovation) / (sq(gate_sigma) * _heading_innov_var); + setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma); // set the magnetometer unhealthy if the test fails - if (_yaw_test_ratio > 1.0f) { + if (aid_src_status.innovation_rejected) { _innov_check_fail_status.flags.reject_yaw = true; // if we are in air we don't want to fuse the measurement @@ -618,13 +617,13 @@ bool Ekf::fuseYaw(const float innovation, const float variance) // by interference or a large initial gyro bias if (!_control_status.flags.in_air && isTimedOut(_time_last_in_air, (uint64_t)5e6) - && isTimedOut(_time_last_heading_fuse, (uint64_t)1e6) + && isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6) ) { // constrain the innovation to the maximum set by the gate // we need to delay this forced fusion to avoid starting it // immediately after touchdown, when the drone is still armed - float gate_limit = sqrtf((sq(gate_sigma) * _heading_innov_var)); - _heading_innov = math::constrain(innovation, -gate_limit, gate_limit); + float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance)); + aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit); // also reset the yaw gyro variance to converge faster and avoid // being stuck on a previous bad estimate @@ -636,7 +635,6 @@ bool Ekf::fuseYaw(const float innovation, const float variance) } else { _innov_check_fail_status.flags.reject_yaw = false; - _heading_innov = innovation; } // apply covariance correction via P_new = (I -K*H)*P @@ -672,9 +670,11 @@ bool Ekf::fuseYaw(const float innovation, const float variance) fixCovarianceErrors(true); // apply the state corrections - fuse(Kfusion, _heading_innov); + fuse(Kfusion, aid_src_status.innovation); _time_last_heading_fuse = _time_last_imu; + aid_src_status.time_last_fuse = _time_last_imu; + aid_src_status.fused = true; return true; } diff --git a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp index bf5dde9268..140875b9d6 100644 --- a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp +++ b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp @@ -47,7 +47,8 @@ void Ekf::controlZeroInnovationHeadingUpdate() // fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low float innovation = 0.f; float obs_var = _control_status.flags.vehicle_at_rest ? 0.001f : 0.1f; - fuseYaw(innovation, obs_var); + estimator_aid_source_1d_s unused; + fuseYaw(innovation, obs_var, unused); _last_static_yaw = NAN; @@ -61,7 +62,8 @@ void Ekf::controlZeroInnovationHeadingUpdate() if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) { float innovation = wrap_pi(euler_yaw - _last_static_yaw); float obs_var = 0.01f; - fuseYaw(innovation, obs_var); + estimator_aid_source_1d_s unused; + fuseYaw(innovation, obs_var, unused); } } else { @@ -76,7 +78,8 @@ void Ekf::controlZeroInnovationHeadingUpdate() if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) { float innovation = 0.f; float obs_var = 0.01f; - fuseYaw(innovation, obs_var); + estimator_aid_source_1d_s unused; + fuseYaw(innovation, obs_var, unused); } _last_static_yaw = NAN; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c373db0004..8730189609 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -650,10 +650,19 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) // fake position PublishAidSourceStatus(_ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub); - // GNSS velocity & position + // EV yaw + PublishAidSourceStatus(_ekf.aid_src_ev_yaw(), _status_ev_yaw_pub_last, _estimator_aid_src_ev_yaw_pub); + + // GNSS yaw/velocity/position + PublishAidSourceStatus(_ekf.aid_src_gnss_yaw(), _status_gnss_yaw_pub_last, _estimator_aid_src_gnss_yaw_pub); PublishAidSourceStatus(_ekf.aid_src_gnss_vel(), _status_gnss_vel_pub_last, _estimator_aid_src_gnss_vel_pub); PublishAidSourceStatus(_ekf.aid_src_gnss_pos(), _status_gnss_pos_pub_last, _estimator_aid_src_gnss_pos_pub); + // mag heading + PublishAidSourceStatus(_ekf.aid_src_mag_heading(), _status_mag_heading_pub_last, _estimator_aid_src_mag_heading_pub); + + // mag 3d + PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub); } void EKF2::PublishAttitude(const hrt_abstime ×tamp) @@ -1368,9 +1377,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.reject_ver_vel = _ekf.innov_check_fail_status_flags().reject_ver_vel; status_flags.reject_hor_pos = _ekf.innov_check_fail_status_flags().reject_hor_pos; status_flags.reject_ver_pos = _ekf.innov_check_fail_status_flags().reject_ver_pos; - status_flags.reject_mag_x = _ekf.innov_check_fail_status_flags().reject_mag_x; - status_flags.reject_mag_y = _ekf.innov_check_fail_status_flags().reject_mag_y; - status_flags.reject_mag_z = _ekf.innov_check_fail_status_flags().reject_mag_z; status_flags.reject_yaw = _ekf.innov_check_fail_status_flags().reject_yaw; status_flags.reject_airspeed = _ekf.innov_check_fail_status_flags().reject_airspeed; status_flags.reject_sideslip = _ekf.innov_check_fail_status_flags().reject_sideslip; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 77931a5e02..fd712747a5 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -257,9 +257,15 @@ private: hrt_abstime _status_fake_pos_pub_last{0}; + hrt_abstime _status_ev_yaw_pub_last{0}; + + hrt_abstime _status_gnss_yaw_pub_last{0}; hrt_abstime _status_gnss_vel_pub_last{0}; hrt_abstime _status_gnss_pos_pub_last{0}; + hrt_abstime _status_mag_pub_last{0}; + hrt_abstime _status_mag_heading_pub_last{0}; + float _last_baro_bias_published{}; float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements @@ -324,9 +330,16 @@ private: uORB::PublicationMulti _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)}; + uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)}; uORB::PublicationMulti _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)}; uORB::PublicationMulti _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)}; + uORB::PublicationMulti _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)}; + uORB::PublicationMulti _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)}; + + uORB::PublicationMulti _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)}; + + // publications with topic dependent on multi-mode uORB::PublicationMulti _attitude_pub; uORB::PublicationMulti _local_position_pub; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 01d5cbe406..01fde8f176 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -171,6 +171,17 @@ void LoggedTopics::add_default_topics() 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); + // add_optional_topic_multi("estimator_aid_src_baro_hgt", 100, MAX_ESTIMATOR_INSTANCES); + // add_optional_topic_multi("estimator_aid_src_rng_hgt", 100, MAX_ESTIMATOR_INSTANCES); + // add_optional_topic_multi("estimator_aid_src_fake_pos", 100, MAX_ESTIMATOR_INSTANCES); + // add_optional_topic_multi("estimator_aid_src_gnss_yaw", 100, MAX_ESTIMATOR_INSTANCES); + // add_optional_topic_multi("estimator_aid_src_gnss_vel", 100, MAX_ESTIMATOR_INSTANCES); + // add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES); + // add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES); + // add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES); + // add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); + // log all raw sensors at minimal rate (at least 1 Hz) add_topic_multi("battery_status", 200, 2); add_topic_multi("differential_pressure", 1000, 2); @@ -245,6 +256,18 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_local_position"); add_topic("wind"); add_topic("yaw_estimator_status"); + + add_optional_topic_multi("estimator_aid_src_airspeed", 0, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_baro_hgt", 0, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_rng_hgt", 0, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_fake_pos", 0, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_gnss_vel", 0, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_gnss_pos", 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); + #endif /* CONFIG_ARCH_BOARD_PX4_SITL */ }