如何從旋轉矩陣計算偏航，俯仰搖擺？

Question

我必須計算在Android的偏航，側傾，俯仰從gyroscope's智能手機輸出和I寫了這個代碼：

如果（event.sensor.getType（）== Sensor.TYPE_GYROSCOPE）{

 float xgyr=event.values[0];    //rotation around x-axis [rad/sec] 
     float ygyr=event.values[1];    // rotation around y-axis 
     float zgyr=event.values[2];    // rotation around z-axis 


     // This timestep's delta rotation to be multiplied by the current rotation 
     // after computing it from the gyro sample data. 

     double EPSILON = 0.0;   //EPSILON value to be defined 
     if (timestamp != 0) { 
      final float dT = (event.timestamp - timestamp) * NS2S; 
      // Axis of the rotation sample, not normalized yet. 
      float axisX = event.values[0]; 
      float axisY = event.values[1]; 
      float axisZ = event.values[2]; 

      // Calculate the angular speed of the sample, teta is the vector length 
      float omegaMagnitude = (float) Math.sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ); 

      // Normalize the rotation vector if it's big enough to get the axis 
      if (omegaMagnitude > EPSILON) {     //EPSILON TO BE DEFINED 
       axisX /= omegaMagnitude; 
       axisY /= omegaMagnitude; 
       axisZ /= omegaMagnitude; 
      } 

您好，我有利用陀螺儀輸出來計算偏航滾動，俯仰和我寫了這個代碼：

   float thetaOverTwo = omegaMagnitude * dT/2.0f;  //Insert initial value for orientation omegaMagnitude 
      float sinThetaOverTwo = (float) Math.sin(thetaOverTwo); 
      float cosThetaOverTwo = (float) Math.cos(thetaOverTwo); 

      /*rotation vector, a non-normalized three-dimensional vector the direction of which specifies the rotation axis, 
      and the length of which is teta, Combining two consecutive quaternion rotations is therefore just as simple as using the rotation matrix. 
      Remember that two successive rotation matrices, A1 , A2 are combined A3 = A2*A1*/ 

      //Quaternions 
      deltaRotationVector[0] = sinThetaOverTwo * axisX; 
      deltaRotationVector[1] = sinThetaOverTwo * axisY; 
      deltaRotationVector[2] = sinThetaOverTwo * axisZ; 
      deltaRotationVector[3] = cosThetaOverTwo; 
     } 
     timestamp = event.timestamp; 
     float[] deltaRotationMatrix = new float[9]; 
     SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector); 
     // User code should concatenate the delta rotation we computed with the current rotation 
     // in order to get the updated rotation. 
     // rotationCurrent = rotationCurrent * deltaRotationMatrix; The initial rotation has to be computedand then at each step updated 
     /*Rotation current is a vector with rotation on pitch, roll, yaw (3x1) multiplied by 3x3 rotation matrix i have the new orientation*/ 

     /*In the "xyz (pitch-roll-yaw) convention," theta is pitch, psi is roll, and phi is yaw. */ 




     double pitch = Math.atan2(deltaRotationMatrix[6], deltaRotationMatrix[7]); 
     double roll = Math.acos(deltaRotationMatrix[8]); 
     double yaw = - Math.atan2(deltaRotationMatrix[2], deltaRotationMatrix[5]); 

我不知道問題出在哪裏，但我得到的偏航，滾動，俯仰值使用此代碼是錯誤的，因爲我獲得了不同的總價值即使手機處於相同的方向。我從最後三行代碼中的旋轉矩陣計算它們。並且公式中寫入的偏航，俯仰滾動的值以弧度表示？

Answer 1

我在Android上的經驗有限，但根據參考手冊，您可以從SensorManager.getOrientation (...)中獲取這些值，而無需進行任何計算。如果這是正確的，我建議使用這個來代替任何自制的計算，因爲由於傳感器融合算法在引擎蓋下工作，值應該更精確。