forked from Archive/PX4-Autopilot
Add helper function to alter heading in rotation matrices
This commit is contained in:
parent
fdc86c247a
commit
2be738e806
|
@ -225,24 +225,14 @@ 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)) {
|
||||
// get the roll, pitch, yaw estimates from the rotation matrix using a 321 Tait-Bryan rotation sequence
|
||||
Eulerf euler_init(_ahrs_ekf_gsf[model_index].R);
|
||||
|
||||
// set the yaw angle
|
||||
euler_init(2) = wrap_pi(_ekf_gsf[model_index].X(2));
|
||||
|
||||
// update the rotation matrix
|
||||
_ahrs_ekf_gsf[model_index].R = Dcmf(euler_init);
|
||||
// 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);
|
||||
|
||||
} else {
|
||||
// Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
|
||||
const Vector3f rot312(wrap_pi(_ekf_gsf[model_index].X(2)), // yaw
|
||||
asinf(_ahrs_ekf_gsf[model_index].R(2, 1)), // roll
|
||||
atan2f(-_ahrs_ekf_gsf[model_index].R(2, 0),
|
||||
_ahrs_ekf_gsf[model_index].R(2, 2))); // pitch
|
||||
|
||||
// Calculate the body to earth frame rotation matrix
|
||||
_ahrs_ekf_gsf[model_index].R = taitBryan312ToRotMat(rot312);
|
||||
// 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;
|
||||
|
|
|
@ -484,16 +484,11 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
|
|||
Dcmf R_to_earth;
|
||||
if (shouldUse321RotationSequence(_R_to_earth)) {
|
||||
// rolled more than pitched so use 321 rotation order
|
||||
Eulerf euler321(_state.quat_nominal);
|
||||
euler321(2) = 0.0f;
|
||||
R_to_earth = Dcmf(euler321);
|
||||
R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth);
|
||||
|
||||
} else {
|
||||
// pitched more than rolled so use 312 rotation order
|
||||
const Vector3f rotVec312(0.0f, // yaw
|
||||
asinf(_R_to_earth(2, 1)), // roll
|
||||
atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2))); // pitch
|
||||
R_to_earth = taitBryan312ToRotMat(rotVec312);
|
||||
R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth);
|
||||
|
||||
}
|
||||
|
||||
|
@ -1654,19 +1649,12 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
|
|||
// update the rotation matrix using the new yaw value
|
||||
// determine if a 321 or 312 Euler sequence is best
|
||||
if (shouldUse321RotationSequence(_R_to_earth)) {
|
||||
// use a 321 sequence
|
||||
Eulerf euler321(_R_to_earth);
|
||||
euler321(2) = yaw;
|
||||
_R_to_earth = Dcmf(euler321);
|
||||
_R_to_earth = updateEuler321YawInRotMat(yaw, _R_to_earth);
|
||||
|
||||
} else {
|
||||
// Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
|
||||
// We use a 312 sequence as an alternate when there is more pitch tilt than roll tilt
|
||||
// to avoid gimbal lock
|
||||
const Vector3f rot312(yaw,
|
||||
asinf(_R_to_earth(2, 1)),
|
||||
atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)));
|
||||
_R_to_earth = taitBryan312ToRotMat(rot312);
|
||||
_R_to_earth = updateEuler312YawInRotMat(yaw, _R_to_earth);
|
||||
|
||||
}
|
||||
|
||||
|
|
|
@ -702,7 +702,6 @@ void Ekf::fuseHeading()
|
|||
{
|
||||
Vector3f mag_earth_pred;
|
||||
float measured_hdg;
|
||||
float predicted_hdg;
|
||||
|
||||
// Calculate the observation variance
|
||||
float R_YAW;
|
||||
|
@ -725,12 +724,11 @@ void Ekf::fuseHeading()
|
|||
// 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
|
||||
Eulerf euler321(_state.quat_nominal);
|
||||
predicted_hdg = euler321(2);
|
||||
const float predicted_hdg = getEuler321Yaw(_R_to_earth);
|
||||
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
// Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle
|
||||
euler321(2) = 0.0f;
|
||||
const Dcmf R_to_earth(euler321);
|
||||
// Rotate the measurements into earth frame using the zero yaw angle
|
||||
const Dcmf R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth);
|
||||
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;
|
||||
|
@ -788,12 +786,9 @@ void Ekf::fuseHeading()
|
|||
|
||||
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
|
||||
const Vector3f rotVec312(0.0f, // yaw
|
||||
asinf(_R_to_earth(2, 1)), // roll
|
||||
atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2))); // pitch
|
||||
const Dcmf R_to_earth = taitBryan312ToRotMat(rotVec312);
|
||||
// Calculate the body to earth frame rotation matrix from the euler angles
|
||||
// using a 312 rotation sequence with yaw angle set to to zero
|
||||
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) {
|
||||
|
|
|
@ -86,3 +86,16 @@ float getEuler312Yaw(const matrix::Quatf& q) {
|
|||
float getEuler312Yaw(const matrix::Dcmf& R) {
|
||||
return atan2f(-R(0, 1), R(1, 1));
|
||||
}
|
||||
|
||||
matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf& rot_in) {
|
||||
matrix::Eulerf euler321(rot_in);
|
||||
euler321(2) = yaw;
|
||||
return matrix::Dcmf(euler321);
|
||||
}
|
||||
|
||||
matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf& rot_in) {
|
||||
const matrix::Vector3f rotVec312(yaw, // yaw
|
||||
asinf(rot_in(2, 1)), // roll
|
||||
atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch
|
||||
return taitBryan312ToRotMat(rotVec312);
|
||||
}
|
||||
|
|
|
@ -31,6 +31,9 @@ float getEuler321Yaw(const matrix::Dcmf& R);
|
|||
float getEuler312Yaw(const matrix::Quatf& q);
|
||||
float getEuler312Yaw(const matrix::Dcmf& R);
|
||||
|
||||
matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf& rot_in);
|
||||
matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf& rot_in);
|
||||
|
||||
namespace ecl{
|
||||
inline float powf(float x, int exp)
|
||||
{
|
||||
|
|
Loading…
Reference in New Issue