Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
/*
 * @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;
}
Ejemplo n.º 3
0
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;
}
Ejemplo n.º 4
0
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);
}
Ejemplo n.º 5
0
/* 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;
  
  
}
Ejemplo n.º 6
0
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;
}
Ejemplo n.º 7
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;
}
Ejemplo n.º 8
0
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;
}