diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index dbf3d05989..927a83e24f 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -239,13 +239,13 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) && PX4_ISFINITE(_wmm_declination_rad) ) { // using declination from the world magnetic model - fuseDeclination(_wmm_declination_rad, 0.5f, update_all_states); + fuseDeclination(_wmm_declination_rad, 0.5f, update_all_states, update_tilt); } else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f) ) { // using previously saved declination - fuseDeclination(math::radians(_params.mag_declination_deg), R_DECL, update_all_states); + fuseDeclination(math::radians(_params.mag_declination_deg), R_DECL, update_all_states, update_tilt); } else { // if there is no aiding coming from an inertial frame we need to fuse some declination diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp index a9d07853a1..88fa425939 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp @@ -140,7 +140,7 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima return true; } -bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_states) +bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_states, bool update_tilt) { VectorState H; float decl_pred; @@ -160,7 +160,13 @@ bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_s // Calculate the Kalman gains VectorState Kfusion = P * H / innovation_variance; - if (!update_all_states) { + if (update_all_states) { + if (!update_tilt) { + Kfusion(State::quat_nominal.idx + 0) = 0.f; + Kfusion(State::quat_nominal.idx + 1) = 0.f; + } + + } else { // zero non-mag Kalman gains if not updating all states // copy mag_I and mag_B Kalman gains diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e0dba0eddc..1dad06d304 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -668,7 +668,8 @@ private: // fuse magnetometer declination measurement // R: declination observation variance (rad**2) - bool fuseDeclination(const float decl_measurement_rad, const float R, bool update_all_states = false); + bool fuseDeclination(const float decl_measurement_rad, const float R, bool update_all_states = false, + bool update_tilt = false); #endif // CONFIG_EKF2_MAGNETOMETER