User avatar
LetHopeItsSnowing
Posts: 357
Joined: Sat May 26, 2012 6:40 am
Location: UK
Contact: Website

Get directional vector from Yaw, Pitch & Roll

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
"am I getting slower, or is stuff more complicated; either way I now have to write it down - stuffaboutcode.com"

User avatar
Davespice
Forum Moderator
Forum Moderator
Posts: 1662
Joined: Fri Oct 14, 2011 8:06 pm
Location: The Netherlands
Contact: Twitter

Re: Get directional vector from Yaw, Pitch & Roll

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

User avatar
LetHopeItsSnowing
Posts: 357
Joined: Sat May 26, 2012 6:40 am
Location: UK
Contact: Website

Re: Get directional vector from Yaw, Pitch & Roll

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!
Last edited by LetHopeItsSnowing on Fri May 15, 2015 11:15 am, edited 1 time in total.
"am I getting slower, or is stuff more complicated; either way I now have to write it down - stuffaboutcode.com"

User avatar
PeterO
Posts: 5066
Joined: Sun Jul 22, 2012 4:14 pm

Re: Get directional vector from Yaw, Pitch & Roll

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
Last edited by PeterO on Fri May 15, 2015 11:41 am, edited 1 time in total.
Discoverer of the PI2 XENON DEATH FLASH!
Interests: C,Python,PIC,Electronics,Ham Radio (G0DZB),1960s British Computers.
"The primary requirement (as we've always seen in your examples) is that the code is readable. " Dougie Lawson

User avatar
LetHopeItsSnowing
Posts: 357
Joined: Sat May 26, 2012 6:40 am
Location: UK
Contact: Website

Re: Get directional vector from Yaw, Pitch & Roll

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.
"am I getting slower, or is stuff more complicated; either way I now have to write it down - stuffaboutcode.com"

User avatar
PeterO
Posts: 5066
Joined: Sun Jul 22, 2012 4:14 pm

Re: Get directional vector from Yaw, Pitch & Roll

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
Discoverer of the PI2 XENON DEATH FLASH!
Interests: C,Python,PIC,Electronics,Ham Radio (G0DZB),1960s British Computers.
"The primary requirement (as we've always seen in your examples) is that the code is readable. " Dougie Lawson

ryan88
Posts: 22
Joined: Sun Jun 09, 2013 4:53 pm

Re: Get directional vector from Yaw, Pitch & Roll

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
euler_angles_small.png (61.1 KiB) Viewed 13189 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:

Image
Image
Image

Or, representing this as a matrix:

Image

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

Image
Image
Image

Or, representing this as a matrix:

Image

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

Image
Image
Image

Or, representing this as a matrix:

Image

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

Image

Performing the matrix multiplications gives:

Image

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:

Image
Image
Image

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):

Image

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

Image
Image
Image

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.

User avatar
aTao
Posts: 1087
Joined: Wed Dec 12, 2012 10:41 am
Location: Howlin Eigg

Re: Get directional vector from Yaw, Pitch & Roll

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.
>)))'><'(((<

User avatar
PeterO
Posts: 5066
Joined: Sun Jul 22, 2012 4:14 pm

Re: Get directional vector from Yaw, Pitch & Roll

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
Discoverer of the PI2 XENON DEATH FLASH!
Interests: C,Python,PIC,Electronics,Ham Radio (G0DZB),1960s British Computers.
"The primary requirement (as we've always seen in your examples) is that the code is readable. " Dougie Lawson

User avatar
LetHopeItsSnowing
Posts: 357
Joined: Sat May 26, 2012 6:40 am
Location: UK
Contact: Website

Re: Get directional vector from Yaw, Pitch & Roll

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:

Image
Image
Image

Or, representing this as a matrix:

Image

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

Image
Image
Image

Or, representing this as a matrix:

Image

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

Image
Image
Image

Or, representing this as a matrix:

Image

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

Image

Performing the matrix multiplications gives:

Image

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:

Image
Image
Image

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):

Image

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

Image
Image
Image

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
"am I getting slower, or is stuff more complicated; either way I now have to write it down - stuffaboutcode.com"

User avatar
PeterO
Posts: 5066
Joined: Sun Jul 22, 2012 4:14 pm

Re: Get directional vector from Yaw, Pitch & Roll

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
Discoverer of the PI2 XENON DEATH FLASH!
Interests: C,Python,PIC,Electronics,Ham Radio (G0DZB),1960s British Computers.
"The primary requirement (as we've always seen in your examples) is that the code is readable. " Dougie Lawson

User avatar
LetHopeItsSnowing
Posts: 357
Joined: Sat May 26, 2012 6:40 am
Location: UK
Contact: Website

Re: Get directional vector from Yaw, Pitch & Roll

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!
"am I getting slower, or is stuff more complicated; either way I now have to write it down - stuffaboutcode.com"

User avatar
aTao
Posts: 1087
Joined: Wed Dec 12, 2012 10:41 am
Location: Howlin Eigg

Re: Get directional vector from Yaw, Pitch & Roll

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.
>)))'><'(((<

User avatar
aTao
Posts: 1087
Joined: Wed Dec 12, 2012 10:41 am
Location: Howlin Eigg

Re: Get directional vector from Yaw, Pitch & Roll

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 [email protected] is that once you have rolled, yaw and pitch will affect your heading in different ways than if you hadnt rolled...
>)))'><'(((<

Johnny5C
Posts: 82
Joined: Thu Apr 18, 2013 3:32 pm

Re: Get directional vector from Yaw, Pitch & Roll

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...

User avatar
PeterO
Posts: 5066
Joined: Sun Jul 22, 2012 4:14 pm

Re: Get directional vector from Yaw, Pitch & Roll

Sat May 16, 2015 2:10 pm

aTao wrote: Indeed, but the @Argghh gimbal [email protected] 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
Discoverer of the PI2 XENON DEATH FLASH!
Interests: C,Python,PIC,Electronics,Ham Radio (G0DZB),1960s British Computers.
"The primary requirement (as we've always seen in your examples) is that the code is readable. " Dougie Lawson

User avatar
aTao
Posts: 1087
Joined: Wed Dec 12, 2012 10:41 am
Location: Howlin Eigg

Re: Get directional vector from Yaw, Pitch & Roll

Sat May 16, 2015 3:37 pm

PeterO wrote:
aTao wrote: Indeed, but the @Argghh gimbal [email protected] 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
Posts: 22
Joined: Sun Jun 09, 2013 4:53 pm

Re: Get directional vector from Yaw, Pitch & Roll

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 [email protected] 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 [email protected] 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.

User avatar
LetHopeItsSnowing
Posts: 357
Joined: Sat May 26, 2012 6:40 am
Location: UK
Contact: Website

Re: Get directional vector from Yaw, Pitch & Roll

Sun May 17, 2015 12:50 pm

Thanks Ryan. Ive got Yaw around Y because in the world of minecraft Y is up.
"am I getting slower, or is stuff more complicated; either way I now have to write it down - stuffaboutcode.com"

Return to “Astro Pi”