Browse Source

updateYawInRotMat with hidden rotation sequence handling

master
kamilritz 5 years ago committed by Paul Riseborough
parent
commit
9797e4d28f
  1. 4
      EKF/EKFGSF_yaw.cpp
  2. 8
      EKF/ekf_helper.cpp
  3. 6
      EKF/utils.cpp
  4. 3
      EKF/utils.hpp

4
EKF/EKFGSF_yaw.cpp

@ -226,9 +226,7 @@ void EKFGSF_yaw::ahrsAlignYaw()
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) { for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
Dcmf& R = _ahrs_ekf_gsf[model_index].R; Dcmf& R = _ahrs_ekf_gsf[model_index].R;
const float yaw = wrap_pi(_ekf_gsf[model_index].X(2)); const float yaw = wrap_pi(_ekf_gsf[model_index].X(2));
R = shouldUse321RotationSequence(R) ? R = updateYawInRotMat(yaw, R);
updateEuler321YawInRotMat(yaw, R) :
updateEuler312YawInRotMat(yaw, R);
_ahrs_ekf_gsf[model_index].aligned = true; _ahrs_ekf_gsf[model_index].aligned = true;
} }

8
EKF/ekf_helper.cpp

@ -481,9 +481,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle // rotate the magnetometer measurements into earth frame using a zero yaw angle
const Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
updateEuler321YawInRotMat(0.f, _R_to_earth) :
updateEuler312YawInRotMat(0.f, _R_to_earth);
// the angle of the projection onto the horizontal gives the yaw angle // the angle of the projection onto the horizontal gives the yaw angle
const Vector3f mag_earth_pred = R_to_earth * mag_init; const Vector3f mag_earth_pred = R_to_earth * mag_init;
@ -1640,9 +1638,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
_R_to_earth = Dcmf(_state.quat_nominal); _R_to_earth = Dcmf(_state.quat_nominal);
// update the rotation matrix using the new yaw value // update the rotation matrix using the new yaw value
_R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? _R_to_earth = updateYawInRotMat(yaw, _R_to_earth);
updateEuler321YawInRotMat(yaw, _R_to_earth) :
updateEuler312YawInRotMat(yaw, _R_to_earth);
// calculate the amount that the quaternion has changed by // calculate the amount that the quaternion has changed by
const Quatf quat_after_reset(_R_to_earth); const Quatf quat_after_reset(_R_to_earth);

6
EKF/utils.cpp

@ -99,3 +99,9 @@ matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf& rot_in) {
atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch
return taitBryan312ToRotMat(rotVec312); return taitBryan312ToRotMat(rotVec312);
} }
matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf& rot_in) {
return shouldUse321RotationSequence(rot_in) ?
updateEuler321YawInRotMat(yaw, rot_in) :
updateEuler312YawInRotMat(yaw, rot_in);
}

3
EKF/utils.hpp

@ -37,6 +37,9 @@ float getEuler312Yaw(const matrix::Dcmf& R);
matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf& rot_in); matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf& rot_in);
matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf& rot_in); matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf& rot_in);
// Checks which euler rotation sequence to use and update yaw in rotation matrix
matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf& rot_in);
namespace ecl{ namespace ecl{
inline float powf(float x, int exp) inline float powf(float x, int exp)
{ {

Loading…
Cancel
Save