fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 01:54:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Clean euler321 and euler312 code and comment

Browse Source
This commit is contained in:
kamilritz 2020-08-17 19:39:39 +02:00 committed by Paul Riseborough
parent 2be738e806
commit 4fb4e0ca01
4 changed files with 19 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
EKF/EKFGSF_yaw.cpp
View File

@ -224,17 +224,12 @@ void EKFGSF_yaw::ahrsAlignYaw()
{
// Align yaw angle for each model
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
if (shouldUse321RotationSequence(_ahrs_ekf_gsf[model_index].R)) {
// update the rotation matrix with 321 rotation sequence
_ahrs_ekf_gsf[model_index].R = updateEuler321YawInRotMat(wrap_pi(_ekf_gsf[model_index].X(2)),
_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));
R = shouldUse321RotationSequence(R) ?
updateEuler321YawInRotMat(yaw, R) :
updateEuler312YawInRotMat(yaw, R);
} else {
// update the rotation matrix with 312 rotation sequence
_ahrs_ekf_gsf[model_index].R = updateEuler312YawInRotMat(wrap_pi(_ekf_gsf[model_index].X(2)),
_ahrs_ekf_gsf[model_index].R);
}
_ahrs_ekf_gsf[model_index].aligned = true;
}
}
@ -250,11 +245,10 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
}
// Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
if (shouldUse321RotationSequence(_ahrs_ekf_gsf[model_index].R)) {
_ekf_gsf[model_index].X(2) = getEuler321Yaw(_ahrs_ekf_gsf[model_index].R);
} else {
_ekf_gsf[model_index].X(2) = getEuler312Yaw(_ahrs_ekf_gsf[model_index].R);
}
const Dcmf& R = _ahrs_ekf_gsf[model_index].R;
_ekf_gsf[model_index].X(2) = shouldUse321RotationSequence(R) ?
getEuler321Yaw(R) :
getEuler312Yaw(R);
// calculate delta velocity in a horizontal front-right frame
const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel;

26
EKF/ekf_helper.cpp
View File

@ -481,16 +481,9 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Dcmf R_to_earth;
if (shouldUse321RotationSequence(_R_to_earth)) {
// rolled more than pitched so use 321 rotation order
R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth);
} else {
// pitched more than rolled so use 312 rotation order
R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth);
}
const Dcmf R_to_earth = shouldUse321RotationSequence(_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
const Vector3f mag_earth_pred = R_to_earth * mag_init;
@ -1647,16 +1640,9 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
_R_to_earth = Dcmf(_state.quat_nominal);
// update the rotation matrix using the new yaw value
// determine if a 321 or 312 Euler sequence is best
if (shouldUse321RotationSequence(_R_to_earth)) {
_R_to_earth = updateEuler321YawInRotMat(yaw, _R_to_earth);
} else {
// We use a 312 sequence as an alternate when there is more pitch tilt than roll tilt
// to avoid gimbal lock
_R_to_earth = updateEuler312YawInRotMat(yaw, _R_to_earth);
}
_R_to_earth = shouldUse321RotationSequence(_R_to_earth) ?
updateEuler321YawInRotMat(yaw, _R_to_earth) :
updateEuler312YawInRotMat(yaw, _R_to_earth);
// calculate the amount that the quaternion has changed by
const Quatf quat_after_reset(_R_to_earth);

8
EKF/mag_fusion.cpp
View File

@ -721,9 +721,7 @@ void Ekf::fuseHeading()
// update transformation matrix from body to world frame using the current state estimate
_R_to_earth = Dcmf(_state.quat_nominal);
// determine if a 321 or 312 Euler sequence is best
if (shouldUse321RotationSequence(_R_to_earth)) {
// rolled more than pitched so use 321 rotation order to calculate the observed yaw angle
const float predicted_hdg = getEuler321Yaw(_R_to_earth);
if (_control_status.flags.mag_hdg) {
@ -741,7 +739,6 @@ void Ekf::fuseHeading()
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_control_status.flags.ev_yaw) {
// calculate the yaw angle for a 321 sequence
measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat);
} else {
@ -781,16 +778,13 @@ void Ekf::fuseHeading()
fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov);
} else {
// pitched more than rolled so use 312 rotation order to calculate the observed yaw angle
const float predicted_hdg = getEuler312Yaw(_R_to_earth);
if (_control_status.flags.mag_hdg) {
// Calculate the body to earth frame rotation matrix from the euler angles
// using a 312 rotation sequence with yaw angle set to to zero
// rotate the magnetometer measurements into earth frame using a zero yaw angle
const Dcmf R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth);
// rotate the magnetometer measurements into earth frame using a zero yaw angle
if (_control_status.flags.mag_3D) {
// don't apply bias corrections if we are learning them
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;

3
EKF/utils.hpp
View File

@ -23,6 +23,9 @@ float kahanSummation(float sum_previous, float input, float &accumulator);
// this produces the inverse rotation to that produced by the math library quaternion to Dcmf operator
matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat);
// We should use a 3-2-1 Tait-Bryan (yaw-pitch-roll) rotation sequence
// when there is more roll than pitch tilt and a 3-1-2 rotation sequence
// when there is more pitch than roll tilt to avoid gimbal lock.
bool shouldUse321RotationSequence(const matrix::Dcmf& R);
float getEuler321Yaw(const matrix::Quatf& q);