[0:00]Hey folks, for this screencast, I'm going to talk about something that I've been working on the last few days, and I went ahead and put the equations in my free Aerospace Mechanics textbook here.
[0:10]So if you run over my GitHub C Montalvo 251 and you go to my Latex repo and you go to Aerospace Mechanics, I've got my entire textbook here.
[0:20]I even threw the tech file in there in case you want to and even the figures if you want to, you know, use them, just make sure to give me credit for it forever you want, you know, reference this repo.
[0:29]If you click this link, you can just download the PDF itself, but I I hilariously have the the raw PDF downloaded and you can see like I just updated this recently.
[0:38]So what I want to talk about, what I've been working on is I've been beefing up the uh the Euler angle parameterization.
[0:46]So one of the things that we do in in aerospace uh convention is we talk about the 3-2-1 sequence and so that's where, you know, you have, I'm going to use this as an airplane.
[0:53]Um it's a little uh small camera. And basically you do a uh a rotation about the Z-axis, that's your yaw angle and so that's why there are some cosine si and sine si.
[1:05]And this is a three-dimensional transformation. I'm not going to I'm not going to really go into the detail of this thing. Anybody with uh, you know, some rudimentary uh undergraduate dynamics 3D dynamics, you know, you'll you'll have seen that.
[1:17]And so I I'd recommend looking through somewhere else. I just have these equations there for reference, but you basically have your your yaw axis about the Z and that's where the three comes in.
[1:28]That's the third axis, and then the second rotation is a uh pitch through theta about the Y-axis.
[1:36]That's the two rotation, and then you finally have a um rotation through roll or fee about the X-axis, which is the one.
[1:47]So that's a 3-2-1 rotation, and you can kind of see this graphic here which basically shows, let me move this out of the way, which shows this projectile moving about the Z-axis through sai, and then about the intermediate axis through theta, and then about the final axis through roll.
[2:04]And you can do some matrix multiplication and you can multiply this out and you can get this really, really large uh equation here, uh that basically rotates the body to the inertial frame.
[2:13]And I did another video a while ago about, you know, the the AHRS filter, which basically takes your IMU measurements and estimates the quaternions based on your accelerometer, gyro, and your magnetotomoter.
[2:26]Now, one of the things that I was realizing is that that that is a fairly complex algorithm and sometimes, you know, you might create a drone that can't move through, you know, 90 degrees, which creates that, you know, gimbal lock or singularity at pi over two.
[2:43]And so you might want to just get roll and pitch directly from an IMU. And so I've put together this chapter here on guidance, navigation and controls.
[2:50]And I've talked a little bit about state estimation, and I talk about the eternal, the inertial measurement unit, the IMU, which has a a magnetometer, a a rate gyro, and then also an accelerometer.
[3:03]And so I also talk about the GPS, which, you know, the GPS and the IMU put together gives you an INS, an inertial navigation system.
[3:11]And then I go on to talk about is it possible to get Euler angles from directly from your IMU, rather than using this complex AHRS filter?
[3:20]And it turns out it is. And so what you and what I want to do in this video is go through the just the derivation of it and then in a separate video, I'm going to talk about, um, I'm actually going to use some real sensors and and push, plug these equations in so you can see it actually work on the on the fly.
[3:38]So let's talk about the direct measurement of roll and pitch. So the idea is and let me grab a uh, I have a I have a small microcontroller in here with a uh, let me see, a small microcontroller over here that has an accelerometer.
[3:53]I'm going to use this in a separate video. So these are the circuit playground Expressos that we that I use in my instrumentation class.
[4:00]And so basically, the accelerometer, the Z-axis is is this way. And so if I hold the accelerometer like this, where gravity is pointing down, what I can, what the math I'm basically going to realize is that my gravity vector is 9.8.
[4:16]And as long as I'm not shaking this thing around, as long as the uh circuit Python is is is is steady, not level, but it's steady, because you need to assume that you're measuring gravity.
[4:20]You need to assume that you're measuring gravity. What the math I'm basically going to realize is that my gravity vector is 9.8, and as long as I'm not shaking this thing around, as long as the uh circuit Python is is is is steady, not level, but it's steady, I should be reading the gravity vector straight down.
[4:28]And so that's basically what this is. The gravity vector in the inertial frame can be written as [0,0,g]T. And if I normalize that, the gravity vector is 0,0,1, right?
[4:35]So that's basically all of all of the acceleration that the accelerometer is reading should be in the Z-axis. Now, the accelerometer is going to give me AX, AY, and AZ.
[4:47]It's going to give me accelerometer measurement in three axes of the body frame of the of the sensor. And what I can do is I can normalize that here, and so if I take, you know, the accelerometer measurement here, can you see that on there?
[4:56]Let's see, let me move this out of the way. So if I take my accelerometer measurement right here, and divide it by the norm, you know, I'll get a normalized acceleration vector here, I call A bar.
[5:07]And so what you can understand is that my gravity vector is always pointing down, and if I rotate about the yaw axis, you'll notice that that doesn't change the gravity vector in the body frame.
[5:22]And so what that means is is that that sai rotation, that yaw rotation, that third rotation, does not affect my gravity vector that I'm measuring.
[5:31]But if I rotate about the Y-axis or rotate about the X-axis, if I rotate about either axes, what happens is is that the gravity vector is now along different axes.
[5:44]And so if I if I pitch up and roll, my gravity vector is still down, but my three components, my X, Y, and Z in the accelerometer frame, are now different.
[5:55]And what you you can do that very easily. What you can do is you can take the gravity vector that you're supposed to be measuring, again, I said the the sensor needs to be steady, it doesn't need to be level, but it needs to be steady because you need to assume that you're measuring gravity.
[6:07]I can rotate this, rotate this through the uh pitch transformation matrix. And if you scroll up back to the Euler angle, um section, this is a 2D transformation through theta, the pitch angle.
[6:21]And then this is the transformation matrix through roll. And what you end up getting is this pretty simple equation with, you know, sin(theta), sin(phi)cos(theta), and cos(phi)cos(theta).
[6:31]And you can look at the top equation and you can see that that equation is one equation with one unknown. And so you can take the accelerometer measurement in the X-axis, or the normalized acceleration in the X-axis, and do an inverse sine and take the negative component, and that gives you theta.
[6:48]And then what you can do furthermore is you can take this second equation and this third equation and you can divide the two, and you'll notice that the cosine theta will cancel.
[6:59]Now, this is where that gimbal lock comes in. If cosine of theta is zero, it means your sensor is like this. And that means that you can't get the roll angle because what's happening is is your gravity vector is now straight down along the X-axis.
[7:14]And if I rotate or roll the vehicle or roll the sensor, it won't change my measurement. And so this these equations are only valid for benign maneuvers.
[7:25]So this would work well on a on a ship, um on a quadcopter that doesn't flip, uh it would definitely not work on a rocket or a spacecraft.
[7:37]So you have to keep that in mind when you're when you're using these equations. So uh as I was saying, if you if you divide these two equations, the cosine of theta will cancel and you'll basically get a tan a tangent of of phi, the roll angle.
[7:46]And so on the left-hand side, that's the normalized component in the Y-axis and the normalized component in the Z-axis, you divide those two, take the inverse tan, and that will give you the roll angle.
[7:57]So that is how you can get a direct measurement of the roll angle and the pitch angle just from an accelerometer. And as I said before, as I said, uh later on, I'm I'm going to uh actually put those equations on this circuit playground and and post that in a separate video.
[8:13]What I wanted to do is I wanted to to go on and and and move further and there's some other equations and supplemental stuff is talking about measuring yaw using a magnetometer.
[8:23]Now, this sensor unfortunately, the Circuit Playground does not have a magnetometer, so I am going to have to attach a separate uh magnetometer sensor to it, but the idea for measuring yaw is exactly the same.
[8:34]You assume that, and this isn't true, but you assume that the magnetic field strength is pointing north if you're in the Northern Hemisphere, and essentially along the X-axis.
[8:45]And if you normalize it, you'll get a vector that's [1,0,0]. Then what you do is you understand that the that vector, as I yaw the vehicle like this, is basically and let me see if I can if I can put this. So that's north.
[8:58]As I rotate the Circuit Playground or the sensor left and right, that's like yawing the vehicle, right, moving this way.
[9:11]I'll get a cosine and a sine component depending on which way I'm going. And so we can do the same thing where we can take the uh measurement of the magnetometer in the body frame and rotate that through the roll angle and rotate that through the pitch angle, and equate that, set that equal to the magnetic field of the Earth that we think it's supposed to be uh through this sai angle.
[9:34]And you can see that I can take this first row and this second row and I can divide those and get a tangent component. And I end up getting this pretty long equation here.
[9:43]Now, I've actually used this equation before um on a rocket and it it did okay. Um but it is the magnetometer is very sensitive to calibration errors and, you know, magnetic interference.
[9:56]So if you put a magnetometer near some rotating propellers, you're going to get some magnetic interference. And so I've seen this equation numerous times like on GitHub for and like on Adafruit or on Digi-Key or those other DFRobot.
[10:10]You know, everybody uses this similar equation and I basically just go through here and kind of derive it, um and hopefully you can follow along and and see if that helps.
[10:20]Um I also just to a a plugin here. I also have a a chapter here on how to get attitude estimation in low Earth orbit and you can read that through at your leisure.
[10:29]Um there's also some spacecraft position estimation using some ground station networks. And then finally, the last thing that I want to do, and I'm going to post another video on this, is about heading angle estimation using uh GPS.
[10:41]So you can get the heading angle of the sensor just by it rotating and use the magnetic field as your reference, but sometimes it's beneficial to say that, you know, hey, if I have a vehicle that's just moving forward through space, what I can do is I can basically fit a line between every successive GPS point.
[11:05]And so what I can do is I can take a GPS coordinate and then a follow a a following GPS coordinate and subtract the two and get a delta and say that is my heading angle.
[11:15]In order to do that, what you need to do first is take your GPS coordinates, your latitude, longitude, and altitude, and convert it to the Cartesian plane.
[11:28]And that, um that equation, let me go back up to the table of contents, external models, uh GPS coordinates to Cartesian, is uh this this equation here, where you take the the latitude of your measurement minus your origin point.
[11:40]So you have to assume an origin point for your basically your Cartesian flat Earth model. And then you multiply by this this kappa. And this kappa is converting a degree of latitude or longitude into nautical miles and then meters to feet.
[12:00]And I'm not sure if you know this, but um one degree of latitude is 60 nautical miles on the surface. And so that's where this uh this this 60 comes in.
[12:09]So you can see that here. 60. That kappa is 60 times feet over nautical miles, which uh is, let me see if you can scroll over to that, is uh 6007, you can't see that, hang on, let me zoom in there, you go.
[12:22]Is that 6076. And then the meters to feet obviously is the .3048. And so what you can do is you can take that delta latitude, let me move my mouse out of the way.
[12:34]You can take that delta latitude, multiply by kappa, and that will give you an X coordinate. And remember latitude is is is north to south. Um and then your longitude you you do your longitude and you also, you sorry, you do your delta longitude and you multiply by that kappa to get to meters or or feet in this case.
[12:54]Um but you have to take into account the fact that as you go up in latitude, you move across longitude more quickly. And so if your latitude is zero, then you're basically going around the equator, but if you're higher up on the poles, if your latitude, your longitude will change quicker, and so therefore you have to take that into account when you do your X and Y calculations.
[13:15]And then Z is this height above the surface, so that's your your altitude. So if you if you do that conversion where you go from your latitude, longitude, altitude and you convert it to your Cartesian plane, you can then and let me see heading angle estimation.
[13:32]You can then take deltas based on your successive measurements. And you basically do a DX over a DT, and that will give your velocity in the X direction, and then you do a a a Y I plus 1 minus Y I, Y I plus one is your measurement before, sorry, Y I plus one is your successive measurement and then Y I is your previous measurement.
[13:54]You take that delta and that gives you a delta Y, divide that by delta T and that gives you a VY. And then your heading is just the tangent inverse of your VX and your VY.
[14:02]Now, you'll notice by the way that when you do VX over VY, or sorry, VY over VX, you'll notice that the delta T cancels. So you could just do DY over DX and do a tan inverse and that will give you your heading.
[14:13]Now keep in mind, this only works if you're moving, right? So if you're moving in a certain direction, as you're moving in that direction, you can get your heading, but if you stop in place, that's really where the magnetometer uh sort of shines.
[14:26]The other thing that you can keep in mind is that your your magnetometer is going to be prone to a lot of those uh biases and things like that.
[14:35]And so sometimes it's nice to fuse the measurement of the magnetometer and the uh heading together or, you know, use the magnetometer when you're not moving and then the heading angle, or sorry, the GPS measurement heading angle when you're moving.
[14:50]And then not to mention, the IMU is going to update at like 10, 100, 1000 Hertz, and your GPS is only going to update, you know, every four, every two, three, four seconds. Uh so there's also that to keep in mind. Um so anyway, hopefully this was a uh interesting video about different ways to get your Euler angles from uh from these measurements.
[15:11]And then there was an entirely one more method where I didn't even mention, which is taking your rate gyro and integrating your rate gyro. You know, obviously integrating your rate gyro, you're going to end up integrating your biases, but that will also give you Euler angles if you have a really nice rate gyro.
[15:26]And then that AHRS filter, that attitude heading reference system filter that I talked about earlier, which estimated quaternions, that's basically a combination of all the two. You have the accelerometer that that measures your gravity vector.
[15:37]You integrate your gyro and you fuse the two, but it didn't include the GPS, which is why I wanted to talk about that here. So, uh, this is what I've been working on, um, putting this into my my FASTKit, uh, tool, tool set.
[15:50]So if you go back over to my repos over here and and you go over to, let's see, uh more repos, more, yeah, my repositories and you go over to FASTKit. This is my autopilot software that I've been working on.
[16:03]Um I've been integrating, uh, you know, GPS heading. You can see my latest commit. GPS heading, speed and magnetometer, yaw angle. I've been putting that in there. And so future videos, expect to see some stuff where I, uh, I test all this.
[16:16]And like I said, I'm going to do a a follow-up video where I I use that Circuit Playground and I and I move the the Circuit Playground around and get roll and pitch. So, hope you, uh, enjoy these future videos and see you in the next one.
