diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 7ed5ca39d7..1f2d41c689 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -366,9 +366,9 @@ void Ekf::controlExternalVisionFusion() 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)) - wrap_pi(measured_hdg); + float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg); float obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f); - updateQuaternion(innovation, obs_var); + fuseYaw(innovation, obs_var); } // record observation and estimate for use next time diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index a0bd4935fb..4075058fc9 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -200,7 +200,7 @@ bool Ekf::initialiseFilter() // rotate the magnetometer measurements into earth frame using a zero yaw angle // the angle of the projection onto the horizontal gives the yaw angle const Vector3f mag_earth_pred = updateYawInRotMat(0.f, _R_to_earth) * _mag_lpf.getState(); - float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); + float yaw_new = wrap_pi(-atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination()); // update quaternion states and corresponding covarainces resetQuatStateYaw(yaw_new, 0.f, false); @@ -208,6 +208,7 @@ bool Ekf::initialiseFilter() // set the earth magnetic field states using the updated rotation _state.mag_I = _R_to_earth * _mag_lpf.getState(); _state.mag_B.zero(); + } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) { float yaw_new = wrap_pi(math::radians(_params.heading_init_deg)); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index d6015507dc..7156bcad48 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -590,7 +590,7 @@ private: // update quaternion states and covariances using a yaw innovation and yaw observation variance // innovation : prediction - measurement // variance : observaton variance - bool updateQuaternion(const float innovation, const float variance); + bool fuseYaw(const float innovation, const float variance); // fuse the yaw angle obtained from a dual antenna GPS unit void fuseGpsYaw(); diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index b8ebb06119..366838f282 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -327,14 +327,14 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag) // 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(); + float measured_hdg = wrap_pi(-atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination()); - float innovation = wrap_pi(getEulerYaw(_R_to_earth)) - wrap_pi(measured_hdg); + float innovation = wrap_pi(getEulerYaw(_R_to_earth) - 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)) { + if (fuseYaw(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 b4ef11a9b7..605a2a35e7 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -434,7 +434,7 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states) } // update quaternion states and covariances using the yaw innovation and yaw observation variance -bool Ekf::updateQuaternion(const float innovation, const float variance) +bool Ekf::fuseYaw(const float innovation, const float variance) { // assign intermediate state variables const float q0 = _state.quat_nominal(0); diff --git a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp index 57e413ec2a..dd3ce2f7c0 100644 --- a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp +++ b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp @@ -44,18 +44,18 @@ void Ekf::controlZeroInnovationHeadingUpdate() // 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)); + const float euler_yaw = 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; + // fuse last static yaw at a limited rate (every 1000 milliseconds) + if (isTimedOut(_time_last_heading_fuse, (uint64_t)1'000'000)) { + float innovation = wrap_pi(euler_yaw - _last_static_yaw); float obs_var = 0.001f; - updateQuaternion(innovation, obs_var); + fuseYaw(innovation, obs_var); } } else { @@ -67,15 +67,15 @@ void Ekf::controlZeroInnovationHeadingUpdate() // if necessary fuse zero innovation to prevent unconstrained quaternion variance growth float innovation = 0.f; float obs_var = 0.25f; - updateQuaternion(innovation, obs_var); + fuseYaw(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); + fuseYaw(innovation, obs_var); } - _last_static_yaw = wrap_pi(getEulerYaw(_R_to_earth)); + _last_static_yaw = getEulerYaw(_R_to_earth); } }