C3DTMatrix quaternionToInvertedMatrix(_C3DTQuaternion q)
{
C3DTMatrix m;
q = quaternionNormalize(q);
float xx = q.cartesian.x * q.cartesian.x;
float yy = q.cartesian.y * q.cartesian.y;
float zz = q.cartesian.z * q.cartesian.z;
m.flts[0] = -(1.0f - 2.0f * (yy + zz));
m.flts[1] = -(2.0f * (q.cartesian.x*q.cartesian.y + q.cartesian.w*q.cartesian.z));
m.flts[2] = -(2.0f * (q.cartesian.x*q.cartesian.z - q.cartesian.w*q.cartesian.y));
m.flts[3] = 0.0f;
m.flts[4] = 2.0f * (q.cartesian.x*q.cartesian.y - q.cartesian.w*q.cartesian.z);
m.flts[5] = 1.0f - 2.0f * (xx + zz);
m.flts[6] = 2.0f * (q.cartesian.y*q.cartesian.z + q.cartesian.w*q.cartesian.x);
m.flts[7] = 0.0f;
m.flts[8] = 2.0f * (q.cartesian.x*q.cartesian.z + q.cartesian.w*q.cartesian.y);
m.flts[9] = 2.0f * (q.cartesian.y*q.cartesian.z - q.cartesian.w*q.cartesian.x);
m.flts[10] = 1.0f - 2.0f * (xx + yy);
m.flts[11] = 0.0f;
m.flts[12] = 0.0f;
m.flts[13] = 0.0f;
m.flts[14] = 0.0f;
m.flts[15] = 1.0f;
return m;
}
/*
* @brief Perform q1 * q2
*/
inline Eigen::Vector4d quaternionMultiplication(
const Eigen::Vector4d& q1,
const Eigen::Vector4d& q2) {
Eigen::Matrix4d L;
L(0, 0) = q1(3); L(0, 1) = q1(2); L(0, 2) = -q1(1); L(0, 3) = q1(0);
L(1, 0) = -q1(2); L(1, 1) = q1(3); L(1, 2) = q1(0); L(1, 3) = q1(1);
L(2, 0) = q1(1); L(2, 1) = -q1(0); L(2, 2) = q1(3); L(2, 3) = q1(2);
L(3, 0) = -q1(0); L(3, 1) = -q1(1); L(3, 2) = -q1(2); L(3, 3) = q1(3);
Eigen::Vector4d q = L * q2;
quaternionNormalize(q);
return q;
}
C3DTVector quaternionToDirectionVector(_C3DTQuaternion q)
{
C3DTVector v;
q = quaternionNormalize(q);
v.cartesian.x = 2.0f * (q.cartesian.x*q.cartesian.z - q.cartesian.w*q.cartesian.y);
v.cartesian.y = 2.0f * (q.cartesian.y*q.cartesian.z + q.cartesian.w*q.cartesian.x);
v.cartesian.z = 1.0f - 2.0f * (q.cartesian.x*q.cartesian.x + q.cartesian.y*q.cartesian.y);
v.cartesian.w = 0.0f;
return v;
}
void eulerToQuaternion(vector3d_t v, quaternion_t q)
{
float cosX2 = cosf(v[VEC3_X] / 2.0f);
float sinX2 = sinf(v[VEC3_X] / 2.0f);
float cosY2 = cosf(v[VEC3_Y] / 2.0f);
float sinY2 = sinf(v[VEC3_Y] / 2.0f);
float cosZ2 = cosf(v[VEC3_Z] / 2.0f);
float sinZ2 = sinf(v[VEC3_Z] / 2.0f);
q[QUAT_W] = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2;
q[QUAT_X] = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2;
q[QUAT_Y] = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2;
q[QUAT_Z] = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2;
quaternionNormalize(q);
}
/* Derivation of the acceleration to get velocity and position
*/
void derivate_accel(mpudata_t *mpu){
double ax = (float)mpu->calibratedAccel[VEC3_X]/16384;//*9.81/16384;
double ay = (float)mpu->calibratedAccel[VEC3_Y]/16384;//*9.81/16384;
double az = (float)mpu->calibratedAccel[VEC3_Z]/16384;//*9.81/16384;
double deltaTms = mpu->dmpTimestamp - timeOld;
double deltaT = deltaTms/1000;
double vx, vy, vz;
double px, py, pz;
quaternion_t dmpQuat;
matrix3d_t rotMatrix;
dmpQuat[QUAT_W] = (float)mpu->rawQuat[QUAT_W];
dmpQuat[QUAT_X] = (float)mpu->rawQuat[QUAT_X];
dmpQuat[QUAT_Y] = (float)mpu->rawQuat[QUAT_Y];
dmpQuat[QUAT_Z] = (float)mpu->rawQuat[QUAT_Z];
quaternionNormalize(dmpQuat);
quaternionToRotMatrix(dmpQuat,rotMatrix);
//%printf("Rotations Matrix\n");
//printf("%f\t%f\t%f\n%f\t%f\t%f\n%f\t%f\t%f\n",rotMatrix[0][0],rotMatrix[0][1],rotMatrix[0][2],rotMatrix[1][0],rotMatrix[1][1],rotMatrix[1][2],rotMatrix[2][0],rotMatrix[2][1],rotMatrix[2][2]);
// if first ..... einfügen !!!!!!!!!!!!!!!
vx = ax * deltaT;
vy = ay * deltaT;
vz = az * deltaT;
px = 0.5 * ax * deltaT*deltaT;
py = 0.5 * ay * deltaT*deltaT;
pz = 0.5 * az * deltaT*deltaT;
//printf("Position: X %f Y %f Z %f\n",px,py,pz);
printf("%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f\n",ax,ay,az,vx,vy,vz,px,py,pz,deltaT, dmpQuat[QUAT_W],dmpQuat[QUAT_X],dmpQuat[QUAT_Y],dmpQuat[QUAT_Z]);
//printf("%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f\n",ax,ay,az,vx,vy,vz,px,py,pz,deltaT, dmpQuat[QUAT_W],dmpQuat[QUAT_X],dmpQuat[QUAT_Y],dmpQuat[QUAT_Z],rotMatrix[0][0],rotMatrix[0][1],rotMatrix[0][2],rotMatrix[1][0],rotMatrix[1][1],rotMatrix[1][2],rotMatrix[2][0],rotMatrix[2][1],rotMatrix[2][2]);
//printf("%f;%f;%f;%f\n",dmpQuat[QUAT_W],dmpQuat[QUAT_X],dmpQuat[QUAT_Y],dmpQuat[QUAT_Z]);
timeOld = mpu->dmpTimestamp;
}
int data_fusion(mpudata_t *mpu)
{
quaternion_t dmpQuat;
vector3d_t dmpEuler;
quaternion_t unfusedQuat;
dmpQuat[QUAT_W] = (float)mpu->rawQuat[QUAT_W];
dmpQuat[QUAT_X] = (float)mpu->rawQuat[QUAT_X];
dmpQuat[QUAT_Y] = (float)mpu->rawQuat[QUAT_Y];
dmpQuat[QUAT_Z] = (float)mpu->rawQuat[QUAT_Z];
quaternionNormalize(dmpQuat);
quaternionToEuler(dmpQuat, dmpEuler);
mpu->fusedEuler[VEC3_X] = dmpEuler[VEC3_X];
mpu->fusedEuler[VEC3_Y] = -dmpEuler[VEC3_Y];
mpu->fusedEuler[VEC3_Z] = -dmpEuler[VEC3_Z]; //0;
return 0;
}
/*
* @brief Convert a rotation matrix to a quaternion.
* @note Pay attention to the convention used. The function follows the
* conversion in "Indirect Kalman Filter for 3D Attitude Estimation:
* A Tutorial for Quaternion Algebra", Equation (78).
*
* The input quaternion should be in the form
* [q1, q2, q3, q4(scalar)]^T
*/
inline Eigen::Vector4d rotationToQuaternion(
const Eigen::Matrix3d& R) {
Eigen::Vector4d score;
score(0) = R(0, 0);
score(1) = R(1, 1);
score(2) = R(2, 2);
score(3) = R.trace();
int max_row = 0, max_col = 0;
score.maxCoeff(&max_row, &max_col);
Eigen::Vector4d q = Eigen::Vector4d::Zero();
if (max_row == 0) {
q(0) = std::sqrt(1+2*R(0, 0)-R.trace()) / 2.0;
q(1) = (R(0, 1)+R(1, 0)) / (4*q(0));
q(2) = (R(0, 2)+R(2, 0)) / (4*q(0));
q(3) = (R(1, 2)-R(2, 1)) / (4*q(0));
} else if (max_row == 1) {
q(1) = std::sqrt(1+2*R(1, 1)-R.trace()) / 2.0;
q(0) = (R(0, 1)+R(1, 0)) / (4*q(1));
q(2) = (R(1, 2)+R(2, 1)) / (4*q(1));
q(3) = (R(2, 0)-R(0, 2)) / (4*q(1));
} else if (max_row == 2) {
q(2) = std::sqrt(1+2*R(2, 2)-R.trace()) / 2.0;
q(0) = (R(0, 2)+R(2, 0)) / (4*q(2));
q(1) = (R(1, 2)+R(2, 1)) / (4*q(2));
q(3) = (R(0, 1)-R(1, 0)) / (4*q(2));
} else {
q(3) = std::sqrt(1+R.trace()) / 2.0;
q(0) = (R(1, 2)-R(2, 1)) / (4*q(3));
q(1) = (R(2, 0)-R(0, 2)) / (4*q(3));
q(2) = (R(0, 1)-R(1, 0)) / (4*q(3));
}
if (q(3) < 0) q = -q;
quaternionNormalize(q);
return q;
}
int data_fusion(mpudata_t *mpu) {
quaternion_t dmpQuat;
vector3d_t dmpEuler;
quaternion_t magQuat;
quaternion_t unfusedQuat;
float deltaDMPYaw;
float deltaMagYaw;
float newMagYaw;
float newYaw;
dmpQuat[QUAT_W] = (float)mpu->rawQuat[QUAT_W];
dmpQuat[QUAT_X] = (float)mpu->rawQuat[QUAT_X];
dmpQuat[QUAT_Y] = (float)mpu->rawQuat[QUAT_Y];
dmpQuat[QUAT_Z] = (float)mpu->rawQuat[QUAT_Z];
quaternionNormalize(dmpQuat);
quaternionToEuler(dmpQuat, dmpEuler);
mpu->fusedEuler[VEC3_X] = dmpEuler[VEC3_X];
mpu->fusedEuler[VEC3_Y] = -dmpEuler[VEC3_Y];
mpu->fusedEuler[VEC3_Z] = 0;
eulerToQuaternion(mpu->fusedEuler, unfusedQuat);
deltaDMPYaw = -dmpEuler[VEC3_Z] + mpu->lastDMPYaw;
mpu->lastDMPYaw = dmpEuler[VEC3_Z];
magQuat[QUAT_W] = 0;
magQuat[QUAT_X] = mpu->calibratedMag[VEC3_X];
magQuat[QUAT_Y] = mpu->calibratedMag[VEC3_Y];
magQuat[QUAT_Z] = mpu->calibratedMag[VEC3_Z];
tilt_compensate(magQuat, unfusedQuat);
newMagYaw = -atan2f(magQuat[QUAT_Y], magQuat[QUAT_X]);
if (newMagYaw != newMagYaw) {
printf("newMagYaw NAN\n");
return -1;
}
if (newMagYaw < 0.0f)
newMagYaw = TWO_PI + newMagYaw;
newYaw = mpu->lastYaw + deltaDMPYaw;
if (newYaw > TWO_PI)
newYaw -= TWO_PI;
else if (newYaw < 0.0f)
newYaw += TWO_PI;
deltaMagYaw = newMagYaw - newYaw;
if (deltaMagYaw >= (float)M_PI)
deltaMagYaw -= TWO_PI;
else if (deltaMagYaw < -(float)M_PI)
deltaMagYaw += TWO_PI;
if (yaw_mixing_factor > 0)
newYaw += deltaMagYaw / yaw_mixing_factor;
if (newYaw > TWO_PI)
newYaw -= TWO_PI;
else if (newYaw < 0.0f)
newYaw += TWO_PI;
mpu->lastYaw = newYaw;
if (newYaw > (float)M_PI)
newYaw -= TWO_PI;
mpu->fusedEuler[VEC3_Z] = newYaw;
eulerToQuaternion(mpu->fusedEuler, mpu->fusedQuat);
return 0;
}
