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

add functions to compute yaw (321 and 312 sequence)

Browse Source 
from quaternion and rotation matrix
This commit is contained in:
Kamil Ritz 2020-08-15 13:52:56 +02:00 committed by Paul Riseborough
parent 15afa8ae17
commit fdc86c247a
9 changed files with 60 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/EKFGSF_yaw.cpp
View File

@ -261,11 +261,9 @@ 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)) {
// use 321 Tait-Bryan rotation to define yaw state
_ekf_gsf[model_index].X(2) = atan2f(_ahrs_ekf_gsf[model_index].R(1, 0), _ahrs_ekf_gsf[model_index].R(0, 0));
_ekf_gsf[model_index].X(2) = getEuler321Yaw(_ahrs_ekf_gsf[model_index].R);
} else {
// use 312 Tait-Bryan rotation to define yaw state
_ekf_gsf[model_index].X(2) = atan2f(-_ahrs_ekf_gsf[model_index].R(0, 1), _ahrs_ekf_gsf[model_index].R(1, 1)); // first rotation (yaw)
_ekf_gsf[model_index].X(2) = getEuler312Yaw(_ahrs_ekf_gsf[model_index].R);
}
// calculate delta velocity in a horizontal front-right frame

3
EKF/airspeed_fusion.cpp
View File

@ -182,8 +182,7 @@ void Ekf::get_true_airspeed(float *tas)
*/
void Ekf::resetWindStates()
{
const Eulerf euler321(_state.quat_nominal);
const float euler_yaw = euler321(2);
const float euler_yaw = getEuler321Yaw(_state.quat_nominal);
if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available

3
EKF/covariance.cpp
View File

@ -1096,8 +1096,7 @@ void Ekf::resetWindCovariance()
if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
// Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py
// TODO: explicitly include the sideslip angle in the derivation
Eulerf euler321(_state.quat_nominal);
const float euler_yaw = euler321(2);
const float euler_yaw = getEuler321Yaw(_state.quat_nominal);
const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
constexpr float initial_sideslip_uncertainty = math::radians(15.0f);
const float initial_wind_var_body_y = sq(_airspeed_sample_delayed.true_airspeed * sinf(initial_sideslip_uncertainty));

3
EKF/ekf.h
View File

@ -816,9 +816,6 @@ private:
void checkRangeAidSuitability();
bool isRangeAidSuitable() { return _is_range_aid_suitable; }
// return the square of two floating point numbers - used in auto coded sections
static constexpr float sq(float var) { return var * var; }
// set control flags to use baro height
void setControlBaroHeight();

13
EKF/ekf_helper.cpp
View File

@ -402,8 +402,8 @@ bool Ekf::realignYawGPS()
if (!_control_status.flags.mag_aligned_in_flight) {
// This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
const Eulerf euler321(_state.quat_nominal);
yaw_new = euler321(2) + courseYawError;
const float current_yaw = getEuler321Yaw(_state.quat_nominal);
yaw_new = current_yaw + courseYawError;
_control_status.flags.mag_aligned_in_flight = true;
} else if (_control_status.flags.wind) {
@ -473,11 +473,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
float yaw_new;
float yaw_new_variance = 0.0f;
if (_control_status.flags.ev_yaw) {
// convert the observed quaternion to a rotation matrix
const Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// calculate the yaw angle for a 312 sequence
yaw_new = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
yaw_new = getEuler312Yaw(_ev_sample_delayed.quat);
if (increase_yaw_var) {
yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
@ -1586,8 +1582,7 @@ void Ekf::startEvVelFusion() {
void Ekf::startEvYawFusion() {
// reset the yaw angle to the value from the vision quaternion
const Eulerf euler_obs(_ev_sample_delayed.quat);
const float yaw = euler_obs(2);
const float yaw = getEuler321Yaw(_ev_sample_delayed.quat);
const float yaw_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
resetQuatStateYaw(yaw, yaw_variance, true);

15
EKF/mag_fusion.cpp
View File

@ -744,10 +744,7 @@ void Ekf::fuseHeading()
} else if (_control_status.flags.ev_yaw) {
// calculate the yaw angle for a 321 sequence
// Expressions obtained from yaw_input_321.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m
const float Tbn_1_0 = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)+_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2));
const float Tbn_0_0 = sq(_ev_sample_delayed.quat(0))+sq(_ev_sample_delayed.quat(1))-sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3));
measured_hdg = atan2f(Tbn_1_0,Tbn_0_0);
measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat);
} else {
measured_hdg = predicted_hdg;
@ -786,9 +783,9 @@ 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
predicted_hdg = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1));
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
@ -810,11 +807,7 @@ 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 312 sequence
// Values from yaw_input_312.c file produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
const float Tbn_0_1_neg = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)-_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2));
const float Tbn_1_1 = sq(_ev_sample_delayed.quat(0))-sq(_ev_sample_delayed.quat(1))+sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3));
measured_hdg = atan2f(Tbn_0_1_neg,Tbn_1_1);
measured_hdg = getEuler312Yaw(_ev_sample_delayed.quat);
} else {
measured_hdg = predicted_hdg;

26
EKF/utils.cpp
View File

@ -61,4 +61,28 @@ matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
bool shouldUse321RotationSequence(const matrix::Dcmf& R) {
return fabsf(R(2, 0)) < fabsf(R(2, 1));
}
}
float getEuler321Yaw(const matrix::Quatf& q) {
// Values from yaw_input_321.c file produced by
// https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m
const float a = 2.f * (q(0) * q(3) + q(1) * q(2));
const float b = sq(q(0)) + sq(q(1)) - sq(q(2)) - sq(q(3));
return atan2f(a, b);
}
float getEuler321Yaw(const matrix::Dcmf& R) {
return atan2f(R(1, 0), R(0, 0));
}
float getEuler312Yaw(const matrix::Quatf& q) {
// Values from yaw_input_312.c file produced by
// https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
const float a = 2.f * (q(0) * q(3) - q(1) * q(2));
const float b = sq(q(0)) - sq(q(1)) + sq(q(2)) - sq(q(3));
return atan2f(a, b);
}
float getEuler312Yaw(const matrix::Dcmf& R) {
return atan2f(-R(0, 1), R(1, 1));
}

10
EKF/utils.hpp
View File

@ -2,6 +2,9 @@
#pragma once
// return the square of two floating point numbers - used in auto coded sections
static constexpr float sq(float var) { return var * var; }
// converts Tait-Bryan 312 sequence of rotations from frame 1 to frame 2
// to the corresponding rotation matrix that rotates from frame 2 to frame 1
// rot312(0) - First rotation is a RH rotation about the Z axis (rad)
@ -21,6 +24,13 @@ float kahanSummation(float sum_previous, float input, float &accumulator);
matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat);
bool shouldUse321RotationSequence(const matrix::Dcmf& R);
float getEuler321Yaw(const matrix::Quatf& q);
float getEuler321Yaw(const matrix::Dcmf& R);
float getEuler312Yaw(const matrix::Quatf& q);
float getEuler312Yaw(const matrix::Dcmf& R);
namespace ecl{
inline float powf(float x, int exp)
{

13
test/test_EKF_utils.cpp
View File

@ -55,3 +55,16 @@ TEST(eclPowfTest, compareToStandardImplementation)
}
}
}
TEST(euler312YawTest, fromQuaternion)
{
matrix::Quatf q1(3.5f,2.4f,-0.5f,-3.f);
q1.normalize();
const matrix::Eulerf euler1(q1);
EXPECT_FLOAT_EQ(euler1(2), getEuler321Yaw(q1));
matrix::Quatf q2(0.f,0,-1.f,0.f);
q2.normalize();
const matrix::Eulerf euler2(q2);
EXPECT_FLOAT_EQ(euler2(2), getEuler321Yaw(q2));
}