I'm just going to work through the Mathematics. I'm not going to do this by hand. I'm going to show you in mathematic notebook, how we do this, because there's all subtle details, because in the homework, especially in the next homeworks, you're going to actually be running this stuff. I'm giving you numbers, there is some key steps you want to do. So here, the book comes with this rigid body kinematics library, I'm going to use it. It doesn't contain a tilde operator so here Mathematica. Its like an M file in Math Lab if you're familiar with that or death in Python. I'm just defining until the operator. So let me execute these, so that's all good. Now, what I'm doing next is in estimation if you do this mathematically, you need the truth. And the beauty in the math world is, we can make up truth. We can say, the true orientation will be in this case a 30 /20 -10 degree 321 euler angle set. That's what I am defining, right? And I am going to use that, use our mappings, to go from euler angles to go from a DCM. So, I am generating BN true and that will give me this matrix and I'll do that step. That's kind of done. The next set here is V1N. This has two observations, like S and M, but instead of calling S and M, it's just V1 and V2. The first observation and the second observation and the first observation in the initial frame is pointing at 1,0,0 just keeping that simple and the second one is 0,0,1. I need some unit vectors. So those are defined and now the true measurements you should have is this is, these vectors and N frame components times this DCM should give me what I would measure at this orientation. Again, assuming you've sensors where you measure using other orientation. So these are the truth, and at least are given to four, five to six digits. But those are the true measurement you would have. Now, when you're doing this stuff, we have to corrupt it. And you can see here I'm taking the true measurements instead of 0.8137 and so forth, I'm making it 819. The third digit, I just randomly made something different. If you do the research you would just by hand do this, you would have some random noise generator with some colored noise, white noise, whatever you need. You would artificially corrupt the truth, right? because then you could look at it statistically how well am I estimating this stuff? I just did a simple hand example. So that's where I'm taking this true measurements and I'm tweaking them but a critical step here is we have to normalize them. Don't forget that step. Especially coming from homework assignments. I am giving you these vectors and observations. I am only giving you four or five digits. That's not very precise. Just take those and as the first step, normalize these vectors. And now you are guaranteed it has unit length and all this math will work out. Because otherwise, as we are putting these vectors into your DCM. You have something that is only four digits to unit length vector and your DCM won't be exactly orthogonal, right. So make them to machine precision normal vectors and your life is much easier. So that's a critical step I wanted to show there. So now we do the triad frame. We set t1 equals to v1. V1 was my better measurement. V2 was the ugly cousin that, the cheap one, that was a second measurement. So we have t1 = v1B, but everything's in B frame components which will give me these three vectors. I'm just going to go evaluate this. So this is t1, t2, t3, its within some precision that is showing me on the screen that I quickly compute with the first setting in equal and then a cross product and then another cross product of T1 and T2. But every one of these matrices represent a vector in B frames now, right. Now, from the known measurements, I can do the same thing. T1 in the M frame is V1 M, it's going to be equal to this then a cross product gives you this and then another cross product gives you the third one. This sets up the triad frame, same t1, t2, and t3 but in N frame components, following our slides. So now to find our corded frame, we have to set up the B part D that we had, let me just show that. All right, that was here. It has to be the first column, second column, third column. So you want to make sure you don't get the transposes wrong and here's my mathematica. So if I'm doing this here in Mathematica I can easily add them as rows not columns so I'm adding my first row, second row, third row but then I transpose in the answer that makes them first, second, third columns essentially. And so that's my B T. N bar t, the same thing and now all you have to do is multiply one matrix times the other with the right transpose and that's it. This is now my estimated attitude. So, good, now how good is this? If you had a DCM, if we go up above, this was the true DCM that we have. 813798 and down here, we end up with 818799. It's close, not exactly the same, right? How good is this estimate? >> And DCMs are horrible to give you that. because you get the numbers and who knows, I don't know it's kind of close, is that good? Is that bad? So we don't use DCMs to quantify this how close that estimate was to the truth in this case. What we do instead is we want to get the relative attitude. What is the attitude of b relative to br? Right. The true B was here. The estimated b wasn't exactly the same. It's tweaked a little bit. So in DCM space, I just need to know what is B to B bar or B bar to B. That's the relative DCM that we need. And we can do that again, matrix multiplication. So I've got B bar to N and BN true. Transpose one that gives me B bar to B and if I evaluate this, now I get something that's actually for me easier to look at because I got a matrix. But this is close to what type of matrix now? It's close to an entity. Why is that expected? The identity means what in DCM space? >> Zero rotation. >> Zero rotation, they're identical. So your estimation, if you do good You should expect at this point something that looks darn close to an identity matrix, right. And that's what we get. So, that's good but I still can say, is it really close, or just close. I am an engineer, I want numbers, how close am I in this stuff? And so, what we're going to do here, the c to prv takes my DCM to principle rotation vector, that was the p times e had one, the gamma one we had in our book. That is what it is and the norm of it, and I'm converting to degrees just from here we have to extract a principle rotation angle. Different approaches how could have used that. But that's what I'm getting. This number, that's where that principle rotation angle is handy, because the actual B frame and estimated B frame are close to within 1.85 degrees. An estimation theory you don't typically care if you're off one degree about B1, or one degree about B2 and so forth. It's just I'm off by one degree, and there's a whole series of rotations that could happen. So that gives us our error in this estimation technique. Yes sir. >> Did you say that that rigid body kinematics thing came with the book? The lightyear toolbox? >> Yes. >> What if we bought it used? >> There's a website at the back that you could download it or, if you actually look at my website, and as one of my students I encourage you to do that. There's a library. I always keep the latest ones up there so you can get the Mathematica, MatLab, there is C versions of Rigid Body Kinematics and some other libraries. So if you just go to my website there is a numerical tool section. I keep the very latest one there. With AA, I think last year I gave him the latest ones but I don't do it, monthly, these I kind of tweak. See on my website, you're welcome to just go there and you can find some of them. Again, for the homeworks you're welcome to use this, perfectly fine, unless the homework says create a sub routine that takes all other angles to DCM. Don't just give me back my sub routine from my book believe me I've seen it, I've written the bloody thing I will recognize it, do in your own style. That's the purpose, if it do this estimation stuff, you're welcome to use these tools absolutely. Or write your own we both have Interesting, you know, you can learn from both. Cool. But I want to just step this through, and we're going to do this for other methods, but that normalization of these vectors is really key, otherwise you end up with something that's going to quickly cause issues. If you measure only to 8 digits but enter in your computer, you're holding 16 digits, take that measurement and normalize it again. That way, you're making sure this DCM has a proper orthogonal behavior that you would expect, or you're going to be stretching and scaling these vectors in ways you didn't necessarily want them to be.
