From becffa3441e53d0f63c142b6c2360ad705d18f21 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 1 Apr 2022 11:09:44 -0400 Subject: [PATCH] ekf2: cleanup zero innovation heading fusion --- src/modules/ekf2/CMakeLists.txt | 1 + src/modules/ekf2/EKF/CMakeLists.txt | 1 + src/modules/ekf2/EKF/control.cpp | 13 +- src/modules/ekf2/EKF/ekf.h | 24 +- src/modules/ekf2/EKF/ekf_helper.cpp | 3 +- src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 5 +- src/modules/ekf2/EKF/mag_control.cpp | 22 +- src/modules/ekf2/EKF/mag_fusion.cpp | 301 ++++++------------ .../EKF/zero_innovation_heading_update.cpp | 81 +++++ 9 files changed, 203 insertions(+), 248 deletions(-) create mode 100644 src/modules/ekf2/EKF/zero_innovation_heading_update.cpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index e47d1f2f60..6f94ab5fc9 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -71,6 +71,7 @@ px4_add_module( EKF/terrain_estimator.cpp EKF/utils.cpp EKF/vel_pos_fusion.cpp + EKF/zero_innovation_heading_update.cpp EKF/zero_velocity_update.cpp EKF2.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 745317d132..6523943b39 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -56,6 +56,7 @@ add_library(ecl_EKF terrain_estimator.cpp utils.cpp vel_pos_fusion.cpp + zero_innovation_heading_update.cpp zero_velocity_update.cpp ) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index b3f2882321..5792fe2b73 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -181,6 +181,8 @@ void Ekf::controlFusionModes() // Additional horizontal velocity data from an auxiliary sensor can be fused controlAuxVelFusion(); + controlZeroInnovationHeadingUpdate(); + controlZeroVelocityUpdate(); // Fake position measurement for constraining drift when no other velocity or position measurements @@ -357,14 +359,11 @@ void Ekf::controlExternalVisionFusion() resetYawToEv(); } - if (shouldUse321RotationSequence(_R_to_earth)) { - float measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat); - fuseYaw321(measured_hdg, _ev_sample_delayed.angVar); + float measured_hdg = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed.quat) : getEuler312Yaw(_ev_sample_delayed.quat); - } else { - float measured_hdg = getEuler312Yaw(_ev_sample_delayed.quat); - fuseYaw312(measured_hdg, _ev_sample_delayed.angVar); - } + float innovation = wrap_pi(getEulerYaw(_R_to_earth)) - wrap_pi(measured_hdg); + float obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f); + updateQuaternion(innovation, obs_var); } // record observation and estimate for use next time diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e8f2b18228..28a3efff2b 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -587,28 +587,10 @@ private: // ekf sequential fusion of magnetometer measurements bool fuseMag(const Vector3f &mag, bool update_all_states = true); - // fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer) - bool fuseHeading(float measured_hdg = NAN, float obs_var = NAN); - - // fuse the yaw angle defined as the first rotation in a 321 Tait-Bryan rotation sequence - // 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 - 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 - 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 + // update quaternion states and covariances using a yaw innovation and yaw observation variance // 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 - bool updateQuaternion(const float innovation, const float variance, const float gate_sigma, - const Vector4f &yaw_jacobian); + bool updateQuaternion(const float innovation, const float variance); // fuse the yaw angle obtained from a dual antenna GPS unit void fuseGpsYaw(); @@ -882,6 +864,8 @@ private: void controlZeroVelocityUpdate(); + void controlZeroInnovationHeadingUpdate(); + // control fusion of auxiliary velocity observations void controlAuxVelFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 445bfea6a9..172973bc28 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1211,7 +1211,6 @@ void Ekf::stopMag3DFusion() if (_control_status.flags.mag_3D) { saveMagCovData(); - _control_status.flags.mag_3D = false; _control_status.flags.mag_dec = false; @@ -1747,6 +1746,8 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer) _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; } + _last_static_yaw = NAN; + // capture the reset event _state_reset_status.quat_counter++; diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index efc0bed755..2e2c8d2d05 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -156,8 +156,8 @@ void Ekf::fuseGpsYaw() _yaw_signed_test_ratio_lpf.update(matrix::sign(_heading_innov) * _yaw_test_ratio); - if (!_control_status.flags.in_air - && fabsf(_yaw_signed_test_ratio_lpf.getState()) > 0.2f) { + if ((fabsf(_yaw_signed_test_ratio_lpf.getState()) > 0.2f) + && !_control_status.flags.in_air && isTimedOut(_time_last_heading_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 @@ -189,6 +189,7 @@ void Ekf::fuseGpsYaw() if (is_fused) { _time_last_gps_yaw_fuse = _time_last_imu; + _time_last_heading_fuse = _time_last_imu; } } diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index b3667c57c9..b8ebb06119 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -83,25 +83,11 @@ void Ekf::controlMagFusion() _time_last_mov_3d_mag_suitable = _imu_sample_delayed.time_us; } - // When operating without a magnetometer and no other source of yaw aiding is active, - // yaw fusion is run selectively to enable yaw gyro bias learning when stationary on - // ground and to prevent uncontrolled yaw variance growth - // Also fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE || _control_status.flags.mag_fault || !_control_status.flags.tilt_align) { stopMagFusion(); - - if (!_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) { - // TODO: setting _is_yaw_fusion_inhibited to true is required to tell - // fuseHeading to perform a "zero innovation heading fusion" - // We should refactor it to avoid using this flag here - _is_yaw_fusion_inhibited = true; - fuseHeading(); - _is_yaw_fusion_inhibited = false; - } - return; } @@ -340,9 +326,15 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag) Vector3f mag_earth_pred = R_to_earth * (mag - _state.mag_B); // the angle of the projection onto the horizontal gives the yaw angle + // calculate the yaw innovation and wrap to the interval between +-pi float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); - if (fuseHeading(measured_hdg, sq(_params.mag_heading_noise))) { + float innovation = wrap_pi(getEulerYaw(_R_to_earth)) - wrap_pi(measured_hdg); + + float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f); + + // Update the quaternion states and covariance matrix + if (updateQuaternion(innovation, obs_var)) { _time_last_mag_heading_fuse = _time_last_imu; } } diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 386ca3f093..b4ef11a9b7 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -433,91 +433,8 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) return false; } -bool Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation) -{ - // assign intermediate state variables - const float &q0 = _state.quat_nominal(0); - const float &q1 = _state.quat_nominal(1); - const float &q2 = _state.quat_nominal(2); - const float &q3 = _state.quat_nominal(3); - - const float R_YAW = fmaxf(yaw_variance, 1.0e-4f); - const float measurement = wrap_pi(yaw); - - // calculate 321 yaw observation matrix - // choose A or B computational paths to avoid singularity in derivation at +-90 degrees yaw - bool canUseA = false; - const float SA0 = 2*q3; - const float SA1 = 2*q2; - const float SA2 = SA0*q0 + SA1*q1; - const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3); - float SA4, SA5_inv; - - if (sq(SA3) > 1e-6f) { - SA4 = 1.0F/sq(SA3); - SA5_inv = sq(SA2)*SA4 + 1; - canUseA = fabsf(SA5_inv) > 1e-6f; - } - - bool canUseB = false; - const float SB0 = 2*q0; - const float SB1 = 2*q1; - const float SB2 = SB0*q3 + SB1*q2; - const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3); - float SB3, SB5_inv; - - if (sq(SB2) > 1e-6f) { - SB3 = 1.0F/sq(SB2); - SB5_inv = SB3*sq(SB4) + 1; - canUseB = fabsf(SB5_inv) > 1e-6f; - } - - Vector4f H_YAW; - - if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) { - const float SA5 = 1.0F/SA5_inv; - const float SA6 = 1.0F/SA3; - const float SA7 = SA2*SA4; - const float SA8 = 2*SA7; - const float SA9 = 2*SA6; - - H_YAW(0) = SA5*(SA0*SA6 - SA8*q0); - H_YAW(1) = SA5*(SA1*SA6 - SA8*q1); - H_YAW(2) = SA5*(SA1*SA7 + SA9*q1); - H_YAW(3) = SA5*(SA0*SA7 + SA9*q0); - } else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) { - const float SB5 = 1.0F/SB5_inv; - const float SB6 = 1.0F/SB2; - const float SB7 = SB3*SB4; - const float SB8 = 2*SB7; - const float SB9 = 2*SB6; - - H_YAW(0) = -SB5*(SB0*SB6 - SB8*q3); - H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2); - H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2); - H_YAW(3) = -SB5*(-SB0*SB7 - SB9*q3); - } else { - return false; - } - - // calculate the yaw innovation and wrap to the interval between +-pi - float innovation; - - if (zero_innovation) { - innovation = 0.0f; - - } else { - innovation = wrap_pi(atan2f(_R_to_earth(1, 0), _R_to_earth(0, 0)) - measurement); - } - - // define the innovation gate size - float innov_gate = math::max(_params.heading_innov_gate, 1.0f); - - // Update the quaternion states and covariance matrix - return updateQuaternion(innovation, R_YAW, innov_gate, H_YAW); -} - -bool Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation) +// update quaternion states and covariances using the yaw innovation and yaw observation variance +bool Ekf::updateQuaternion(const float innovation, const float variance) { // assign intermediate state variables const float q0 = _state.quat_nominal(0); @@ -525,86 +442,113 @@ bool Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation) const float q2 = _state.quat_nominal(2); const float q3 = _state.quat_nominal(3); - const float R_YAW = fmaxf(yaw_variance, 1.0e-4f); - const float measurement = wrap_pi(yaw); - - // calculate 312 yaw observation matrix // choose A or B computational paths to avoid singularity in derivation at +-90 degrees yaw bool canUseA = false; - const float SA0 = 2*q3; - const float SA1 = 2*q2; - const float SA2 = SA0*q0 - SA1*q1; - const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3); - float SA4, SA5_inv; + bool canUseB = false; + + float SA0; + float SA1; + float SA2; + float SA3; + float SA4; + float SA5_inv; + + float SB0; + float SB1; + float SB2; + float SB3; + float SB4; + float SB5_inv; + + const bool fuse_yaw_321 = shouldUse321RotationSequence(_R_to_earth); + + if (fuse_yaw_321) { + // calculate 321 yaw observation matrix + SA0 = 2*q3; + SA1 = 2*q2; + SA2 = SA0*q0 + SA1*q1; + SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3); + + SB0 = 2*q0; + SB1 = 2*q1; + SB2 = SB0*q3 + SB1*q2; + SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3); + + } else { + // calculate 312 yaw observation matrix + SA0 = 2*q3; + SA1 = 2*q2; + SA2 = SA0*q0 - SA1*q1; + SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3); + + SB0 = 2*q0; + SB1 = 2*q1; + SB2 = -SB0*q3 + SB1*q2; + SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3); + } if (sq(SA3) > 1e-6f) { - SA4 = 1.0F/sq(SA3); - SA5_inv = sq(SA2)*SA4 + 1; + SA4 = 1.f/sq(SA3); + SA5_inv = sq(SA2)*SA4 + 1.f; canUseA = fabsf(SA5_inv) > 1e-6f; } - bool canUseB = false; - const float SB0 = 2*q0; - const float SB1 = 2*q1; - const float SB2 = -SB0*q3 + SB1*q2; - const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3); - float SB3, SB5_inv; - if (sq(SB2) > 1e-6f) { - SB3 = 1.0F/sq(SB2); - SB5_inv = SB3*sq(SB4) + 1; + SB3 = 1.f/sq(SB2); + SB5_inv = SB3*sq(SB4) + 1.f; canUseB = fabsf(SB5_inv) > 1e-6f; } Vector4f H_YAW; if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) { - const float SA5 = 1.0F/SA5_inv; - const float SA6 = 1.0F/SA3; + const float SA5 = 1.f/SA5_inv; + const float SA6 = 1.f/SA3; const float SA7 = SA2*SA4; const float SA8 = 2*SA7; const float SA9 = 2*SA6; - H_YAW(0) = SA5*(SA0*SA6 - SA8*q0); - H_YAW(1) = SA5*(-SA1*SA6 + SA8*q1); - H_YAW(2) = SA5*(-SA1*SA7 - SA9*q1); - H_YAW(3) = SA5*(SA0*SA7 + SA9*q0); + if (fuse_yaw_321) { + // calculate 321 yaw observation matrix + H_YAW(0) = SA5*(SA0*SA6 - SA8*q0); + H_YAW(1) = SA5*(SA1*SA6 - SA8*q1); + H_YAW(2) = SA5*(SA1*SA7 + SA9*q1); + H_YAW(3) = SA5*(SA0*SA7 + SA9*q0); + + } else { + // calculate 312 yaw observation matrix + H_YAW(0) = SA5*(SA0*SA6 - SA8*q0); + H_YAW(1) = SA5*(-SA1*SA6 + SA8*q1); + H_YAW(2) = SA5*(-SA1*SA7 - SA9*q1); + H_YAW(3) = SA5*(SA0*SA7 + SA9*q0); + } + } else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) { - const float SB5 = 1.0F/SB5_inv; - const float SB6 = 1.0F/SB2; + const float SB5 = 1.f/SB5_inv; + const float SB6 = 1.f/SB2; const float SB7 = SB3*SB4; const float SB8 = 2*SB7; const float SB9 = 2*SB6; - H_YAW(0) = -SB5*(-SB0*SB6 + SB8*q3); - H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2); - H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2); - H_YAW(3) = -SB5*(SB0*SB7 + SB9*q3); + if (fuse_yaw_321) { + // calculate 321 yaw observation matrix + H_YAW(0) = -SB5*(SB0*SB6 - SB8*q3); + H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2); + H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2); + H_YAW(3) = -SB5*(-SB0*SB7 - SB9*q3); + + } else { + // calculate 312 yaw observation matrix + H_YAW(0) = -SB5*(-SB0*SB6 + SB8*q3); + H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2); + H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2); + H_YAW(3) = -SB5*(SB0*SB7 + SB9*q3); + } + } else { return false; } - float innovation; - - if (zero_innovation) { - innovation = 0.0f; - - } else { - // calculate the the innovation and wrap to the interval between +-pi - innovation = wrap_pi(atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)) - measurement); - } - - // define the innovation gate size - float innov_gate = math::max(_params.heading_innov_gate, 1.0f); - - // Update the quaternion states and covariance matrix - return updateQuaternion(innovation, R_YAW, innov_gate, H_YAW); -} - -// update quaternion states and covariances using the yaw innovation, yaw observation variance and yaw Jacobian -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 // calculate the innovation variance _heading_innov_var = variance; @@ -613,10 +557,10 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f float tmp = 0.0f; for (uint8_t col = 0; col <= 3; col++) { - tmp += P(row, col) * yaw_jacobian(col); + tmp += P(row, col) * H_YAW(col); } - _heading_innov_var += yaw_jacobian(row) * tmp; + _heading_innov_var += H_YAW(row) * tmp; } float heading_innov_var_inv; @@ -633,7 +577,7 @@ bool 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"); + ECL_ERR("yaw fusion numerical error - covariance reset"); return false; } @@ -643,7 +587,7 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f for (uint8_t row = 0; row <= 15; row++) { for (uint8_t col = 0; col <= 3; col++) { - Kfusion(row) += P(row, col) * yaw_jacobian(col); + Kfusion(row) += P(row, col) * H_YAW(col); } Kfusion(row) *= heading_innov_var_inv; @@ -652,13 +596,16 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f if (_control_status.flags.wind) { for (uint8_t row = 22; row <= 23; row++) { for (uint8_t col = 0; col <= 3; col++) { - Kfusion(row) += P(row, col) * yaw_jacobian(col); + Kfusion(row) += P(row, col) * H_YAW(col); } Kfusion(row) *= heading_innov_var_inv; } } + // define the innovation gate size + 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); @@ -669,7 +616,10 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f // if we are in air we don't want to fuse the measurement // we allow to use it when on the ground because the large innovation could be caused // by interference or a large initial gyro bias - if (!_control_status.flags.in_air && isTimedOut(_time_last_in_air, (uint64_t)5e6)) { + if (!_control_status.flags.in_air + && isTimedOut(_time_last_in_air, (uint64_t)5e6) + && isTimedOut(_time_last_heading_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 @@ -697,10 +647,10 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f for (unsigned row = 0; row < _k_num_states; row++) { - KH[0] = Kfusion(row) * yaw_jacobian(0); - KH[1] = Kfusion(row) * yaw_jacobian(1); - KH[2] = Kfusion(row) * yaw_jacobian(2); - KH[3] = Kfusion(row) * yaw_jacobian(3); + KH[0] = Kfusion(row) * H_YAW(0); + KH[1] = Kfusion(row) * H_YAW(1); + KH[2] = Kfusion(row) * H_YAW(2); + KH[3] = Kfusion(row) * H_YAW(3); for (unsigned column = 0; column < _k_num_states; column++) { float tmp = KH[0] * P(0, column); @@ -732,61 +682,6 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f return false; } -bool Ekf::fuseHeading(float measured_hdg, float obs_var) -{ - // observation variance - float R_YAW = PX4_ISFINITE(obs_var) ? obs_var : 0.01f; - - // update transformation matrix from body to world frame using the current state estimate - const float predicted_hdg = getEulerYaw(_R_to_earth); - - if (!PX4_ISFINITE(measured_hdg)) { - measured_hdg = predicted_hdg; - } - - // handle special case where yaw measurement is unavailable - bool fuse_zero_innov = false; - - if (_is_yaw_fusion_inhibited) { - // The yaw measurement cannot be trusted but we need to fuse something to prevent a badly - // conditioned covariance matrix developing over time. - if (!_control_status.flags.vehicle_at_rest) { - // Vehicle is not at rest so fuse a zero innovation if necessary to prevent - // unconstrained quaternion variance growth and record the predicted heading - // to use as an observation when movement ceases. - // TODO a better way of determining when this is necessary - const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3); - - if (sumQuatVar > _params.quat_max_variance) { - fuse_zero_innov = true; - R_YAW = 0.25f; - } - - _last_static_yaw = predicted_hdg; - - } else { - // Vehicle is at rest so use the last moving prediction as an observation - // to prevent the heading from drifting and to enable yaw gyro bias learning - // before takeoff. - if (!PX4_ISFINITE(_last_static_yaw)) { - _last_static_yaw = predicted_hdg; - } - - measured_hdg = _last_static_yaw; - } - - } else { - _last_static_yaw = predicted_hdg; - } - - if (shouldUse321RotationSequence(_R_to_earth)) { - return fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov); - - } else { - return fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov); - } -} - bool Ekf::fuseDeclination(float decl_sigma) { // assign intermediate state variables diff --git a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp new file mode 100644 index 0000000000..57e413ec2a --- /dev/null +++ b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * 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 zero_innovation_heading_update.cpp + * Control function for ekf heading update when at rest or no other heading source available + */ + +#include "ekf.h" + +void Ekf::controlZeroInnovationHeadingUpdate() +{ + // When at rest or no source of yaw aiding is active yaw fusion is run selectively to enable yaw gyro + // bias learning when stationary on ground and to prevent uncontrolled yaw variance growth + if (_control_status.flags.vehicle_at_rest && _control_status.flags.tilt_align) { + + const float euler_yaw = wrap_pi(getEulerYaw(_R_to_earth)); + + // record static yaw when transitioning to at rest + if (!PX4_ISFINITE(_last_static_yaw)) { + _last_static_yaw = euler_yaw; + } + + // fuse last static yaw at a limited rate (every 200 milliseconds) + if (isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) { + float innovation = euler_yaw - _last_static_yaw; + float obs_var = 0.001f; + updateQuaternion(innovation, obs_var); + } + + } else { + // vehicle moving or tilt alignment not yet completed + + const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3); + + if (sumQuatVar > _params.quat_max_variance) { + // if necessary fuse zero innovation to prevent unconstrained quaternion variance growth + float innovation = 0.f; + float obs_var = 0.25f; + updateQuaternion(innovation, obs_var); + + } else if (!_control_status.flags.tilt_align) { + // fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low + float innovation = 0.f; + float obs_var = 0.01f; + updateQuaternion(innovation, obs_var); + } + + _last_static_yaw = wrap_pi(getEulerYaw(_R_to_earth)); + } +}