So now the triad method was a nice way to go, here's an observation. A unit direction vector, here's another observation, a unit direction vector. And yes, Tibo, they're not orthogonal, I'm sorry, they're not aligned in some way. They have to give you a unique plane, then you can do this stuff. Now we want to look at, what if we want to use all of the information? This was fast, this was easy, makes it work, has flown. But what if we wanted to know everything, we want to use everything? We have some information an that we left on the table. And particularly, there's all kinds of systems these days flying with multihead star tracker systems. You not only have one star tracker, you have multiple star trackers. because as you're rotating, as Kenneth Spencer was highlighting earlier, sometimes your sensor is staring at the Earth. What star tracker doesn't have exact x-ray vision to look through the Earth and see where the stars are behind the Earth. So it's blocked, it's a fantastic expensive sensor giving the absolutely zero right now. So you want to make sure you have a second sensor. But normal on operations hopefully, you're flying in a way where both star trackers are useful. And now you can blend these informations, or even with this magnetic fields we have horizon sensors. There's all kinds of ways we can get headings. Even with relative motion if you think of this stuff, there's fiducials, computer science using this stunts too. So fiducials are visual trackers that you're doing. If I'm looking in my image plane and I'm tracking this point and this point and this point. Each one of those points that I'm tracking is a unit direction vector in that camera frame. How do I reconstruct now the full attitude, right? And the same algorithms work for this kind of stuff. So this is a fundamental thing, more and more you're finding them actually built into the solutions which is kind of nice. But if you have to write them yourself, this is what we're going to do. So how do we deal with multiple observations? I now have N number of observations and this is an interesting thing. So assume we have N observations, N bigger than 1, because we know N1 is not enough. I need at least two, the two is immediately making it over determinant. But you may have three, you may have five, especially with visual cameras sensors. You're tracking stars, if you're doing a star track, mathematics inside. You're tracking five, six, ten stars, that's ten headings, you have to reconstruct all of a sudden. So we have N as a large number fundamental, or it could be a large number. How do we now do this? And Wahba, she's a mathematician who did this back, many decades ago now. But she post this at a conference and said, well for this estimation problem. Fundamentally it always breaks down to this, we have N number of observations measured in the body frame. I know what these observations are in the inertial frame. Again assuming we know where my location is, and I know what my environment is, otherwise this doesn't work. How do I find the optimal attitude description that maps all these known quantities into these measured quantities? I mean known is with uncertainty and measured is always with uncertainty as well. So how do we do this? And she came up with this cost function here. It's essentially a least squares fit function. We're looking at the residual, if this were the matrix that maps this perfectly into this, then vb minus BN v in the N frame, that's going to give you 0. 0 squared is 0 times weight still 0, summed up is still 0. So if you had a perfect measurement, no noise, no corruption, no ignorance and you map it up together. This cost function gives me 0. You had a question? >> Why scale it by a half? Why would that make a difference? >> Laziness, right? This is what we're after, laziness in this class. Because if I don't do a half, you will see in a moment. If you have a function you want to optimize, if you have y of x, find the minimum of that function, you take the derivative of that. And if you have something squared, taking the derivative. I get two times that, and there's a two factor write out. And I'm really lazy, so I put a half in front of it. That's honestly, if you look at the papers, there's mathematical language and very flowery. And why this makes sense, but the bottom line is we're lazy, that's really what it's about. So let's talk about these terms. So if it were perfect we would get this, but we know with measurement errors. This is not going to have five observations that are perfectly going to match from known quantities to the measured quantities. There's going to be some things off. They're not completely compliant. So this is going to be non zero. So we take the errors, [COUGH] norm them. That's a vector, we did the norm, L2 norm. We square that, that's basically the dot product between these two things, right? And we sum them all up, that's a least squares error measure, like you're doing a least squares estimation. It'll quickly come in here. The next thing we could introduce is weights. That's really important because we know, for example, a sum sensor is way better than an emingnetic field sensor, now what is way better mean? This is were you control it with the weights, do I trust the sound sensor 5 times more than the magnetic field sensor? Then you just have to make sure the weights are such that. The weight of the sun sensor is 5 times bigger than the weight of the magnetic field sensor. The actual value of the weight doesn't matter. So you could make magnetic field sensor 100 and then the sun sensor 500. Or you could make one 5,000ths, and then the other one 1,000ths. The absolute value doesn't matter, it's the relative value of the weights. So again laziness, the easiest way to have is what? One, exactly. Yeah, we just pick one and then everything is relative to one. So if something's better, I would go, that's good, that one's one. These one's twice as good or twice as bad. You can go to 0.5 or 2 depending on which one you're scaling on. They're all equally good and in homeworks, that's kind of what you do. You just set all the weights to one. Since absolute values don't matter, this is what allows us to throw a one-half in front of this and just avoid a factor of two afterwards. If you're optimizing a cost function and you multiply the cost function times some positive scaler, you don't change the optimum location of it. So it's just a convenience. But now this is Wahba's problem that she posed. And if you go Google Wahba's problem, it's amazing to this day, decades and decades later. People are still solving Wahba's problem in novel and interesting ways. And I will show you some classic methods. There's Davenport's q-method which was very nice, kind of groundbreaking in what it does and it enabled other methods. There's QUESTs method. The QUEST method flown probably that's the most popular thing flying right now. And it was done by Malcolm Shuster and there's lots of add-ons that were done around QUEST. And then there's something very recent called OLE method, it's a different kind of a thing. And also dosens kind of optimisation, but in a different way. So those are the three that we're going to focus on today and the rest. But this is it, so the least squares fit. If these mathematics look weird, everybody has seen this kind of a least squares problem. We've all taken measurements in labs somewhere, and it's supposed to be a straight line, you never get a straight with line measurements. And then you have to fit somehow a least squares behavior to it. That's what we're doing with the attitude measure. But we have to come up with a way to fit this three by three DCM that was embedded in this function, right? And that's kind of tricky. And so how do we do this? And that's what lots of papers to look at. So I'm going to just start here for a few minutes and then we'll continue this. But I wanted to set up at least the idea. So embedded in here is this DCM. And tracking this in a DCM form here to project inertial quantities into body frame quantities, the DCM makes perfect sense. But I have nine coordinates, I have to estimate nine coordinates that could do this. And it's not just estimating nine coordinates, it's estimating nine coordinates draw subject to how many constraints? Thank you. See? Okay, so I'll never forget that anyway. >> I know. [LAUGH] >> I'll let go. But that's what makes it really tedious, right? We have nine coordinates. It immediately becomes a highly constrained optimization process because of the DCM. So is there an easier way? And this was done back many decades before all these modern computing techniques, and this is Devenport's q-Method that was published. It's a really beautiful, beautiful solution, but it has some challenges that you will see. So instead of going directly for the DCM, we can write the DCM in terms of an infinity of different attitude coordinates. Let's see if there are other coordinates that make our life easier. In Davenport's q-method, he was dealing with quaternions here. So q is a common name often for quaternions. The q is also a common name for CRP, so we just have to kind of pay attention as to what we're discussing here. Here, this is quaternions, and I've rewritten in terms of the betas that we have. But sometimes you'll find formulas where it's in terms of qs and this could be q0, q1, 2, and 3. And some papers have q1, 2, 3, and then q4, right? It's the 4 through 0 that is the same, that's the scaler part and it just kind of flips things around a little. So depending on the Voksen papers, just something you have to pay attention to. I rewrote this to be consistent with how we've been doing it in this class. So if we do this, we have a cost function and we can start to expand this cost function. We need the norm of this, and the norm with the A vector squared is the same thing as the A vector dotted with itself. Where in matrix form it's A transposed A gives you a norm squared, that's essentially what we're doing. We don't just have A, we have VB minus BN VN so it's this times this, now we just do start multiplying out. You have a VB transposed with VB, a VK transposed this whole thing flips the order times this, that gives you BN transposed BN. Which is going to be simply identity and that leaves you with this. And then there's some VB transposed with this. Here, two terms that have to be rewritten into the same form. Remember, this is a scalar. And so when we have some of this math, when you carry this out yourself, this is something you should be able to do, especially in exam. You will get this form and one form that's flipped. But in a whole series of matrix math gives you a scalar answer. You can always transpose that matrix math because the transpose of a scalar is still the same answer, right? So you can often transpose every element and reverse the order, and that's how you end up getting this too. So I'll let you do that on your own, but that's there. Now if you look at this, this becomes very simple. A unit vector dotted with itself is just going to be 1. A unit vector dotted with itself is going to be 1. 1 plus 1 is 2, that's already a factor 2 here, and there's a half, wow. I love this, right? All those twos vanish and we just end up with this form. So this is just a classic first step, how to rewrite. We still have Wahba's problem, but we've rewritten it in a form that'll be a little bit easier to optimize. And that's where we'll pick up next time. So we'll start up from here, okay? Good, I'll see you on Tuesday. Have a good weekend.