Get directional vector from Yaw, Pitch & Roll

LetHopeItsSnowing · Fri May 01, 2015 10:52 am

I want to calculate the directional vector (x,y,z) from the Astro Pi's Yaw, Pitch and Roll measurements.

To calculate the directional vector from Yaw and Pitch I would use:

x = cos(yaw) * cos(pitch)
y = sin(yaw) * cos(pitch)
z = sin(pitch)

I having a major mathematics melt down and I can get my head around how I would apply roll to that calculation and google is not being my friend...

Anyone got any helpful advice?

Martin

Fri May 01, 2015 10:57 am

You might find it's better to use radians for this as opposed to degrees. Not sure if I can help any more than that though.
Have a read here: https://github.com/astro-pi/astro-pi-ha ... on_radians

LetHopeItsSnowing · Fri May 15, 2015 8:52 am

After some brain pulsing and experimentation this is how...

You have to apply each movement to the vector individually, so:

if:
- y is round and round (yaw)
- x is forward and backward (pitch)
- z is side to side (roll)

#apply yaw (around y)
x = x * cos(yaw) - z * sin(yaw)
z = z * cos(yaw) + x * sin(yaw)

#apply pitch (around x)
y = y * cos(roll) - z * sin(roll)
z = z * cos(roll) + y * sin(roll)

#apply roll (around z)
x = x * cos(pitch) - y * sin(pitch)
y = y * cos(pitch) + x * sin(pitch)

Magic!

PeterO · Fri May 15, 2015 10:32 am

Are you sure about this ? I only ask because in its interpretation in aeronautics roll does not change the direction that the airframe is pointing.

Your original equations looked right to me.

I've done 3D stuff like this myself so I know how confusing it can be !

PeterO

LetHopeItsSnowing · Fri May 15, 2015 11:14 am

Your right, my original description of getting a directional vector is what was wrong, I didnt want a direction vector what I wanted was the ability to transform x,y,z co-ordinates based on yaw, pitch and roll.

PeterO · Fri May 15, 2015 12:13 pm

I just looked up a data sheet on a MEMS gyroscope, and they provide angular velocity about the local x,y,z axis of the chip.

After some interesting lunchtime discussion here in the office, we have concluded that all you have to do is add these three othogonal rotational vectors to give the single rotational axis and the rotational velocity about that axis. You can then rotate your object around that axis to get the new orientaion a short time later.

HTH

PeterO

ryan88 · Sat May 16, 2015 8:53 am

LetHopeItsSnowing wrote:Your right, my original description of getting a directional vector is what was wrong, I didnt want a direction vector what I wanted was the ability to transform x,y,z co-ordinates based on yaw, pitch and roll.

The easiest way to transform x, y, z coordinates based on yaw, pitch and roll (euler angles) is to use a direction cosine matrix. If you look at the diagram below you can see that it has four axis systems. The (ox3y3z3) axis system is the body axis system and is fixed to your astro pi and will rotate with it. The (ox0y0z0) axis system is the reference axis system, this will probably be relative to the ground depending on how you define it and will not rotate with astro pi. The other two axis systems (ox1y1z1) and (ox2y2z2) and just intermediate axis systems used to transform from the reference axis system to the body axis system.

: euler_angles_small.png (61.1 KiB) Viewed 15972 times

To transform from the reference axis system to the body axis system, three rotations need to be applied to the x, y, z coordinates. First you have to yaw about axis oz0 through the angle Ψ, this will take you from the (ox0y0z0) axis system to the (ox1y1z1) axis system. Then you need to pitch about the oy1 axis through angle θ, this will take you from the (ox1y1z1) axis system to the (ox2y2z2) axis system. Finally roll about axis ox2 through the angle ϕ, this will take you from the (ox2y2z2) axis system to the (ox3y3z3) axis system.

So if we perform the first rotation of yawing about oz0 through the angle Ψ and get:

$ox_1=ox_0\cos\psi+oy_0\sin\psi$
$oy_1=-ox_0\sin\psi+oy_0\cos\psi$
$oz_1=oz_0$

Or, representing this as a matrix:

$\begin{bmatrix} ox_1\\ oy_1\\ oz_1 \end{bmatrix} = \begin{bmatrix} \cos\psi & \sin\psi & 0\\ -\sin\psi & \cos\psi & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} ox_0\\ oy_0\\ oz_0 \end{bmatrix}$

Then we perform the second rotation of pitching about oy1 through the angle θ and get:

$ox_2=ox_1\cos\theta-oz_1\sin\theta$
$oy_2=oy_1$
$oz_2=ox_1\sin\theta+oz_1\cos\theta$

Or, representing this as a matrix:

$\begin{bmatrix}ox_2\\oy_2\\oz_2\end{bmatrix}=\begin{bmatrix}\cos\theta&0&-\sin\theta\\0&1&0\\\sin\theta&0&\cos\theta\end{bmatrix}\begin{bmatrix}ox_1\\oy_1\\oz_1\end{bmatrix}$

Then finally we perform the third rotation of rolling about ox2 through the angle ϕ we get:

$ox_3=ox_2$
$oy_3=oy_2\cos\phi+oz_2\sin\phi$
$oz_3=-oy_2\sin\phi+oz_2\cos\phi$

Or, representing this as a matrix:

$\begin{bmatrix}ox_3\\oy_3\\oz_3\end{bmatrix}=\begin{bmatrix}1&0&0\\0&\cos\phi&sin\phi\\0&-\sin\phi&\cos\phi\end{bmatrix}\begin{bmatrix}ox_2\\oy_2\\oz_2\end{bmatrix}$

If we repeatedly substitute the above three matrix equations, we get the following expression for perform all three rotations:

$\begin{bmatrix} ox_3\\ oy_3\\ oz_3 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0\\ 0 & \cos\phi & \sin\phi\\ 0 & -\sin\phi & \cos\phi \end{bmatrix} \begin{bmatrix} \cos\theta & 0 & -\sin\theta\\ 0 & 1 & 0\\ \sin\theta & 0 & \cos\theta \end{bmatrix} \begin{bmatrix} \cos\psi & \sin\psi & 0\\ -\sin\psi & \cos\psi & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} ox_0\\ oy_0\\ oz_0 \end{bmatrix}$

Performing the matrix multiplications gives:

$\begin{bmatrix} ox_3\\ oy_3\\ oz_3 \end{bmatrix} = \begin{bmatrix} \cos\psi\cos\theta & \sin\psi\cos\theta & -\sin\theta\\ \cos\psi\sin\theta\sin\phi-\sin\psi\cos\phi & \sin\psi\sin\theta\sin\phi+\cos\psi\cos\phi & \cos\theta\sin\phi\\ \cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi & \sin\psi\sin\theta\cos\phi-\cos\psi\sin\phi & \cos\theta\cos\phi \end{bmatrix} \begin{bmatrix} ox_0\\ oy_0\\ oz_0 \end{bmatrix}$

So the above equation will rotate x, y, z coordinates in the reference axis system to the body axis system. If you're not too comfortable with matrices, the following equations are equivalent:

$ox_3 = ox_0\cos\psi\cos\theta + oy_0\sin\psi\cos\theta - oz_0\sin\theta$
$oy_3 = ox_0(\cos\psi\sin\theta\sin\phi-\sin\psi\cos\phi) + oy_0(\sin\psi\sin\theta\sin\phi + \cos\psi\cos\phi) + oz_0\cos\theta\sin\phi$
$oz_3 = ox_0(\cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi) + oy_0(\sin\psi\sin\theta\cos\phi - \cos\psi\sin\phi) + oz_0\cos\theta\cos\phi$

If you wish to go the other way around (i.e. from the body axis system to the reference axis system) you need to take the inverse of the big direction cosine matrix in the middle (which for a direction cosine matrix is the same as its transpose):

$\begin{bmatrix} ox_0\\ oy_0\\ oz_0 \end{bmatrix} = \begin{bmatrix} \cos\psi\cos\theta & \cos\psi\sin\theta\sin\phi-\sin\psi\cos\phi & \cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi\\ \sin\psi\cos\theta & \sin\psi\sin\theta\sin\phi+\cos\psi\cos\phi & \sin\psi\sin\theta\cos\phi-\cos\phi\sin\theta\\ -\sin\theta & \cos\theta\sin\phi & \cos\theta\cos\phi \end{bmatrix} \begin{bmatrix} ox_3\\ oy_3\\ oz_3 \end{bmatrix}$

Again, if you're not too familiar with matrices, the following equations are equivalent:

$ox_0 = ox_3\cos\psi\cos\theta+oy_3(\cos\psi\sin\theta\sin\phi-\sin\psi\cos\phi)+oz_3(\cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi)$
$oy_0 = ox_3\sin\psi\cos\theta+oy_3(\sin\psi\sin\theta\sin\phi+\cos\psi\cos\phi)+oz_3(\sin\psi\sin\theta\cos\phi-\cos\phi\sin\theta)$
$oz_o = -ox_3\sin\theta+oy_3\cos\theta\sin\phi+oz_3\cos\theta\cos\phi$

Sorry this post was so maths heavy, but this really is the proper way to do this so I thought I would try to explain it thoroughly. If you have any questions I'll be happy to help.

aTao · Sat May 16, 2015 10:59 am

PeterO wrote:Are you sure about this ? I only ask because in its interpretation in aeronautics roll does not change the direction that the airframe is pointing.

PeterO

But roll does affect the direction that yaw and pitch act in.

Roll 90 port then yaw 90 starboard and you are facing the stars...

Arghhhh... gimbal lock.

PeterO · Sat May 16, 2015 11:15 am

aTao wrote:
PeterO wrote:Are you sure about this ? I only ask because in its interpretation in aeronautics roll does not change the direction that the airframe is pointing.
PeterO
But roll does affect the direction that yaw and pitch act in.
Roll 90 port then yaw 90 starboard and you are facing the stars...

And now no matter how much you roll either way you are STILL pointing at the stars... My initial statement stands correct !
PeterO

LetHopeItsSnowing · Sat May 16, 2015 11:42 am

ryan88 wrote:
LetHopeItsSnowing wrote:Your right, my original description of getting a directional vector is what was wrong, I didnt want a direction vector what I wanted was the ability to transform x,y,z co-ordinates based on yaw, pitch and roll.
The easiest way to transform x, y, z coordinates based on yaw, pitch and roll (euler angles) is to use a direction cosine matrix. If you look at the diagram below you can see that it has four axis systems. The (ox3y3z3) axis system is the body axis system and is fixed to your astro pi and will rotate with it. The (ox0y0z0) axis system is the reference axis system, this will probably be relative to the ground depending on how you define it and will not rotate with astro pi. The other two axis systems (ox1y1z1) and (ox2y2z2) and just intermediate axis systems used to transform from the reference axis system to the body axis system.

euler_angles_small.png
To transform from the reference axis system to the body axis system, three rotations need to be applied to the x, y, z coordinates. First you have to yaw about axis oz0 through the angle Ψ, this will take you from the (ox0y0z0) axis system to the (ox1y1z1) axis system. Then you need to pitch about the oy1 axis through angle θ, this will take you from the (ox1y1z1) axis system to the (ox2y2z2) axis system. Finally roll about axis ox2 through the angle ϕ, this will take you from the (ox2y2z2) axis system to the (ox3y3z3) axis system.

So if we perform the first rotation of yawing about oz0 through the angle Ψ and get:

$ox_1=ox_0\cos\psi+oy_0\sin\psi$
$oy_1=-ox_0\sin\psi+oy_0\cos\psi$
$oz_1=oz_0$

Or, representing this as a matrix:

$\begin{bmatrix} ox_1\\ oy_1\\ oz_1 \end{bmatrix} = \begin{bmatrix} \cos\psi & \sin\psi & 0\\ -\sin\psi & \cos\psi & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} ox_0\\ oy_0\\ oz_0 \end{bmatrix}$

Then we perform the second rotation of pitching about oy1 through the angle θ and get:

$ox_2=ox_1\cos\theta-oz_1\sin\theta$
$oy_2=oy_1$
$oz_2=ox_1\sin\theta+oz_1\cos\theta$

Or, representing this as a matrix:

$\begin{bmatrix}ox_2\\oy_2\\oz_2\end{bmatrix}=\begin{bmatrix}\cos\theta&0&-\sin\theta\\0&1&0\\\sin\theta&0&\cos\theta\end{bmatrix}\begin{bmatrix}ox_1\\oy_1\\oz_1\end{bmatrix}$

Then finally we perform the third rotation of rolling about ox2 through the angle ϕ we get:

$ox_3=ox_2$
$oy_3=oy_2\cos\phi+oz_2\sin\phi$
$oz_3=-oy_2\sin\phi+oz_2\cos\phi$

Or, representing this as a matrix:

$\begin{bmatrix}ox_3\\oy_3\\oz_3\end{bmatrix}=\begin{bmatrix}1&0&0\\0&\cos\phi&sin\phi\\0&-\sin\phi&\cos\phi\end{bmatrix}\begin{bmatrix}ox_2\\oy_2\\oz_2\end{bmatrix}$

If we repeatedly substitute the above three matrix equations, we get the following expression for perform all three rotations:

$\begin{bmatrix} ox_3\\ oy_3\\ oz_3 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0\\ 0 & \cos\phi & \sin\phi\\ 0 & -\sin\phi & \cos\phi \end{bmatrix} \begin{bmatrix} \cos\theta & 0 & -\sin\theta\\ 0 & 1 & 0\\ \sin\theta & 0 & \cos\theta \end{bmatrix} \begin{bmatrix} \cos\psi & \sin\psi & 0\\ -\sin\psi & \cos\psi & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} ox_0\\ oy_0\\ oz_0 \end{bmatrix}$

Performing the matrix multiplications gives:

$\begin{bmatrix} ox_3\\ oy_3\\ oz_3 \end{bmatrix} = \begin{bmatrix} \cos\psi\cos\theta & \sin\psi\cos\theta & -\sin\theta\\ \cos\psi\sin\theta\sin\phi-\sin\psi\cos\phi & \sin\psi\sin\theta\sin\phi+\cos\psi\cos\phi & \cos\theta\sin\phi\\ \cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi & \sin\psi\sin\theta\cos\phi-\cos\psi\sin\phi & \cos\theta\cos\phi \end{bmatrix} \begin{bmatrix} ox_0\\ oy_0\\ oz_0 \end{bmatrix}$

So the above equation will rotate x, y, z coordinates in the reference axis system to the body axis system. If you're not too comfortable with matrices, the following equations are equivalent:

$ox_3 = ox_0\cos\psi\cos\theta + oy_0\sin\psi\cos\theta - oz_0\sin\theta$
$oy_3 = ox_0(\cos\psi\sin\theta\sin\phi-\sin\psi\cos\phi) + oy_0(\sin\psi\sin\theta\sin\phi + \cos\psi\cos\phi) + oz_0\cos\theta\sin\phi$
$oz_3 = ox_0(\cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi) + oy_0(\sin\psi\sin\theta\cos\phi - \cos\psi\sin\phi) + oz_0\cos\theta\cos\phi$

If you wish to go the other way around (i.e. from the body axis system to the reference axis system) you need to take the inverse of the big direction cosine matrix in the middle (which for a direction cosine matrix is the same as its transpose):

$\begin{bmatrix} ox_0\\ oy_0\\ oz_0 \end{bmatrix} = \begin{bmatrix} \cos\psi\cos\theta & \cos\psi\sin\theta\sin\phi-\sin\psi\cos\phi & \cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi\\ \sin\psi\cos\theta & \sin\psi\sin\theta\sin\phi+\cos\psi\cos\phi & \sin\psi\sin\theta\cos\phi-\cos\phi\sin\theta\\ -\sin\theta & \cos\theta\sin\phi & \cos\theta\cos\phi \end{bmatrix} \begin{bmatrix} ox_3\\ oy_3\\ oz_3 \end{bmatrix}$

Again, if you're not too familiar with matrices, the following equations are equivalent:

$ox_0 = ox_3\cos\psi\cos\theta+oy_3(\cos\psi\sin\theta\sin\phi-\sin\psi\cos\phi)+oz_3(\cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi)$
$oy_0 = ox_3\sin\psi\cos\theta+oy_3(\sin\psi\sin\theta\sin\phi+\cos\psi\cos\phi)+oz_3(\sin\psi\sin\theta\cos\phi-\cos\phi\sin\theta)$
$oz_o = -ox_3\sin\theta+oy_3\cos\theta\sin\phi+oz_3\cos\theta\cos\phi$

Sorry this post was so maths heavy, but this really is the proper way to do this so I thought I would try to explain it thoroughly. If you have any questions I'll be happy to help.

I understood this right up to "The easiest way to transform x, y, z coordinates based on yaw, pitch and roll (euler angles)"...

Based on the first paragraph (being only bit I understood), this is what I have implemented. I apply the rotation on each axis individually and low and behold I come up with the right answer...

This is what I did with it https://twitter.com/martinohanlon/statu ... 0305339392

PeterO · Sat May 16, 2015 12:16 pm

Looks good.... but..... the true test will be to see what happens if you yaw while pitched up 45deg

PeterO

LetHopeItsSnowing · Sat May 16, 2015 12:32 pm

You will have to believe me that it works, other than the fact my arrow is pointing down rather than up - my mistake due to Minecraft's odd spacial co-ordinates z - 1 is north z + 1 is south!

aTao · Sat May 16, 2015 1:35 pm

LetHopeItsSnowing wrote:You will have to believe me that it works, other than the fact my arrow is pointing down rather than up - my mistake due to Minecraft's odd spacial co-ordinates z - 1 is north z + 1 is south!

Not so strange in the land of computers, since z+ is often (OpenGl for example) pointing out of the screen to behind you. So, if you are stood on a VR world facing north x+ points east, y+points up and z+ points south.

aTao · Sat May 16, 2015 1:38 pm

PeterO wrote:
aTao wrote:
PeterO wrote:Are you sure about this ? I only ask because in its interpretation in aeronautics roll does not change the direction that the airframe is pointing.
PeterO
But roll does affect the direction that yaw and pitch act in.
Roll 90 port then yaw 90 starboard and you are facing the stars...
And now no matter how much you roll either way you are STILL pointing at the stars... My initial statement stands correct !
PeterO

Indeed, but the @Argghh gimbal lock@ is that once you have rolled, yaw and pitch will affect your heading in different ways than if you hadnt rolled...

Johnny5C · Sat May 16, 2015 1:56 pm

This is heavy stuff guys.

Does anyone know what x, y and z are in relation to the HAT?

I'm writing a script to use the raw accelerometer values, but my astro pi is doing some data logging, so I cant check...

PeterO · Sat May 16, 2015 2:10 pm

aTao wrote: Indeed, but the @Argghh gimbal lock@ is that once you have rolled, yaw and pitch will affect your heading in different ways than if you hadnt rolled...

You keep saying this, and I've not disagreed with you, so what is your point ?
PeterO

aTao · Sat May 16, 2015 3:37 pm

PeterO wrote:
aTao wrote: Indeed, but the @Argghh gimbal lock@ is that once you have rolled, yaw and pitch will affect your heading in different ways than if you hadnt rolled...
You keep saying this, and I've not disagreed with you, so what is your point ?
PeterO

I guess my confusion comes from (in the pile of maths above) why yaw first?

ryan88 · Sun May 17, 2015 7:45 am

LetHopeItsSnowing wrote:I understood this right up to "The easiest way to transform x, y, z coordinates based on yaw, pitch and roll (euler angles)"...

Based on the first paragraph (being only bit I understood), this is what I have implemented. I apply the rotation on each axis individually and low and behold I come up with the right answer...

This is what I did with it https://twitter.com/martinohanlon/statu ... 0305339392

Yes, but I noticed that you have yaw around y, pitch around x and roll around z. Typically yaw would be around z, pitch around y and roll around x. How are the x, y and z axes defined with respect to the astro pi board?

aTao wrote:
PeterO wrote:
aTao wrote:But roll does affect the direction that yaw and pitch act in.
Roll 90 port then yaw 90 starboard and you are facing the stars...
And now no matter how much you roll either way you are STILL pointing at the stars... My initial statement stands correct !
PeterO
Indeed, but the @Argghh gimbal lock@ is that once you have rolled, yaw and pitch will affect your heading in different ways than if you hadnt rolled...

If you really want to avoid gimbal lock, use quaternions.

aTao wrote:
PeterO wrote:
aTao wrote: Indeed, but the @Argghh gimbal lock@ is that once you have rolled, yaw and pitch will affect your heading in different ways than if you hadnt rolled...
You keep saying this, and I've not disagreed with you, so what is your point ?
PeterO
I guess my confusion comes from (in the pile of maths above) why yaw first?

Yaw was first in my post simply because that was the way it was defined in the diagram. If the diagram had been drawn differently, then it could quite easily have been roll-pitch-yaw, or pitch-roll-yaw, or pitch-yaw-roll.

In the aerospace industry, the general convention is to use yaw-pitch-roll, but this is not set in stone because depending on the application, it may make more sense to define it using a different sequence instead.

LetHopeItsSnowing · Sun May 17, 2015 12:50 pm

Thanks Ryan. Ive got Yaw around Y because in the world of minecraft Y is up.

Get directional vector from Yaw, Pitch & Roll

Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll

Re: Get directional vector from Yaw, Pitch & Roll