diff --git a/EKF/control.cpp b/EKF/control.cpp index 6a53301c5f..da72eb8eb6 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -112,12 +112,12 @@ void Ekf::controlFusionModes() } else { if (_control_status.flags.in_air) { - // if transitioning from a non-3D fusion mode, we need to initialise the yaw angle and field states + // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states if (!_control_status.flags.mag_3D) { _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } - // always use 3D mag fusion when airborne + // use 3D mag fusion when airborne _control_status.flags.mag_hdg = false; _control_status.flags.mag_2D = false; _control_status.flags.mag_3D = true; @@ -143,7 +143,7 @@ void Ekf::controlFusionModes() _control_status.flags.mag_3D = false; } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) { - // if transitioning from a non-3D fusion mode, we need to initialise the yaw angle and field states + // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states if (!_control_status.flags.mag_3D) { _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 578dd20669..5441c8a082 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -115,11 +115,11 @@ bool Ekf::init(uint64_t timestamp) _output_new.vel.setZero(); _output_new.pos.setZero(); _output_new.quat_nominal = matrix::Quaternion(); - + _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); _vel_corr.setZero(); - + _imu_down_sampled.delta_ang.setZero(); _imu_down_sampled.delta_vel.setZero(); _imu_down_sampled.delta_ang_dt = 0.0f; @@ -180,6 +180,7 @@ bool Ekf::update() fuseMag2D(); } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) { + // fusion of a Euler yaw angle from either a 321 or 312 rotation sequence fuseHeading(); } else { diff --git a/EKF/ekf.h b/EKF/ekf.h index f11682878d..38b0cfe716 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -187,7 +187,7 @@ private: // ekf sequential fusion of magnetometer measurements void fuseMag(); - // fuse heading measurement (currently uses estimate from magnetometer) + // fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer) void fuseHeading(); // fuse projecton of magnetometer onto horizontal plane