1,212
edits
Changes
→Q-Method
TRIAD method is useful when we have only two vector measurements. If we need better accuracy and have more than two vector measurements, we need to use a more general algorithm. Before developing an algorithm, we need to make our problem statement more precise.
Say we have N unit vectors <math>v_k</math>, k = 1,2...N. We have a sensor measurement in the body frame for each vector, <math>v_{kb}</math>, and a mathematical model of the components in the inertial frame, <math>v_{ki}</math>. Our aim is to find a rotation matrix <math>R_{bi}</math>, such that<br \>
[[File:Equation25.png|frame|center]]
for each of the N vectors. It is clear that this set of equations will be overdetermined for N > 2,
and hence the equation, in general, cannot be satisfied for each k = 1,2..N. Hence,
we wish to find a solution for <math>R_{bi} </math> that in some way minimizes the overall error for
the N vectors. <ref>http://www.dept.aoe.vt.edu/~cdhall/courses/aoe4140/attde.pdf</ref><br \>
[[File:Equation72.png|frame|center]]
In this expression, J is the loss function that has to be minimized, k is the counter for the
N observations, <math>w_k </math> is weight assigned to the kth measurement, <math>v_{kb} </math> is the matrix consisting of the measuredcomponents in the body frame, and <math>v_{ki} </math> is the matrix consisting of components in the inertial
frame as determined using appropriate mathematical models. This loss function is a
sum of the squared errors for each vector measurement. If the measurements and
mathematical models are perfect, then J = 0, since the first equation on this page will be satisfied for all N vectors. If there are any errors or noisy measurements, then J > 0. The smaller
we make J, the better the approximation of <math>R_{bi}</math>.
== Q-Method ==
We can see that in order to minimize J, we need to maximize g(R). This problem can be written in a better and more elegant way by representing rotation matrix R in terms of the quaternion
[[File:Equation29.png|frame|center]]
R can be written in terms of q as<math>R = (q_4^2-q^Tq)I+2qq^T-2q_4[[File:Equation30.png|frame|center]q \times]</math><br \>
Where [q x] is the cross product matrix associated with q
[[File:Equation31.png|frame|center]]
One can verify that the gain function in terms of the quaternions is:
Where K is a 4x4 Matrix given by
[[File:Equation33Equation77.png|frame|center]]Wherein,<br \><math>B = \sum w_k(v_{kb}v_{ki}^T)</math><br \><math>S = B+B^T</math><br \><math>Z = \sum w_k(v_{kb} \times v_{ki})</math><br \><math>\sigma = tr(B)</math> <br \>[[File:Equation34.png|frame|center]]Now we want to maximise g. However, noting that quaternion elements are not independent, the constraint [[File:Equation35.png]] <math>\bar{q}^Tq = 1</math> must also be satisfied. So we use the lagrangian multiplier method and differentiate with respect to [[File:Equation36.png]] <math>\bar{q}</math> we get [[File:Equation37<math>K\bar{q} = \lambda \bar{q}</math>.png]] So now, we have reduced the problem to an eigenvalue problem for the matrix K. Since, K is a 4x4 matrix, it can have at most 4 different eigenvalues. Substituting [[File:Equation37.png]] <math>K\bar{q} = \lambda \bar{q}</math> in [[File:Equation38.png]]<math>g(\bar{q}) = \bar{q}^TKq</math>, we get
[[File:Equation39.png|frame|center]]
Now we want to maximise g. So we will chose the eigenvector corresponding to the maximum eigenvalue.