diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 573cbbb64e..1d0deea066 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -1065,7 +1065,6 @@ void Ekf::controlAuxVelFusion() bool Ekf::hasHorizontalAidingTimedOut() const { return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max) && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max); } diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 5687e3df2b..0a8b86b4f6 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -213,7 +213,6 @@ bool Ekf::initialiseFilter() // reset the essential fusion timeout counters _time_last_hgt_fuse = _time_last_imu; _time_last_hor_pos_fuse = _time_last_imu; - _time_last_delpos_fuse = _time_last_imu; _time_last_hor_vel_fuse = _time_last_imu; _time_last_hagl_fuse = _time_last_imu; _time_last_flow_terrain_fuse = _time_last_imu; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 58767983b8..f5837efdc0 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -102,8 +102,8 @@ public: 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 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(); } @@ -219,8 +219,8 @@ public: { const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg); const bool is_mag_alignment_in_flight_complete = is_using_mag - && _control_status.flags.mag_aligned_in_flight - && ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)1e6); + && _control_status.flags.mag_aligned_in_flight + && ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)1e6); return _control_status.flags.yaw_align && (is_mag_alignment_in_flight_complete || !is_using_mag); } @@ -380,7 +380,6 @@ private: uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of vertical position measurements was performed (uSec) uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec) uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec) - uint64_t _time_last_delpos_fuse{0}; ///< time the last fusion of incremental horizontal position measurements was performed (uSec) uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec) 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_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec) @@ -584,20 +583,20 @@ private: // yaw : angle observation defined as the first rotation in a 321 Tait-Bryan rotation sequence (rad) // yaw_variance : variance of the yaw angle observation (rad^2) // zero_innovation : Fuse data with innovation set to zero - void fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation = false); + bool fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation = false); // fuse the yaw angle defined as the first rotation in a 312 Tait-Bryan rotation sequence // yaw : angle observation defined as the first rotation in a 312 Tait-Bryan rotation sequence (rad) // yaw_variance : variance of the yaw angle observation (rad^2) // zero_innovation : Fuse data with innovation set to zero - void fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation = false); + bool fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation = false); // update quaternion states and covariances using an innovation, observation variance and Jacobian vector // innovation : prediction - measurement // variance : observaton variance // gate_sigma : innovation consistency check gate size (Sigma) // jacobian : 4x1 vector of partial derivatives of observation wrt each quaternion state - void updateQuaternion(const float innovation, const float variance, const float gate_sigma, + bool updateQuaternion(const float innovation, const float variance, const float gate_sigma, const Vector4f &yaw_jacobian); // fuse the yaw angle obtained from a dual antenna GPS unit @@ -629,7 +628,7 @@ private: void fuseEvHgt(); // fuse single velocity and position measurement - void fuseVelPosHeight(const float innov, const float innov_var, const int obs_index); + bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index); void resetVelocity(); @@ -1024,7 +1023,7 @@ private: void stopFakePosFusion(); void fuseFakePosition(); - void setVelPosFaultStatus(const int index, const bool status); + void setVelPosStatus(const int index, const bool healthy); // reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch // yaw : Euler yaw angle (rad) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 895327b604..ebefbe52dd 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1013,8 +1013,8 @@ void Ekf::update_deadreckoning_status() { const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel) && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) - || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max) - || isRecent(_time_last_delpos_fuse, _params.no_aid_timeout_max)); + || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)); + const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max); const bool airDataAiding = _control_status.flags.wind && isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) && diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index f967952858..5922d3211e 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -401,14 +401,18 @@ void Ekf::fuseMag(const Vector3f &mag) const bool is_fused = measurementUpdate(Kfusion, Hfusion, _mag_innov(index)); - if (index == 0) { + switch (index) { + case 0: _fault_status.flags.bad_mag_x = !is_fused; + break; - } else if (index == 1) { + case 1: _fault_status.flags.bad_mag_y = !is_fused; + break; - } else if (index == 2) { + case 2: _fault_status.flags.bad_mag_z = !is_fused; + break; } if (is_fused) { @@ -417,7 +421,7 @@ void Ekf::fuseMag(const Vector3f &mag) } } -void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation) +bool Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation) { // assign intermediate state variables const float &q0 = _state.quat_nominal(0); @@ -481,7 +485,7 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation) H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2); H_YAW(3) = -SB5*(-SB0*SB7 - SB9*q3); } else { - return; + return false; } // calculate the yaw innovation and wrap to the interval between +-pi @@ -498,10 +502,10 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation) float innov_gate = math::max(_params.heading_innov_gate, 1.0f); // Update the quaternion states and covariance matrix - updateQuaternion(innovation, R_YAW, innov_gate, H_YAW); + return updateQuaternion(innovation, R_YAW, innov_gate, H_YAW); } -void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation) +bool Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation) { // assign intermediate state variables const float q0 = _state.quat_nominal(0); @@ -565,7 +569,7 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation) H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2); H_YAW(3) = -SB5*(SB0*SB7 + SB9*q3); } else { - return; + return false; } float innovation; @@ -582,11 +586,11 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation) float innov_gate = math::max(_params.heading_innov_gate, 1.0f); // Update the quaternion states and covariance matrix - updateQuaternion(innovation, R_YAW, innov_gate, H_YAW); + return updateQuaternion(innovation, R_YAW, innov_gate, H_YAW); } // update quaternion states and covariances using the yaw innovation, yaw observation variance and yaw Jacobian -void Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma, +bool Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma, const Vector4f &yaw_jacobian) { // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero @@ -618,7 +622,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f // we reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); ECL_ERR("mag yaw fusion numerical error - covariance reset"); - return; + return false; } // calculate the Kalman gains @@ -668,7 +672,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f resetZDeltaAngBiasCov(); } else { - return; + return false; } } else { @@ -711,7 +715,10 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f // apply the state corrections fuse(Kfusion, _heading_innov); + return true; } + + return false; } void Ekf::fuseHeading(float measured_hdg, float obs_var) diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index 064165b160..8826ce4ab8 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -47,7 +47,6 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio) { - innov_var(0) = P(4, 4) + obs_var(0); innov_var(1) = P(5, 5) + obs_var(1); test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate) * innov_var(0)), @@ -56,13 +55,12 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const float innov_gate, const bool innov_check_pass = (test_ratio(0) <= 1.0f); if (innov_check_pass) { - _time_last_hor_vel_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_hor_vel = false; - fuseVelPosHeight(innov(0), innov_var(0), 0); - fuseVelPosHeight(innov(1), innov_var(1), 1); + bool fuse_vx = fuseVelPosHeight(innov(0), innov_var(0), 0); + bool fuse_vy = fuseVelPosHeight(innov(1), innov_var(1), 1); - return true; + return fuse_vx && fuse_vy; } else { _innov_check_fail_status.flags.reject_hor_vel = true; @@ -73,7 +71,6 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const float innov_gate, bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio) { - innov_var(2) = P(6, 6) + obs_var(2); test_ratio(1) = sq(innov(2)) / (sq(innov_gate) * innov_var(2)); _vert_vel_innov_ratio = innov(2) / sqrtf(innov_var(2)); @@ -94,12 +91,9 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const float innov_gate, co } if (innov_check_pass) { - _time_last_ver_vel_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_ver_vel = false; - fuseVelPosHeight(innovation, innov_var(2), 2); - - return true; + return fuseVelPosHeight(innovation, innov_var(2), 2); } else { _innov_check_fail_status.flags.reject_ver_vel = true; @@ -124,19 +118,12 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const float innov_gate, return false; } - if (!_fuse_hpos_as_odom) { - _time_last_hor_pos_fuse = _time_last_imu; - - } else { - _time_last_delpos_fuse = _time_last_imu; - } - _innov_check_fail_status.flags.reject_hor_pos = false; - fuseVelPosHeight(innov(0), innov_var(0), 3); - fuseVelPosHeight(innov(1), innov_var(1), 4); + bool fuse_x = fuseVelPosHeight(innov(0), innov_var(0), 3); + bool fuse_y = fuseVelPosHeight(innov(1), innov_var(1), 4); - return true; + return fuse_x && fuse_y; } else { _innov_check_fail_status.flags.reject_hor_pos = true; @@ -167,11 +154,9 @@ bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const } if (innov_check_pass) { - _time_last_hgt_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_ver_pos = false; - fuseVelPosHeight(innovation, innov_var, 5); - return true; + return fuseVelPosHeight(innovation, innov_var, 5); } else { _innov_check_fail_status.flags.reject_ver_pos = true; @@ -180,9 +165,8 @@ bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const } // Helper function that fuses a single velocity or position measurement -void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index) +bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index) { - Vector24f Kfusion; // Kalman gain vector for any single observation - sequential fusion is used. const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state @@ -212,7 +196,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o } } - setVelPosFaultStatus(obs_index, !healthy); + setVelPosStatus(obs_index, healthy); if (healthy) { // apply the covariance corrections @@ -222,27 +206,80 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o // apply the state corrections fuse(Kfusion, innov); + + return true; } + + return false; } -void Ekf::setVelPosFaultStatus(const int index, const bool status) +void Ekf::setVelPosStatus(const int index, const bool healthy) { - if (index == 0) { - _fault_status.flags.bad_vel_N = status; + switch (index) { + case 0: + if (healthy) { + _fault_status.flags.bad_vel_N = false; + _time_last_hor_vel_fuse = _time_last_imu; + + } else { + _fault_status.flags.bad_vel_N = true; + } + + break; + + case 1: + if (healthy) { + _fault_status.flags.bad_vel_E = false; + _time_last_hor_vel_fuse = _time_last_imu; - } else if (index == 1) { - _fault_status.flags.bad_vel_E = status; + } else { + _fault_status.flags.bad_vel_E = true; + } + + break; - } else if (index == 2) { - _fault_status.flags.bad_vel_D = status; + case 2: + if (healthy) { + _fault_status.flags.bad_vel_D = false; + _time_last_ver_vel_fuse = _time_last_imu; + + } else { + _fault_status.flags.bad_vel_D = true; + } - } else if (index == 3) { - _fault_status.flags.bad_pos_N = status; + break; + + case 3: + if (healthy) { + _fault_status.flags.bad_pos_N = false; + _time_last_hor_pos_fuse = _time_last_imu; + + } else { + _fault_status.flags.bad_pos_N = true; + } - } else if (index == 4) { - _fault_status.flags.bad_pos_E = status; + break; + + case 4: + if (healthy) { + _fault_status.flags.bad_pos_E = false; + _time_last_hor_pos_fuse = _time_last_imu; + + } else { + _fault_status.flags.bad_pos_E = true; + } + + break; + + case 5: + if (healthy) { + _fault_status.flags.bad_pos_D = false; + _time_last_hgt_fuse = _time_last_imu; + + } else { + _fault_status.flags.bad_pos_D = true; + } - } else if (index == 5) { - _fault_status.flags.bad_pos_D = status; + break; } }