CODEWITHSUNDEEP
orientationgyroscopepitchrotational-matrices
user1753037
user1753037

Reputation: 29

How to compute yaw, pitch roll from a rotation matrix?

I have to compute in Android yaw, roll, pitch from gyroscope´s smartphones output and I wrote this code:

if(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;
             }

Hello, I have to compute yaw roll, pitch using output from a gyroscope and I wrote this code:

             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]);

I don´t know where is the problem, but the yaw, roll, pitch values I obtained using this code are wrong, because i obtained total different values even if the phone is in the same orientation. I compute them from the rotation matrix in the last three lines of code. And also the values of yaw, pitch roll written in the formulas, are in radians?

Upvotes: 2

Views: 5730

Answers (1)

Kay
Kay

Reputation: 13146

I have limited experience on Android but according to reference manual you can get these values from SensorManager.getOrientation (...) without any calculation. If this is correct, I recommend using this instead of any self made calculation because the values should be more precise due to sensor fusion algorithms working under the hood.

Upvotes: 3

Related Questions