Abstract
Various attitude estimation methods are based on an optimization problem posed in 1965 by Grace Wahba, which is called Wahba’s problem in the field of attitude estimation. As a key for attitude determination, many different algorithms for minimizing Wahba’s loss function have been proposed in the past 60 years. Among the most representative are Quaternion Estimator (QUEST), Singular Value Decomposition (SVD), and Fast Optimal Attitude Matrix (FOAM), which are briefly introduced in this research paper, and new algorithms proposed in recent years such as Fast Linear Quaternion Attitude Estimator (FLAE), are also included. The calculation principle and derivation process are given in the article, and simulation under high noise conditions for algorithms mentioned has been completed. Finally, several practical engineering applications related to Wahba’s problem are introduced and the future development trend of attitude estimation is discussed.
1. Introduction
Attitude determination is a measurement technology based on relative positioning [1]. By observing two or more baselines at the same time, the three-dimensional attitude calculation of the carrier can be realized, which is widely used in aerial, marine, and land navigation. Generally, there are two methods to determine the three-axis attitude: (1) Deterministic algorithm like TRIAD. (2) Optimization algorithm such as QUEST or SVD. When using a double baseline to solve the attitude determination problem, deterministic algorithm and several optimal algorithms could all work, but various algorithms have different performance in specific application scenarios.
In the deep study of the optimal algorithm, many scholars have focused on reducing the computational power consumption of the algorithm for many years. This is mainly because of the limited computing power of early computers. Researchers have proposed a series of improved fast quaternion optimization algorithms such as FOAM (Fast Optimal Attitude Matrix), ESOQ (EStimator of the Optimal Quaternion), and ESOQ2 (Second ESOQ), which are mainly used to reduce the amount of calculation and improve robustness. In this period, researchers generally believe that quaternion have fewer constraints than calculating attitude matrix, which reduces the number of floating-point multiplication calculations required for each operation [2].
The early research of attitude determination algorithm mainly comes from solving the problem of satellite attitude determination. The attitude of the body in the inertial coordinate system can be obtained by calculating the installation position of the star sensor in the body. With the continuous progress of navigation technology, attitude determination is also widely used in the static coarse alignment of IMU (Inertial Measurement Unit) and the information fusion attitude determination of multi-source navigation system. The projection of gravity in two inertial coordinate systems is used to construct the observation vector and solve Wahba’s problem to determine the attitude matrix at the initial time. In recent years, the attitude determination algorithm has also been applied in the new field in machine vision, including image mosaic, visual measurement, and so on. The geometric constraints are constructed by using the image feature information to make up for the relative attitude determination or observation error of multiple unmanned platforms.
This paper aims to sort out the representative attitude determination algorithms proposed since the 1960s and derive seven different attitude optimization algorithms such as SVD, QUEST, ESOQ, and FLAE. In the first section, this paper briefly describes Wahba’s problem and introduces the development process of an attitude measurement algorithm. The second section of this paper focuses on comparing various optimization algorithms based on Wahba’s problem and summarizes their mathematical principles in a different expression of the loss function and the methods to obtain a final solution. Then the simulation is carried out, and the embodiment of each algorithm under high observation noise is explored at the simulation to show the performance of each algorithm with high observation noise. The rest of the paper briefly illustrates the application of attitude optimization algorithm in several typical scenes, like posture tracking, image mosaic, and alignment of moving base, and explores the application of attitude measurement algorithm in new scenes in the future. Finally, we summarize the advantages and disadvantages of each algorithm in the practical engineering application. The applicability and validity of other intelligent algorithms that may be used to solve the attitude determination problem are also mentioned in the conclusion.
2. Development of Attitude Determination Algorithm
The materials and methods section should contain sufficient detail so that all procedures can be repeated. It may be divided into subheadings if several methods are described.
In 1964, Harold Black proposed the algebraic method TRIAD (Tri-axial Attitude Determination) to obtain the attitude transform matrix by using two sets of observation vectors. In 1965, Grace Wahba proposed the least square optimization problem (often called Wahba’s problem) of star attitude determination using observation vectors. On this basis, many researchers carried out research on the optimization algorithm of attitude calculation.
The first solution of Wahba’s problem is composed by J. L. Farrell, J. C. Stuelpnagel, R. H. Wessner, J. R. Velman, 1 E. Brock, and R. Desjardins and Wahba gave it in 1966, but it is an immature algorithm, which requires at least three groups of accurate observation vectors to calculate the attitude conversion matrix, attaching an unnecessary burden to the computing power and observation strategy [3]. The first algorithm that can properly deal with Wahba’s problem at the technical level at that time was proposed by Davenport in 1968 [5], which chose to use quaternions to replace the attitude conversion matrix to reduce the number of unknown parameters, so as to solve the Wahba’s problem at a lower computational cost. In 1979, in order to complete the Magsat (Geomagnetic Satellite) mission, the QUEST algorithm was designed [6] and has been used until now, which is the most widely used attitude measurement algorithm. SVD was proposed by Markley in 1988 [4], but it was not widely used in practice at that time because of the large amount of calculations when the computational power was seriously limited. Using the adjoint matrix of vector observation matrix, Markley proposed the FOAM algorithm in 1993, which does not need singular value decomposition of the matrix either. The use of quaternion ensures that FOAM is at least as fast as the QUEST algorithm with employing Newton algorithm to calculate the maximum eigenvalue because the iteration and normalization could be much easier [7]. In 1997, based on Shuster’s proof, Mortari designed ESOQ [8] using Gibbs vector and used an analytical method to obtain the optimal quaternion, and then in the same year, the improved ESOQ algorithm launched ESOQ2 [9]. Compared with the former, the improved algorithm uses only one sequential rotation to avoid introducing a singularity, making the new algorithm more robust and fast.
In the relevant research before 2000, because of the limitation of the computing power of the computer and the extremely accurate observation provided by the star sensor in the application background, the improvement of algorithms was more inclined to be fast and simple, but did not pay attention to enhancing its robustness in the case of observation errors. In this century, with the rapid development of computer computing power, Wahba’s problem has also been applied in more fields. Yang used the general root of quartic equation to solve the optimal quaternion in 2013. The algorithm is fast, but there is still a problem. It may not have a real root of characteristic polynomial, which will lead to plural quaternion [10]. So, in 2015, Yang introduced the idea of Riemannian manifold and developed a more robust iterative method [11]. In 2018, Wu et al. named their proposed method the fast linear attitude estimator (FLAE) because it is faster than known representative algorithms [12]. In his method, Wahba’s problem is transmitted to several one-dimensional equations based on quaternions. Then the linear solution of the multi-dimensional equation equivalent to the traditional Wahba’s problem is established by using the pseudo-inverse matrix. The analytical method and iterative method for solving the eigenvalues are provided at the same time.
The main classical methods developed above are mostly based on Davenport’s Q-method, which needs to solve the matrix characteristic polynomial. In fact, some matrix operations (such as obtaining determinants and adjoint matrices) may be too complex for batch processing. We can also observe that it is difficult to achieve a balance between robustness and time consumption for all existing methods because fast methods are not always robust and vice versa.
3. Optimization Algorithm of Attitude Determination Based on Wahba’s Problem
3.1. Wahba’s Problem and Loss Function
The results and discussion may be presented separately or in one combined section, and may optionally be divided into subheadings.
The Wahba’s problem, in short, is to convert the direction measurement into attitude measurement. Its specific expression is as follows: if there are unit vectors in the inertial coordinate system which are recorded as , . The value obtained by measuring these unit vectors in IMU coordinate system (body coordinate system) is , . The problem is to find a rotation matrix to minimize the loss function.where represents weight factors for each obviation vector. , , are a set of positive weights satisfying , usually chosen as , with the variance parameters of the measurement vectors.
The rotation matrix has three degrees of freedom, and each vector measurement has two degrees of freedom. Therefore, when there is only one vector measurement, this is an under constrained problem; that is, there are countless rotation matrices that can realize . When there are two or more vector measurements, this is an over constrained problem, unless these vector measurements have no error at all; otherwise, . Besides, in Wahba’s problem, these vectors have been normalized, so we only need to consider the direction of , not its module length.
In mathematical statistics, there is a method to convert a quadratic form into the trace of a matrix, which is called “tracetrick.” The formula is
Substitute (2) into (1), then we get , where , .
After the transmission, the problem of obtaining the minimum value of the loss function related to the attitude conversion matrix is transformed into the problem of obtaining the maximum value of .
In addition, Davenport converts the direction cosine matrix into quaternion to establish new constraints. By introducing the Lagrange multiplier, the Wahba’s problem is transformed into the following equation:where,
In several different algorithms, Wahba’s problem is transformed appropriately so as to improve the performance, but they all adopt the same loss function, so each transformation form has a certain equivalence.
3.2. Introduction to Various Optimization Algorithms
3.2.1. SVD Algorithm
Calculate (1) as follows to obtain the reconstruction loss function in SVD algorithm:
Substitute the (5) into the original loss function we get
Because the observation has been determined, in equation (6) should be a known fixed value. So we need to calculate the maximum of to get the minimum of equation (6). Expanding it, we obtain
Suppose that matrix is reversible and there is a singular value decomposition , where , and . The eigenvalue decomposition of matrix is replaced by (7), and the following is obtained:
Set , then it is easy to know is the unit orthogonal matrix. Only when , could get its maximum.
3.2.2. Davenport
In 1968, Davenport provided a solution to Wahba’s problem by parameterizing the attitude matrix into a unit quaternion. When it comes to the transmission of Wahba’s problem, Davenport’s method is consistent with Markley’s SVD. After replacing the attitude conversion matrix with quaternion, we can getwhere
By utilizing Lagrange operator to calculate the maximum value of (10), we can get
If and only if is equal to the maximum eigenvalue of matrix , the eigenvector of is the optimal quaternion .
3.2.3. QUEST
QUEST algorithm uses Davenport’s quaternion method applied in optimization as well, as mentioned in paper [6], we could rearrange (12) as
Describe Y in terms of Gibbs vector then we get
Inserting (14) into (15) leads to an equation for the eigenvalues
Equation (16) is equivalent to the characteristic equation for the eigenvalues of . Considering the problem that the Gibbs vector becomes infinite when the angle of rotation is , a more accurate method which avoids the problems posed by this singularity was developed by Shuster. Derive an expression that permits the computation of without the intermediary of the Gibbs vector, for an eigenvalue of any square matrix satisfies the characteristic equation.
By the Cayley-Hamilton theorem [39], satisfies this same equation in the sensewe get a convenient expression for the characteristic equation, namely,where
In Shuster’s paper [6], it is considered that when is the minimum value, is a very close number to unity. In this way, the solving process of is significantly shortened and the Newton-Raphson method [39] applied to equation (23) with unity as a starting value allows to be computed to arbitrarily high accuracy.
Besides, Markley express Davenport’s eigenvalue condition as [40]
As mentioned in (15), we also could describe the optimal quaternion aswhere
Substituting (24) into (23) gives aquartic equation for the maximum eigenvaluewhich is just the characteristic equation of the matrix .The QUEST algorithm finds the largest root by Newton-Raphson iteration of (26) with the starting value and then solves (24) to find the optimal quaternion.
3.2.4. FOAM
As mentioned above, the SVD algorithm consumed a lot of computational power and was not popularized in the era at that time when first proposed because it involves the singular value decomposition of the matrix. However, the optimal attitude matrix calculated by singular value decomposition has excellent robustness, so in 1993, Markley expressed the singular value decomposition of the matrix by using the adjoint matrix, determinant and “Frobenius norm.”
For a matrix, its Frobenius norm could be defined as follows:
by calculating the adjoint matrix, determinant and f norm of matrix , its singular value decomposition can be obtained:
Substitute (28) into (9), we getwhere
It can be seen that the problem also focus on finding the parameter . From (3), we can obtain
Simplified equation(32), we get
Similar to the QUEST algorithm, the exact solution of (33) can be obtained by Newton-Raphson method. Finally, the optimal attitude matrix is calculated by (29)–(31).
3.2.5. ESOQ
ESOQ and its improved algorithm ESOQ2 emphasized the rapidity of the algorithm, which was the fastest attitude solution algorithm with the least floating-point operation at that time. ESOQ analyzes the characteristic polynomial of matrix and obtains its eigenvalue. In reference [8], Markley gave a method to solve Q by using four-dimensional cross product method and matrix inversion method, respectively.
First, move the right term of equation (12) to the left then obtain the matrix . Notice that . Where is the eigenvalue of matrix and at the same time. Therefore, all qualified can be obtained after the appropriate matrix transformation of matrix .where represents the matrix after deleting the th row and the th column. Four quaternions can be obtained from solving (34). These four quaternions should be completely parallel, only with different modules. In order to enhance the reliability of the results, the quaternion with the largest modulus is often selected as the optimal result, that is,
(34) and (35) is the four-dimensional cross product solution mentioned above. The matrix inversion method uses the adjoint matrix of to calculate the optimal quaternion. Parameter in equation (36) is determined by normalizing the quaternion.
The four-dimensional cross product method and matrix inversion method do not introduce matrix singularity, so using to solve is a more stable approach, and low demand of floating-point operations makes it suitable for fast optimal attitude estimation or huge observation.
However, the eigenvalue solved by ESOQ algorithm may be complex in the process of solving the maximum eigenvalue. Therefore, Markley believed this scheme was not suitable for solving (33) by Newton-Raphson method. In the improved ESOQ2 algorithm, Markley used vector transformation to calculate the maximum eigenvalue of matrix .
The improved algorithm is faster than ESOQ in speed, but ESOQ2 adopts some geometric approximations that are not always accurate, which causes the new algorithm to not be as stable as SVD, QUEST, and FOAM in practice, so we will not introduce it in detail in this paper.
Aiming at the problem that solving the eigenvalue of matrix K in ESOQ algorithm is not fast and stable enough, Yang optimized this process by using the analytical solution method of quartic equation newly proposed by Shmakov in 2012 [13]. After using the new solution to get , the optimized ESOQ algorithm has made full progress in stability while maintaining the excellent high-speed characteristics of ESOQ algorithm.
3.2.6. Yang’s Algorithm Based on Riemannian Manifolds
In 2007, Yang proposed a globally convergent geometric optimization algorithm based on Riemannian manifolds. The algorithm adopts the loss function (10) used by Davenport. On this basis, in order to avoid the calculation of all matrix eigenvectors, Newton-Raphson method based on Riemannian manifold is proposed to calculate the maximum eigenvalue and corresponding eigenvectors [15] so as to avoid the cumbersome process of obtaining all eigenvalues of . Yang’s Newton-Raphson method based on Riemannian manifold is summarized as the following four steps:(1)Establish the Newton vector in [14].(2)Project the vector onto the ttangent space of the sphere where the optimal quaternion is located. represents orthogonal projection to spherical space and . By calculating the Hessian matrix of the loss function (10), the Newton equation of the loss function can be obtained and the geodesic vector can be calculated.(3)Normalize the obtained vector .(4)Calculate searching along geodesic.
The attitude quaternion algorithm based on Riemannian manifold gives a robust iterative method to calculate the optimal quaternion, which is better than the query algorithm in numerical stability and robustness. However, the algorithm needs to consume too many floating-point operations in each quaternion iteration.
3.2.7. FLAE
Jin Xu University of Electronic Science and Technology of China believes that although QUEST, ESOQ, and other methods have made progress in robustness and time consumption, Davenport’s Q-method, on which these algorithms are based, requires too many matrix operations, including obtaining determinant and adjoint matrix, so it is difficult to achieve a balance between robustness and time consumption.
In the FLAE (fast linearized attitude estimator) method proposed by Wu, the quaternion is not used to transmit Wahba’s loss function, but to expand the attitude matrix directly and calculate it in the form of a column vector.
Then, using the transformation relationship between quaternion and attitude rotation matrix. We set
In this way, we can get the loss function used in FLAE.
By calculating the pseudo inverse of matrix , a homogeneous linear equation about Q can be obtained and the basic solution system of the equation can be given by elementary row transformation [37]. By Schmidt orthogonalization, the optimal quaternion can be calculated. In reference [12], the author gives the calculation equation in the case of multi-dimensional observation.where,
Similarly, the eigenvalue of the matrix is solved, and the optimal quaternion is obtained.
3.3. Simulation Verification of Multi-Vector Attitude Determination Algorithm
The mathematical equivalence of various attitude algorithms can be derived strictly. Strict mathematical derivation is complex and difficult. This section verifies the above algorithms from the perspective of simulation. The verification vector uses cases 1–6 set by Markley [7].
We set = [0.352, 0.864, 0.360; −0.864, 0.152, 0.480; 0.360, −0.480, 0.800]. Then calculate the loss function and misalignment angle error of QUEST, SVD, ESOQ, FLAE, FOAM, and Davenport algorithms, respectively, and count the time-consuming of running 2000 times. The computer running the simulation is equipped with Intel Core i5-8400 CPU. The results are shown in Tables 1–3.
During observation, set the signal-to-noise ratio of vector observation to 20 dB. From the above table, we see that the algorithms with poor robustness such as FOAM and ESOQ are difficult to solve the stable attitude when the vector observation is not accurate enough. Due to the improvement of CPU performance, there is little difference in computing speed between the algorithms in the case of fewer observation vectors. In the test environment set in this paper, SVD algorithm and FLAE algorithm have better performance.
4. Applications of Attitude Algorithm
4.1. Alignment of IMU for Moving Base
Before entering the navigation task, the strapdown inertial navigation system (SINS) needs to complete the initial alignment and establish an accurate initial attitude matrix. Alignment is usually carried out under the condition of a static base, but the rapid development of weapon system has higher requirements for this process.
To solve this problem, Yan et al. chose to reduce the gravity deflection error of inertial system caused by this through motion displacement compensation and presented SINS positioning method based on inertial reference datum [16]. Therefore, without any initial attitude information, data storage, or complex nonlinear modeling and filtering calculation, this method proposed in this paper not only realizes the initial alignment of the attitude array on the moving base, but also has the ability of real-time positioning and navigation in the alignment process [17]. On the basis of method in reference [15], Weng Jun attributed the attitude estimation problem of vehicle moving base to Wahba’s problem, which used FOAM algorithm with a small amount of calculation and good robustness to calculate the attitude conversion matrix from the local geographic coordinate system to the carrier coordinate system at the initial time, so as to solve the problem of real-time attitude estimation during vehicle driving.
In practice, the reference vector and measurement vector are calculated, respectively, from the measurements of the on-board IMU, odometer, and accelerometer. In addition to the FOAM algorithm used to obtain the attitude matrix, the other operation parts of the FOAM moving base alignment algorithm can be completed only by simple integral summation, and the filtering algorithm with relatively complex calculation would be much less. Experiments show that the azimuth error convergence speed of FOAM algorithm is fast and the anti-interference capability is strong, which meets the requirements of the vehicle system to obtain the correct attitude information quickly for a long time and can provide high-precision position information for the end of alignment to the integrated navigation stage as well.
Aiming at the problems of low precision and poor adaptability of traditional in-motion coarse alignment methods for vehicle strapdown inertial navigation system (SINS) caused by inaccurate measurement noise, an optimal indirect in-motion coarse alignment method aided by global navigation satellite system (GNSS) is proposed [41] as shown in Figure 1. Wahba’s problem is solved by SVD in the new method, and the vectors construction based on sliding fixed interval is used to weaken the accumulation of constant error algorithm. In the practical application scenario of low precision SINS system, the alignment algorithm proposed in this paper can achieve high-precision in-motion alignment without any initial attitude information.
4.2. Star Sensor Attitude Determination
The Wahba’s problem was first proposed to meet the requirements of star sensor to provide high-precision attitude information relative to an inertial reference frame for satellites in space. Most of the attitude determination algorithms are promoted according to the problems encountered in the application of star sensor by the optimizing of solution process and the improving of the representation of unknown variables. In the process of attitude determination using the star sensor, it is necessary to first obtain the star map image in a certain field of view, then extract the centroid of the star points in the image, and finally use the known navigation star catalog to identify the position of the star in the celestial coordinate system, so as to determine the star point coordinate conversion relationship between the image spatial coordinate system and the celestial coordinate system.
In the field of star sensor attitude determination, the attitude solution algorithm has been relatively mature, so more researches try to consider the influence of the number of star points distributed in the star map and focus on this point to improve the accuracy of attitude measurement. Chen et al. [19] pointed out that the problem can be regarded as an “Over determined Wahba’s problem” under conditional redundancy when the field of view of star sensor is more than 3 through experiments. Zhang et al. [20] introduced the deduction that the attitude measurement accuracy is not only affected by the relative position between the navigation stars described by the condition number, but also related to the different positions of the navigation star group in the star map. Song’s experiment shows that four-star-point method can obtain higher accuracy than the double-star method when applying QUEST algorithm. However, excessive star points will affect the calculation efficiency due to the real-time requirements of the star sensor [21].
Xiao combined the optimal recursive quaternion estimation (ReQUEST) [22, 23] method with the Cubature Kalman filter (CKF). The attitude quaternion determined by the optimal reQUEST method is directly used as the observation in the CKF filter, and the gyro drift is estimated by the CKF filter to compensate for the system error [24]. The method takes CKF filter as the outer framework and embeds the optimal ReQUEST algorithm into CKF filter. According to the observation of starlight, the initial value of K matrix is determined by ReQUEST algorithm. The gyro drift is compensated before one-step prediction and update the time according to the calculated volume points and corresponding weights to solve the prediction of state and variance.
When starlight information is input, the optimal ReQUEST algorithm is used to calculate the K matrix. The quaternion is separated from the K matrix, and the vector part is taken as the observation of CKF. Through simulation, it is proved that the measurement accuracy of quantity used in CKF is improved after embedding ReQUEST algorithm into CKF, so as to speed up the convergence speed and improve the filtering accuracy.
4.3. Image Mosaic
Wahba’s problem is to find the best rotation to align the vector observations corresponding to two sets of given assumptions, which also plays a very important role in current computer vision and robot applications. In Hengyang’s paper, Wahba’s problem is described by truncated least squares (TLS), and the problem is described as quadratic constrained programming (QCOP) in the case of quaternion. Finally, a QUASAR algorithm which is more robust than RANSAC algorithm is proposed [25]. When the latter uses surf feature description operator to establish the corresponding point relationship of different pictures for image stitching, the two camera frames can also be accurately stitched through quasar algorithm even though the overlapping area is small. New method’s effect is better than the common RANSAC, FGR, and other algorithms.
Similarly, the improved Teaser (fast and verifiable point cloud registration algorithm) based on Wahba’s problem also solves the fast solution when there are a large number of outliers in the point cloud, so it provides robust postprocessing for the scanning matching of laser radar and ensures the accuracy of target pose estimation and positioning [26].
In Nasim Kayhan’s paper [42], a content-based image retrieval (CBIR) system was presented which included two stages: feature extraction and similarity matching. The most efficient similarity/distance measure with respect to the weight of the extracted features was used at the similarity matching stage, which has a similar principle of image mosaic. That means the algorithms for multi-vector attitude determination used in QUASAR could be applied in this problem to obtain the weight of the extracted features.
With the rapid development and use of all kinds of accurate sensors, attitude algorithm appears in the form of assistance in all kinds of applications. Under the background of the vigorous development of intelligent navigation of unmanned system, there is a great contradiction between the power consumption, carrying and cost of unmanned system, and the use of precision sensors.
4.4. Human Posture Tracking in High Dynamic
Human body motion tracking is a key technique in robotics, virtual reality, and other human-computer interaction fields. Duan proposes a novel simple-structure Kalman filter to improve the accuracy of human body motion tracking, named the Second EStimator of the Optimal Quaternion Kalman Filter (E2QKF), by combining the Second Estimator of the Optimal Quaternion (ESOQ2) algorithm, the linear Kalman filter, and the joint angle constraint method in paper [27]. Besides the method designed by Duan, Yun, and Zhang proposed a new algorithm with a similar structure which employs EKF + QUEST, CF + ESOQ2 to realize real-time tracking of human body motion.
In the design of Duan, the measurements of the accelerometer and magnetometer were used as the input vectors for the ESOQ2 algorithm to produce the observation quaternion which aims to eliminate the error caused by the acceleration of human motion in the measurement results. The structure of the system is given in Figure 1. The traditional attitude calculation algorithm has low accuracy and is not applicable at high dynamic. But solving the attitude problem through observation vectors is not easy to diverge and does not produce cumulative error, becoming a decent compensation method for gyroscope. The combination of Kalman filter and attitude solution algorithm solves the problem of decreasing the accuracy of attitude solution algorithm in the case of high dynamics, and its high accuracy in the case of low dynamics can also modify the output of gyroscope through Kalman filter, so as to obtain the optimal attitude estimation in complex situations.
ESOQ2 algorithm is used in a combined system of optical motion capture and IMU/Magnetometer to calculate the human motion state to design a complementary filter (CF) with simple-structure [29].In order to eliminate the effect of limb motion acceleration on high-speed human motion measurements, the accelerometer compensation is added to ESOQ2 algorithm as shown in Figure 2. Finally, the fuzzy logic is utilized to calculate the fusion factor for a complementary filter, so as to adaptively fuse the input quaternion with the reference quaternion. The whole algorithm design is more simplified than the traditional method, and the efficiency and accuracy have been greatly improved at the same time.
Yun’s design is similar to the two filter mentioned above, which uses QUEST algorithm to preprocess the measured values output from accelerometer and magnetometer, then generates a quaternion for the filter as input [28]. This pretreatment reduces the dimension of the state vector and linearizes the measurement equation and gives the real-time implementation and test results of quaternion Kalman filter. The experimental results show that the designed filter can realize the accurate tracking of human motion and verify the feasibility of using QUEST algorithm to optimize the real-time tracking accuracy of human motion.
4.5. Information Attitude Determination for Cooperative Navigation
At present, the rapid development of artificial intelligence puts forward new requirements for UAV such as lightweight, autonomy, intelligence, and functional diversification. As a new type of working mode, UAV cooperative operation has attracted extensive attention. The cooperative reconnaissance, search, detection, and positioning by multiple unmanned platforms are typical application of unmanned system in practice which requires enough accurate relative attitude determination and positioning in the aircraft cluster as basis. With the application of visual navigation system to UAV formation control, autonomous aerial refueling, and spacecraft autonomous Rendezvous and Docking (RVD), it has become the focus of many scholars’ research. Visual vector information is widely used in relative navigation, especially in relative attitude determination [35] which draws much attention in recent years.
Zhang et al. proposed a relative attitude determination algorithm of two aircraft formation UAV considering geometric constraints in paper [30, 31], where they establish a solution model for relative attitude problem by using the geometric relationship of triangle formed by vector observation. The solution does not need the position of the target but only the line of sight from the aircraft to the target. So the error caused by the position measurement is avoided and the problem of unobservability caused by coplanar sight vector is solved by using the triangular geometric constraint relationship between aircraft. The relative attitude accuracy could be improved as well. This paper adopts leader-follower mode as the dual aircraft formation control strategy as shown in Figure 3. For any target points out of these two aircraft, enough line of sight vectors can be observed to calculate the relative attitude without knowing the position information of the target, as long as the sight vectors between aircraft and target could form a triangular constraint.
5. Conclusions
This paper briefly introduces the performance and simple derivation of several algorithms to solve Wahba’s problem such as QUEST, SVD, ESOQ, FLAE, FOAM, and Davenport, and carries out simulation verification which is running on the current popular personal notebook. Low signal-to-noise ratio (SNR) is set to simulate the situation of poor measurement accuracy. Theoretical analysis and simulation experiments show that when the vector observation is not accurate enough, the attitude solution algorithm should first pay attention to its robustness because the generation of singular value will have a serious impact on the solution time and accuracy. In the engineering application like visual vector attitude determination and moving base alignment, the environment to obtain vectors is very different from algorithm’s original scenario: star sensor attitude determination. With the accuracy of observation vector decreasing significantly and rapid development on computer performance, the future development trend of multi-vector attitude determination is inclined to enhancing stability and fault tolerance instead of speed only. Besides, algorithm’s versatility still needs to be developed. The researchers at the State University of New York believe the algorithm of attitude determination is widely used in various engineering projects, but its improvement is often aimed at a fixed background [43]. Although those optimization schemes have the effect of improving the accuracy of the solution, the constraints that depend on a specific problem are not portable after all.
Therefore, in order to better apply the attitude solution algorithm in different fields, we first need to improve the performance of the algorithm aiming to specific observation conditions. Secondly, the improvement of the algorithm should go deep into the mathematical level rather than relying on a single task scenario to set constraints, so as to ensure the universality of the new optimization scheme. In addition, some of the most representative computational intelligence algorithms can be used to solve the problems, like monarch butterfly optimization (MBO) [44], earthworm optimization algorithm (EWA) [45], elephant herding optimization (EHO) [46], moth search (MS) algorithm [47], slime mould algorithm (SMA) [48], hunger games search (HGS) [49], Runge Kutta optimizer (RUN) [50], colony predation algorithm (CPA) [51], and Harris hawks optimization (HHO) [52]. The excellent performance of swarm intelligence algorithm to search for the optimal solution makes it possible to solve the attitude determination when dealing with more complex tasks [18, 32–34, 36, 38].
Data Availability
The data for simulation used to support the findings of this study are included within the article.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This paper is concerned with research into the altitude determination of drones in cooperative sworm. It is a part of the Natural Science Foundation of China project(62103424): The simulation and algorithms for attitude determination of drones using relative observation.