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

add renormalize method (#32)

Browse Source
This commit is contained in:
James Goppert 2016-11-02 12:10:30 -04:00 committed by GitHub
parent db4374882b
commit 34fccdd680
2 changed files with 67 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
matrix/Dcm.hpp
View File

@ -81,16 +81,17 @@ public:
*
* @param q quaternion to set dcm to
*/
Dcm(const Quaternion<Type> & q) {
Dcm(const Quaternion<Type> &q)
{
Dcm &dcm = *this;
Type a = q(0);
Type b = q(1);
Type c = q(2);
Type d = q(3);
Type aSq = a*a;
Type bSq = b*b;
Type cSq = c*c;
Type dSq = d*d;
Type aSq = a * a;
Type bSq = b * b;
Type cSq = c * c;
Type dSq = d * d;
dcm(0, 0) = aSq + bSq - cSq - dSq;
dcm(0, 1) = 2 * (b * c - a * d);
dcm(0, 2) = 2 * (a * c + b * d);
@ -111,7 +112,8 @@ public:
*
* @param euler euler angle instance
*/
Dcm(const Euler<Type> & euler) {
Dcm(const Euler<Type> &euler)
{
Dcm &dcm = *this;
Type cosPhi = Type(cos(euler.phi()));
Type sinPhi = Type(sin(euler.phi()));
@ -143,19 +145,30 @@ public:
*
* @param euler euler angle instance
*/
Dcm(const AxisAngle<Type> & aa) {
Dcm(const AxisAngle<Type> &aa)
{
Dcm &dcm = *this;
dcm = Quaternion<Type>(aa);
}
Vector<Type, 3> vee() const { // inverse to Vector.hat() operation
Vector<Type, 3> vee() const // inverse to Vector.hat() operation
{
const Dcm &A(*this);
Vector<Type, 3> v;
v(0) = -A(1,2);
v(1) = A(0,2);
v(2) = -A(0,1);
v(0) = -A(1, 2);
v(1) = A(0, 2);
v(2) = -A(0, 1);
return v;
}
void renormalize()
{
/* renormalize rows */
for (int row = 0; row < 3; row++) {
matrix::Vector3f rvec(this->_data[row]);
this->setRow(row, rvec.normalized());
}
}
};
typedef Dcm<float> Dcmf;

62
test/attitude.cpp
View File

@ -96,40 +96,62 @@ int main()
Quatf q2(dcm_check);
TEST(isEqual(q2, q_check));
// dcm renormalize
Dcmf A = eye<float, 3>();
Dcmf R(euler_check);
for (int i = 0; i < 1000; i++) {
A = R * A;
}
A.renormalize();
float err = 0.0f;
for (int row = 0; row < 3; row++) {
matrix::Vector3f rvec(A._data[row]);
err += fabsf(1.0f - rvec.length());
}
TEST(err < eps);
// constants
double deg2rad = M_PI/180.0;
double rad2deg = 180.0/M_PI;
double deg2rad = M_PI / 180.0;
double rad2deg = 180.0 / M_PI;
// euler dcm round trip check
for (int roll=-90; roll<=90; roll+=90) {
for (int pitch=-90; pitch<=90; pitch+=90) {
for (int yaw=-179; yaw<=180; yaw+=90) {
for (int roll = -90; roll <= 90; roll += 90) {
for (int pitch = -90; pitch <= 90; pitch += 90) {
for (int yaw = -179; yaw <= 180; yaw += 90) {
// note if theta = pi/2, then roll is set to zero
int roll_expected = roll;
int yaw_expected = yaw;
if (pitch == 90) {
roll_expected = 0;
yaw_expected = yaw - roll;
} else if (pitch == -90) {
roll_expected = 0;
yaw_expected = yaw + roll;
}
if (yaw_expected < -180) yaw_expected += 360;
if (yaw_expected > 180) yaw_expected -= 360;
if (yaw_expected < -180) {
yaw_expected += 360;
}
if (yaw_expected > 180) {
yaw_expected -= 360;
}
//printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
Euler<double> euler_expected(
deg2rad*double(roll_expected),
deg2rad*double(pitch),
deg2rad*double(yaw_expected));
deg2rad * double(roll_expected),
deg2rad * double(pitch),
deg2rad * double(yaw_expected));
Euler<double> euler(
deg2rad*double(roll),
deg2rad*double(pitch),
deg2rad*double(yaw));
deg2rad * double(roll),
deg2rad * double(pitch),
deg2rad * double(yaw));
Dcm<double> dcm_from_euler(euler);
//dcm_from_euler.print();
Euler<double> euler_out(dcm_from_euler);
TEST(isEqual(rad2deg*euler_expected, rad2deg*euler_out));
TEST(isEqual(rad2deg * euler_expected, rad2deg * euler_out));
Eulerf eulerf_expected(
float(deg2rad)*float(roll_expected),
@ -160,14 +182,14 @@ int main()
// quaternion derivate
Quatf q1(0, 1, 0, 0);
Vector<float, 4> q1_dot = q1.derivative(Vector3f(1, 2, 3));
float data_q_dot_check[] = {-0.5f, 0.0f, -1.5f, 1.0f};
float data_q_dot_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
Vector<float, 4> q1_dot_check(data_q_dot_check);
TEST(isEqual(q1_dot, q1_dot_check));
// quaternion product
Quatf q_prod_check(
0.93394439f, 0.0674002f, 0.20851f, 0.28236266f);
TEST(isEqual(q_prod_check, q_check*q_check));
TEST(isEqual(q_prod_check, q_check * q_check));
q_check *= q_check;
TEST(isEqual(q_prod_check, q_check));
@ -225,7 +247,7 @@ int main()
q = Quatf(cosf(1.0f / 2), 0.0f, sinf(1.0f / 2), 0.0f);
rot = q.to_axis_angle();
TEST(fabsf(rot(0)) < eps);
TEST(fabsf(rot(1) -1.0f) < eps);
TEST(fabsf(rot(1) - 1.0f) < eps);
TEST(fabsf(rot(2)) < eps);
// get rotation axis from quaternion (zero rotation)
@ -247,8 +269,10 @@ int main()
// Quaternion initialisation per array
float q_array[] = {0.9833f, -0.0343f, -0.1060f, -0.1436f};
Quaternion<float>q_from_array(q_array);
for(int i = 0; i < 4; i++)
for (int i = 0; i < 4; i++) {
TEST(fabsf(q_from_array(i) - q_array[i]) < eps);
}
// axis angle
AxisAnglef aa_true(Vector3f(1.0f, 2.0f, 3.0f));
@ -292,7 +316,7 @@ int main()
// check consistentcy of quaternion and dcm product
Dcmf dcm3(Eulerf(1, 2, 3));
Dcmf dcm4(Eulerf(4, 5, 6));
Dcmf dcm34 = dcm3*dcm4;
Dcmf dcm34 = dcm3 * dcm4;
TEST(isEqual(Eulerf(Quatf(dcm4)*Quatf(dcm3)), Eulerf(dcm34)));
};