Quaternion: rename function to rotate vectors

src/lib/matrix/matrix/Quaternion.hpp

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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++) {