dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1829 from PX4/mathlib_tests 

Quaternion method tests
This commit is contained in:
Lorenz Meier 2015-02-23 22:59:59 +01:00
parent 9a875c53af a252f4a991
commit d232f41ce9
2 changed files with 95 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/lib/mathlib/math/Quaternion.hpp
View File

@ -135,12 +135,34 @@ public:
data[3] = static_cast<float>(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2);
}
void from_dcm(const Matrix<3, 3> &m) {
// avoiding singularities by not using division equations
data[0] = 0.5f * sqrtf(1.0f + m.data[0][0] + m.data[1][1] + m.data[2][2]);
data[1] = 0.5f * sqrtf(1.0f + m.data[0][0] - m.data[1][1] - m.data[2][2]);
data[2] = 0.5f * sqrtf(1.0f - m.data[0][0] + m.data[1][1] - m.data[2][2]);
data[3] = 0.5f * sqrtf(1.0f - m.data[0][0] - m.data[1][1] + m.data[2][2]);
void from_dcm(const Matrix<3, 3> &dcm) {
float tr = dcm.data[0][0] + dcm.data[1][1] + dcm.data[2][2];
if (tr > 0.0f) {
float s = sqrtf(tr + 1.0f);
data[0] = s * 0.5f;
s = 0.5f / s;
data[1] = (dcm.data[2][1] - dcm.data[1][2]) * s;
data[2] = (dcm.data[0][2] - dcm.data[2][0]) * s;
data[3] = (dcm.data[1][0] - dcm.data[0][1]) * s;
} else {
/* Find maximum diagonal element in dcm
* store index in dcm_i */
int dcm_i = 0;
for (int i = 1; i < 3; i++) {
if (dcm.data[i][i] > dcm.data[dcm_i][dcm_i]) {
dcm_i = i;
}
}
int dcm_j = (dcm_i + 1) % 3;
int dcm_k = (dcm_i + 2) % 3;
float s = sqrtf((dcm.data[dcm_i][dcm_i] - dcm.data[dcm_j][dcm_j] -
dcm.data[dcm_k][dcm_k]) + 1.0f);
data[dcm_i + 1] = s * 0.5f;
s = 0.5f / s;
data[dcm_j + 1] = (dcm.data[dcm_i][dcm_j] + dcm.data[dcm_j][dcm_i]) * s;
data[dcm_k + 1] = (dcm.data[dcm_k][dcm_i] + dcm.data[dcm_i][dcm_k]) * s;
data[0] = (dcm.data[dcm_k][dcm_j] - dcm.data[dcm_j][dcm_k]) * s;
}
}
/**

67
src/systemcmds/tests/test_mathlib.cpp
View File

@ -205,5 +205,72 @@ int test_mathlib(int argc, char *argv[])
}
{
// test conversion rotation matrix to quaternion and back
math::Matrix<3,3> R_orig;
math::Matrix<3,3> R;
math::Quaternion q;
float diff = 0.1f;
float tol = 0.00001f;
for(float roll = -M_PI_F;roll <= M_PI_F;roll+=diff) {
for(float pitch = -M_PI_2_F;pitch <= M_PI_2_F;pitch+=diff) {
for(float yaw = -M_PI_F;yaw <= M_PI_F;yaw+=diff) {
R_orig.from_euler(roll,pitch,yaw);
q.from_dcm(R_orig);
R = q.to_dcm();
for(int i = 0;i< 3;i++) {
for(int j=0;j<3;j++) {
if(fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) {
warnx("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!");
rc = 1;
}
}
}
}
}
}
// test against some known values
tol = 0.0001f;
math::Quaternion q_true = {1.0f,0.0f,0.0f,0.0f};
R_orig.identity();
q.from_dcm(R_orig);
for(unsigned i = 0;i<4;i++) {
if(fabsf(q.data[i] - q_true.data[i]) > tol) {
warnx("Quaternion method 'from_dcm()' outside tolerance!");
rc = 1;
}
}
q_true.from_euler(0.3f,0.2f,0.1f);
q = {0.9833f,0.1436f,0.1060f,0.0343f};
for(unsigned i = 0;i<4;i++) {
if(fabsf(q.data[i] - q_true.data[i]) > tol) {
warnx("Quaternion method 'from_euler()' outside tolerance!");
rc = 1;
}
}
q_true.from_euler(-1.5f,-0.2f,0.5f);
q = {0.7222f,-0.6391f,-0.2386f,0.1142f};
for(unsigned i = 0;i<4;i++) {
if(fabsf(q.data[i] - q_true.data[i]) > tol) {
warnx("Quaternion method 'from_euler()' outside tolerance!");
rc = 1;
}
}
q_true.from_euler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3);
q = {0.6830f,0.1830f,-0.6830f,0.1830f};
for(unsigned i = 0;i<4;i++) {
if(fabsf(q.data[i] - q_true.data[i]) > tol) {
warnx("Quaternion method 'from_euler()' outside tolerance!");
rc = 1;
}
}
}
return rc;
}