fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 03:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Quaternion: rename function to rotate vectors

Browse Source
This commit is contained in:
Matthias Grob
2022-03-07 20:03:54 -05:00
committed by Daniel Agar
parent 5affa693f2
commit 68a0414622
7 changed files with 27 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/matrix/matrix/Quaternion.hpp
+2 -2
View File
@@ -485,7 +485,7 @@ public:
* @param vec vector to rotate in frame 1 (typically body frame)
* @return rotated vector in frame 2 (typically reference frame)
*/
Vector3<Type> conjugate(const Vector3<Type> &vec) const
Vector3<Type> rotateVector(const Vector3<Type> &vec) const
{
const Quaternion &q = *this;
Quaternion v(Type(0), vec(0), vec(1), vec(2));
@@ -502,7 +502,7 @@ public:
* @param vec vector to rotate in frame 2 (typically reference frame)
* @return rotated vector in frame 1 (typically body frame)
*/
Vector3<Type> conjugate_inversed(const Vector3<Type> &vec) const
Vector3<Type> rotateVectorInverse(const Vector3<Type> &vec) const
{
const Quaternion &q = *this;
Quaternion v(Type(0), vec(0), vec(1), vec(2));
src/lib/matrix/test/attitude.cpp
+8 -8
View File
@@ -57,27 +57,27 @@ int main()
// quaternion ctor: vector to vector
// identity test
Quatf quat_v(v, v);
TEST(isEqual(quat_v.conjugate(v), v));
TEST(isEqual(quat_v.rotateVector(v), v));
// random test (vector norm can not be preserved with a pure rotation)
Vector3f v1(-80.1f, 1.5f, -6.89f);
quat_v = Quatf(v1, v);
TEST(isEqual(quat_v.conjugate(v1).normalized() * v.norm(), v));
TEST(isEqual(quat_v.rotateVector(v1).normalized() * v.norm(), v));
// special 180 degree case 1
v1 = Vector3f(0.f, 1.f, 1.f);
quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.conjugate(v1), -v1));
TEST(isEqual(quat_v.rotateVector(v1), -v1));
// special 180 degree case 2
v1 = Vector3f(1.f, 2.f, 0.f);
quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.conjugate(v1), -v1));
TEST(isEqual(quat_v.rotateVector(v1), -v1));
// special 180 degree case 3
v1 = Vector3f(0.f, 0.f, 1.f);
quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.conjugate(v1), -v1));
TEST(isEqual(quat_v.rotateVector(v1), -v1));
// special 180 degree case 4
v1 = Vector3f(1.f, 1.f, 1.f);
quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.conjugate(v1), -v1));
TEST(isEqual(quat_v.rotateVector(v1), -v1));
// quat normalization
q.normalize();
@@ -447,8 +447,8 @@ int main()
// conjugate
v = Vector3f(1.5f, 2.2f, 3.2f);
TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q).T()*v1));
TEST(isEqual(q.conjugate(v1), Dcmf(q)*v1));
TEST(isEqual(q.rotateVectorInverse(v1), Dcmf(q).T()*v1));
TEST(isEqual(q.rotateVector(v1), Dcmf(q)*v1));
AxisAnglef aa_q_init(q);
TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f)));
src/lib/matrix/test/dual.cpp
+1 -1
View File
@@ -28,7 +28,7 @@ Vector<Scalar, 3> positionError(const Vector<Scalar, 3> &positionState,
Scalar dt
)
{
return positionMeasurement - (positionState + bodyOrientation.conjugate(velocityStateBody) * dt);
return positionMeasurement - (positionState + bodyOrientation.rotateVector(velocityStateBody) * dt);
}
int main()
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+8 -8
View File
@@ -327,7 +327,7 @@ AttitudeEstimatorQ::Run()
if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) {
float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f;
/* calculate acceleration in body frame */
_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
_pos_acc = _q.rotateVectorInverse((vel - _vel_prev) / vel_dt);
}
_vel_prev_t = lpos.timestamp;
@@ -452,26 +452,26 @@ AttitudeEstimatorQ::update(float dt)
if (_param_att_ext_hdg_m.get() == 1) {
// Vision heading correction
// Project heading to global frame and extract XY component
Vector3f vision_hdg_earth = _q.conjugate(_vision_hdg);
Vector3f vision_hdg_earth = _q.rotateVector(_vision_hdg);
float vision_hdg_err = wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();
corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();
}
if (_param_att_ext_hdg_m.get() == 2) {
// Mocap heading correction
// Project heading to global frame and extract XY component
Vector3f mocap_hdg_earth = _q.conjugate(_mocap_hdg);
Vector3f mocap_hdg_earth = _q.rotateVector(_mocap_hdg);
float mocap_hdg_err = wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();
corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();
}
}
if (_param_att_ext_hdg_m.get() == 0 || !_ext_hdg_good) {
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector3f mag_earth = _q.conjugate(_mag);
Vector3f mag_earth = _q.rotateVector(_mag);
float mag_err = wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
float gainMult = 1.0f;
const float fifty_dps = 0.873f;
@@ -481,7 +481,7 @@ AttitudeEstimatorQ::update(float dt)
}
// Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
}
_q.normalize();
@@ -489,7 +489,7 @@ AttitudeEstimatorQ::update(float dt)
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
// Vector3f k = _q.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f));
// Vector3f k = _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, 1.0f));
// Optimized version with dropped zeros
Vector3f k(
2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
src/modules/commander/accelerometer_calibration.cpp
+1 -1
View File
@@ -561,7 +561,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
// use vehicle_attitude if available
const vehicle_attitude_s &att = attitude_sub.get();
const matrix::Quatf q{att.q};
const Vector3f accel_ref = q.conjugate_inversed(Vector3f{0.f, 0.f, -CONSTANTS_ONE_G});
const Vector3f accel_ref = q.rotateVectorInverse(Vector3f{0.f, 0.f, -CONSTANTS_ONE_G});
// sanity check angle between acceleration vectors
const float angle = AxisAnglef(Quatf(accel_avg, accel_ref)).angle();
src/modules/uuv_pos_control/uuv_pos_control.cpp
+2 -2
View File
@@ -122,7 +122,7 @@ void UUVPOSControl::pose_controller_6dof(const float x_pos_des, const float y_po
_param_pose_gain_y.get() * (pos_des(1) - vlocal_pos.y) - _param_pose_gain_d_y.get() * vlocal_pos.vy,
_param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z) - _param_pose_gain_d_z.get() * vlocal_pos.vz);
Vector3f rotated_input = q_att.conjugate_inversed(p_control_output);//rotate the coord.sys (from global to body)
Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output);//rotate the coord.sys (from global to body)
publish_attitude_setpoint(rotated_input(0),
rotated_input(1),
@@ -143,7 +143,7 @@ void UUVPOSControl::stabilization_controller_6dof(const float x_pos_des, const f
0,
_param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z));
//potential d controller missing
Vector3f rotated_input = q_att.conjugate_inversed(p_control_output);//rotate the coord.sys (from global to body)
Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output);//rotate the coord.sys (from global to body)
publish_attitude_setpoint(rotated_input(0) + x_pos_des, rotated_input(1) + y_pos_des, rotated_input(2),
roll_des, pitch_des, yaw_des);
src/systemcmds/tests/test_mathlib.cpp
+5 -5
View File
@@ -323,7 +323,7 @@ bool MathlibTest::testQuaternionRotate()
R = matrix::Eulerf(roll, pitch, yaw);
q = matrix::Eulerf(roll, pitch, yaw);
vector_r = R * vector;
vector_q = q.conjugate(vector);
vector_q = q.rotateVector(vector);
for (int i = 0; i < 3; i++) {
ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol);
@@ -336,7 +336,7 @@ bool MathlibTest::testQuaternionRotate()
// test some values calculated with matlab
tol = 0.0001f;
q = matrix::Eulerf(M_PI_2_F, 0.0f, 0.0f);
vector_q = q.conjugate(vector);
vector_q = q.rotateVector(vector);
matrix::Vector3f vector_true = {1.00f, -1.00f, 1.00f};
for (unsigned i = 0; i < 3; i++) {
@@ -344,7 +344,7 @@ bool MathlibTest::testQuaternionRotate()
}
q = matrix::Eulerf(0.3f, 0.2f, 0.1f);
vector_q = q.conjugate(vector);
vector_q = q.rotateVector(vector);
vector_true = {1.1566, 0.7792, 1.0273};
for (unsigned i = 0; i < 3; i++) {
@@ -352,7 +352,7 @@ bool MathlibTest::testQuaternionRotate()
}
q = matrix::Eulerf(-1.5f, -0.2f, 0.5f);
vector_q = q.conjugate(vector);
vector_q = q.rotateVector(vector);
vector_true = {0.5095, 1.4956, -0.7096};
for (unsigned i = 0; i < 3; i++) {
@@ -360,7 +360,7 @@ bool MathlibTest::testQuaternionRotate()
}
q = matrix::Eulerf(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q.conjugate(vector);
vector_q = q.rotateVector(vector);
vector_true = { -1.3660, 0.3660, 1.0000};
for (unsigned i = 0; i < 3; i++) {