Logo Questions Linux Laravel Mysql Ubuntu Git Menu
 

extract yaw, pitch, and roll from a rotationMatrix

I have a sensor manager that returns a rotationMatrix based on the devices Magnetometer and Accelerometer. I have been trying to also calculate the yaw pitch and roll of the user's device but am finding that pitch and roll interfere with each other and give inaccurate results. Is there a way to extract YAW PITCH and ROLL of a device from the rotationMatrix?

EDIT Trying to interpret blender's answer below, which i am thankful for but not quite there yet, i am trying to get the angle from a rotaion matrix like this:

       float R[] = phoneOri.getMatrix();
       double rmYaw = Math.atan2(R[4], R[0]);
       double rmPitch = Math.acos(-R[8]);
       double rmRoll = Math.atan2(R[9], R[10]);

i don't know if i am referencing the wrong parts of the matrix or not but i am not getting the results i would think.

i was hoping to get values in degrees, but am getting weird integers.

my matrix is coming from my sensorManager which looks like this:

public void onSensorChanged(SensorEvent evt) {
            int type=evt.sensor.getType();
            if(type == Sensor.TYPE_ORIENTATION){
                yaw = evt.values[0];
                pitch = evt.values[1];
                roll = evt.values[2];
            }
            if (type == Sensor.TYPE_MAGNETIC_FIELD) {
                orientation[0]=(orientation[0]*1+evt.values[0])*0.5f;
                orientation[1]=(orientation[1]*1+evt.values[1])*0.5f;
                orientation[2]=(orientation[2]*1+evt.values[2])*0.5f;
            } else if (type == Sensor.TYPE_ACCELEROMETER) {
                acceleration[0]=(acceleration[0]*2+evt.values[0])*0.33334f;
                acceleration[1]=(acceleration[1]*2+evt.values[1])*0.33334f;
                acceleration[2]=(acceleration[2]*2+evt.values[2])*0.33334f;
            }
            if ((type==Sensor.TYPE_MAGNETIC_FIELD) || (type==Sensor.TYPE_ACCELEROMETER)) {
                float newMat[]=new float[16];

                SensorManager.getRotationMatrix(newMat, null, acceleration, orientation);
                if(displayOri==0||displayOri==2){
                    SensorManager.remapCoordinateSystem(newMat,SensorManager.AXIS_X*-1, SensorManager.AXIS_MINUS_Y*-1,newMat);
                }else{
                    SensorManager.remapCoordinateSystem(newMat,SensorManager.AXIS_Y, SensorManager.AXIS_MINUS_X,newMat);
                }

                matrix=newMat;

sample matrix when device is laying face up on table

0.9916188,  -0.12448014, -0.03459576,  0.0
0.12525482,  0.9918981,   0.021199778, 0.0
0.031676512,-0.025355382, 0.9991765,   0.0
0.0,         0.0,         0.0,         1

ANSWER

double rmPitch = Math.toDegrees( Math.acos(R[10]));
like image 463
erik Avatar asked Jul 16 '12 23:07

erik


People also ask

How do you calculate yaw pitch and roll?

pitch = atan2( -r20, sqrt(r21*r21+r22*r22) ); yaw = atan2( r10, r00 ); roll = atan2( r21, r22 );

How do you calculate yaw pitch roll from accelerometer?

As in accelerometer one can use the X, Y and Z magnetometer readings to calculate yaw. mag_x = magReadX*cos(pitch) + magReadY mag_y = magReadY * cos(roll) - magReadZ yaw = 180 * atan2(-mag_y,mag_x)/M_PI; Now the most common question asked is, why can't we calculate yaw using accelerometer itself?

What is pitch yaw and roll on a robot?

Pitch is a counterclockwise rotation of m about the y-axis. Roll is a counterclockwise rotation of s about the x-axis. Source publication. A strategy for heterogeneous multi-robot self-localization system.


3 Answers

I believe Blender's answer is not correct, since he gave a transformation from Rotation matrix to Euler angles (z-x-z extrinsic), and Roll Pitch Yaw are a different kind of Euler angles (z-y-x extrinsic).

The actual transformation formula would rather be:

yaw=atan2(R(2,1),R(1,1));
pitch=atan2(-R(3,1),sqrt(R(3,2)^2+R(3,3)^2)));
roll=atan2(R(3,2),R(3,3));

Source

Feedback : this implementation revealed to lack numerical stability near the singularity of the representation (gimbal lock). Therefore on C++ I recommend using Eigen library with the following line of code:

R.eulerAngles(2,1,0).reverse();

(More details here)

like image 62
Aquadarius Avatar answered Oct 12 '22 19:10

Aquadarius


Yaw, pitch and roll correspond to Euler angles. You can convert a transformation matrix to Euler angles pretty easily:

enter image description here

like image 44
Blender Avatar answered Oct 12 '22 19:10

Blender


Sensor Manager provides a SensorManager.getOrientation to get all the three angle.

like image 2
Pikanshu Kumar Avatar answered Oct 12 '22 21:10

Pikanshu Kumar