Jin Wu

Zebo Zhou

Hassen Fourati
email: hassen.fourati@gipsa-lab.grenoble-inp.fr.

Rui Li

Ming Liu

Generalized Linear Quaternion Complementary Filter for Attitude Estimation from Multi-Sensor Observations: An Optimization Approach

Keywords: Sensor Fusion, Attitude Estimation, Complementary Filter, Quaternion Optimization, Robotics

whether they are published or not. The documents may come   L'archive ouverte pluridisciplinaire

Generalized Linear Quaternion Complementary Filter for Attitude Estimation from Multi-Sensor

Observations: An Optimization Approach Jin Wu, Member, IEEE, Zebo Zhou, Hassen Fourati, Rui Li, Member, IEEE and Ming Liu, Senior Member, IEEE

Abstract-Focusing on generalized sensor combinations, this paper deals with attitude estimation problem using a linear complementary filter. The quaternion observation model is obtained via a gradient descent algorithm (GDA). An additive measurement model is then established according to derived results. The filter is named as the generalized complementary filter (GCF) where the observation model is simplified to its limit as a linear one that is quite different from previous-reported brute-force computation results. Moreover, we prove that representative derivative-based optimization algorithms are essentially equivalent to each other. Derivations are given to establish the state model based on the quaternion kinematic equation. The proposed algorithm is validated under several experimental conditions involving free-living environment, harsh external field disturbances and aerial flight test aided by robotic vision. Using the specially designed experimental devices, data acquisition and algorithm computations are performed to give comparisons on accuracy, robustness, time-consumption and etc. with representative methods. The results show that not only the proposed filter can give fast, accurate and stable estimates in terms of various sensor combinations, but it also produces robust attitude estimation in the presence of harsh situations e.g. irregular magnetic distortion.

Note to Practitioners-Multi-sensor attitude estimation is a crucial technique in robotic devices. Many existing methods focus on the orientation fusion of specific sensor combinations. In this paper we make the problem more abstract. The results given in this paper are very general and can significantly decrease the space consumption and computation burden without losing the original estimation accuracy. Such performance will be of benefit to robotic platforms requiring flexible and easy-to-tune attitude estimation in the future.

I. INTRODUCTION

T HE development of consumer electronics brings a world- wide mania in cellphones, smart wearables, interactive devices and etc. [START_REF] Chan | Smart wearable systems: Current status and future challenges[END_REF], [START_REF] Fourati | Heterogeneous data fusion algorithm for pedestrian navigation via foot-mounted inertial measurement unit and complementary filter[END_REF], [START_REF] To | Quaternionic Attitude Estimation for Robotic and Human Motion Tracking Using Sequential Monte Carlo Methods with von Mises-Fisher and Non Uniform Densities[END_REF]. Such electronic products indeed improve the quality of our living. As a matter of fact, each product is a mixture of many recent technological advances. Among all these techniques, the sensor fusion is of importance since it gives state estimation of the body's motion from multi-sensor observations [START_REF] Goslinski | Performance Comparison of EKF-Based Algorithms for Orientation Estimation on Android Platform[END_REF], [START_REF] Michel | A comparative analysis of attitude estimation for pedestrian navigation with smartphones[END_REF], [START_REF] Yuan | Event-Triggered Strategy Design for Discrete-Time Nonlinear Quadratic Games With Disturbance Compensations: The Noncooperative Case[END_REF]. Attitude estimation, as part of the overall sensor fusion module, plays a big role in detecting object's attitude and further produces signals for gait analysis, gravity sensing and etc [START_REF] Wu | Dynamic Magnetometer Calibration and Alignment to Inertial Sensors by Kalman Filtering[END_REF], [START_REF] Zhou | Optimal batch distributed asynchronous multi-sensor fusion with feedback[END_REF]. The attitude estimation techniques will provide key bases for later robotic estimation and navigation [START_REF] Liu | Robotic Room-Level Localization Using Multiple Sets of Sonar Measurements[END_REF].

Not only consumer electronics, but many other professional applications also have a high demand on attitude estimation [START_REF] Konigseder | Attitude Estimation Using Redundant Inertial Measurement Units for the Control of a Camera Stabilization Platform[END_REF], [START_REF] Lee | Sliding-Mode-Based Missile-Integrated Attitude Control Schemes Considering Velocity Change[END_REF]. For instance, one satellite needs to be stablized on the orbit and to achieve this, the attitude estimator should give accurate estimates for the control task [START_REF] Li | Multi-group coordination control of multiagent system based on smoothing estimator[END_REF]. Moreover, such applications can also be operated under harsh external conditions i.e. strong vibration, sudden external acceleration, irregular magnetic distortion and etc. [START_REF] Zhou | Integrated Navigation System for a Low-Cost Quadrotor Aerial Vehicle in the Presence of Rotor Influences[END_REF], [START_REF] Makni | Energy-aware Adaptive Attitude Estimation Under External Acceleration for Pedestrian Navigation[END_REF], which makes the attitude estimation more challenging [START_REF] Li | Effective Adaptive Kalman Filter for MEMS-IMU/Magnetometers Integrated Attitude and Heading Reference Systems[END_REF], [START_REF] Wu | A Novel Approach for Attitude Estimation using MEMS Inertial Sensors[END_REF], [START_REF] Wu | A New Technique for INS / GNSS Attitude and Parameter Estimation Using Online Optimization[END_REF]. In engineering practice, a navigation system should have sufficient redundant computation resources to ensure the robustness of the system when emergence happens [START_REF] Meier | PIXHAWK: A system for autonomous flight using onboard computer vision[END_REF]. For example, an unmanned aerial vehicle (UAV) system needs to act quickly for failsafe when some incidents take place such as motor failure, GPS outage, main controller failure [START_REF] Marantos | UAV State Estimation Using Adaptive Complementary Filters[END_REF], [START_REF] Zhou | Equality constrained robust measurement fusion for adaptive kalman-filter-based heterogeneous multi-sensor navigation[END_REF]. This in a degree requires the navigation part to be computationally cheap.

In fact Kalman filter (KF) [START_REF] Kalman | A New Approach to Linear Filtering and Prediction Problems[END_REF] is an optimal filtering approach in the sense of minimum mean squared error (MMSE). Although other filtering approaches e.g. H ∞ filter [START_REF] Yuan | Distributed quantized multi-modal Hfusion filtering for two-time-scale systems[END_REF] have been widely spread, KF still remains its definite coverage in industrial applications. The attitude estimation can be efficiently achieved via KF-related algorithms [START_REF] Valenti | A Linear Kalman Filter for MARG Orientation Estimation Using the Algebraic Quaternion Algorithm[END_REF]. However, conventional KF methods still have some drawbacks:

1) Classical KF requires the state and observation models to be linear. Besides, the noise sources for the two models should be white Gaussian and uncorrelated.

2) Although many techniques have been developed to deal with non-linearities e.g. the extended Kalman filter (EKF) [START_REF] Sabatini | Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing[END_REF], the unscented Kalman filter (UKF) [START_REF] Crassidis | Unscented Filtering for Spacecraft Attitude Estimation[END_REF] and etc.

[26], they will significantly increase the computational burden. 3) In principle, a 3-order KF can obtain good attitude estimates [START_REF] Markley | Attitude Error Representations for Kalman Filtering[END_REF]. For engineering requirements, some KF-based attitude estimators are designed to accommodate highdimension observation models [START_REF] Li | Effective Adaptive Kalman Filter for MEMS-IMU/Magnetometers Integrated Attitude and Heading Reference Systems[END_REF], [START_REF] Suh | Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of external acceleration[END_REF], which makes the filtering time-costly.

Complementary filter (CF) approaches are popular alternatives for low-cost platforms with need of attitude estimation [START_REF] Kottath | Multiple Model Adaptive Complementary Filter for Attitude Estimation[END_REF], [START_REF] Kottath | PSO Aided Adaptive Complementary Filter for Attitude Estimation[END_REF], [START_REF] Johansen | Nonlinear Observer for Tightly Integrated Inertial Navigation Aided by Pseudo-Range Measurements[END_REF], [START_REF] Chang | A Two-Step Approach for an Enhanced Quadrotor Attitude Estimation via IMU Data[END_REF]. Associated thoughts have been proposed for many years [START_REF] Higgins | A Comparison of Complementary and Kalman Filtering[END_REF]. Recent advances mainly focus on the combination of magnetic, angular rate and gravity (MARG) sensors i.e. the magnetometer, gyroscope and accelerometer. Marins et al. uses the Gauss-Newton algorithm (GNA) to obtain the quaternion solution to the accelerometermagnetometer attitude determination [START_REF] Marins | An extended Kalman filter for quaternion-based orientation estimation using MARG sensors[END_REF]. Another method using the error cross product is studied by Euston et al. and Mahony et al. [START_REF] Euston | A complementary filter for attitude estimation of a fixed-wing UAV[END_REF], [START_REF] Mahony | Nonlinear complementary filters on the special orthogonal group[END_REF]. Madgwick et al. [START_REF] Madgwick | Estimation of IMU and MARG orientation using a gradient descent algorithm[END_REF] give another point of view where the gradient descent algorithm (GDA) is adopted. In the similar way, Tian et al. proposed the method based on improved Gauss-Newton algorithm (IGNA) [START_REF] Tian | An adaptive-gain complementary filter for real-time human motion tracking with MARG sensors in free-living environments[END_REF], [START_REF] Tian | Accurate human navigation using wearable monocular visual and inertial sensors[END_REF] while Fourati et al. [START_REF] Fourati | Complementary Observer for Body Segments Motion Capturing by Inertial and Magnetic Sensors[END_REF], [START_REF]A Nonlinear Filtering Approach for the Attitude and Dynamic Body Acceleration Estimation Based on Inertial and Magnetic Sensors: Bio-Logging Application[END_REF] use the Levenberg-Marquardt algorithm (LMA). After attitude determination from vector observations, these methods jointly employ the linear complementary filter (LCF) as the attitude observer.

It can be seen that these advances mainly face the attitude solution from the accelerometer-magnetometer combination. In fact, this is a specific case of the Wahba's problem [START_REF] Wahba | A Least Squares Estimate of Satellite Attitude[END_REF], [START_REF] Markley | How to estimate attitude from vector observations[END_REF]. This leads to a new CF algorithm developed by Marantos et al. [START_REF] Marantos | UAV State Estimation Using Adaptive Complementary Filters[END_REF] which uses the singular value decomposition (SVD, [START_REF] Arun | Least-Squares Fitting of Two 3-D Point Sets[END_REF], [START_REF] Markley | Attitude Determination using Vector Observations and the Singular Value Decomposition[END_REF]) as Wahba's solution, which compensates for the gyroscope's random drift. However, Wahba's problem will have two solutions when there is only one vector observation that makes the attitude solution ambiguous at the same time [START_REF] Markley | How to estimate attitude from vector observations[END_REF], [START_REF] Wu | Attitude Determination Using a Single Sensor Observation : Analytic Quaternion Solutions and Property Discussion[END_REF]. Above all methods generate efficient attitude estimation but they have some joint or respective disadvantages:

1) They just focus on almost the same sensor combination i.e. MARG sensors. Generalized sensor combinations under optimal framework are not well studied. 2) For GDA, IGNA, LMA, brute-force use of the optimization will make the algorithms computationally expensive i.e. some mathematical internals should be investigated further.

Inspired by above representative methods along with their advantages and disadvantages, this paper deals with a novel CF scheme whose main contributions are: 1) Using quaternion representation, the generalized attitude estimation is solved using GDA. Various strapdown sensors like accelerometer, magnetometer, camera, sun sensor, nadir sensor and etc. can be efficiently fused. The architecture is derived to be additive and linear which is simple for implementation and fault detection.

2) With finding in this paper, previous derivative-based optimization methods for attitude estimation are proven to be equivalent to each other.

Experiments on robotic platforms are designed and carried out which verify the proposed filter's effectiveness and advantages compared with representative methods. This paper has the following arrangement of contents: Section II introduces the proposed problem formulation of the generalized sensor fusion from vector observations. Section III contains the proposed GDA method for attitude determination from strapdown sensors. Section IV involves the proposed complementary filter design including the basic structure, robustness ensurance and some further discussions of mathematical properties. Hardware, experiments and results are presented in Section V showing the effectiveness of the proposed filter with respect to reference device and representative methods. Section VI gives the concluding remarks.

II. GENERALIZED SENSOR FUSION

For a sensor combination on a rigid platform, its fusion equation can be given by

       D b 1 = CD r 1 D b 2 = CD r 2 • • • D b n = CD r n (1)
where

D b i = D b x,i , D b y,i , D b z,i
⊤ denotes the ith vector observation in the body frame b while D r i = D r x,i , D r y,i , D r z,i ⊤ denotes the ith vector observation in the reference frame r. C stands for the direction cosine matrix (DCM). The above equation can be converted to a least-square loss function

J (C) = n i=1 CD r i -D b i 2 (2)
with the aim of arg min

CC ⊤ = C ⊤ C = I,det(C)=+1 J (C) (3)
where I is the identity matrix with proper dimension and • is the simplification of Euclidean norm. A possible solution for this problem using SVD is given in [START_REF] Arun | Least-Squares Fitting of Two 3-D Point Sets[END_REF]. When the weights of various sensors are concerned, the problem will be equal to the Wahba's problem [START_REF] Wahba | A Least Squares Estimate of Satellite Attitude[END_REF], such that arg min

CC ⊤ =C ⊤ C=I,det(C)=+1 n i=1 a i CD r i -D b i 2 (4)
where a i denotes the positive weight of the ith sensor with the property of n i=1 a i = 1. Wahba's solutions include a variety of famous algorithms e.g. QUEST, FOAM and SVD [START_REF] Shuster | Three-axis attitude determination from vector observations[END_REF], [START_REF] Markley | Attitude determination using vector observations -A fast optimal matrix algorithm[END_REF], [START_REF] Markley | Attitude Determination using Vector Observations and the Singular Value Decomposition[END_REF]. A recent fast solver FLAE maintains the same accuracy as developed in [START_REF] Wu | Fast Linear Quaternion Attitude Estimator Using Vector Observations[END_REF]. Now we study the fusion for a single sensor. The sub-equation of (1) can be further given by

D b i = CD r i =   C 11 C 12 C 13 C 21 C 22 C 23 C 31 C 32 C 33     D r x,i D r y,i D r z,i   = D r x,i   C 11 C 21 C 31   + D r y,i   C 12 C 22 C 32   + D r z,i   C 13 C 23 C 33   = D r x,i C 1 + D r y,i C 2 + D r z,i C 3 (5)
where C ij is the element of C in the i-th row and j-th column while C i denotes the i-th column of C. Namely, D b i is the linear combination of the three columns of the DCM.

When the DCM is represented by the quaternion q = (q 0 , q 1 , q 2 , q 3 ) ⊤ , the columns can be decomposed such as [START_REF] Wu | Fast Linear Quaternion Attitude Estimator Using Vector Observations[END_REF]

C 1 =   q 2 0 + q 2 1 -q 2 2 -q 3 3 2q 1 q 2 -2q 0 q 3 2q 0 q 2 + 2q 1 q 3   =   q 0 q 1 -q 2 -q 3 -q 3 q 2 q 1 -q 0 q 2 q 3 q 0 q 1       q 0 q 1 q 2 q 3     = P 1 (q)q (6)
C 2 =   2q 1 q 2 + 2q 0 q 3 q 2 0 -q 2 1 + q 2 2 -q 2 3 -2q 0 q 1 + 2q 2 q 3   =   q 3 q 2 q 1 q 0 q 0 -q 1 q 2 -q 3 -q 1 -q 0 q 3 q 2       q 0 q 1 q 2 q 3     = P 2 (q)q (7) C 3 =   -2q 0 q 2 + 2q 1 q 3 2q 2 q 3 + 2q 0 q 1 q 2 0 -q 2 1 -q 2 2 + q 2 3   =   -q 2 q 3 -q 0 q 1 q 1 q 0 q 3 q 2 q 0 -q 1 -q 2 q 3       q 0 q 1 q 2 q 3     = P 3 (q)q (8)
Then ( 5) can be further given by

D b i = D r x,1 C 1 + D r y,i C 2 + D r z,i C 3 = [D r
x,i P 1 (q) + D r y,i P 2 (q) + D r z,i P 3 (q)]q

With this equation, we can extend the 1-dimensional equation to [START_REF] Chan | Smart wearable systems: Current status and future challenges[END_REF]. The fusion error function of the ith sensor is defined by f (q, i) = D r x,i P 1 (q) + D r y,i P 2 (q) + D r z,i P 3 (q) q -D b i (10) Naturally, the error function for the whole sensor combination can be given by the augmented form

f (q, {w, v, • • • , n}) =     f (q, w) f (q, v) • • • f (q, n)     (11)
where {w, v, • • • , n} denotes the set of the indexes of valid sensors which has been sorted in ascending order. In terms of the weights, the corresponding error function is defined by

f (q, {w, v, • • • , n}, {a w , a v , • • • , a n }) =      √ a w f (q, w) √ a v f (q, v) . . . √ a n f (q, n)      (12)
In this way the original problem in (4) can be shifted to

arg min q =1 F (q) 2 (13)
where for simplicity, we use

F (q) = f (q, {w, v, • • • , n}, {a w , a v , • • • , a n }) (14)
to represent the error function [START_REF] Li | Multi-group coordination control of multiagent system based on smoothing estimator[END_REF]. Such problem can be solved via optimization methods. Previously, we have shown that the error function is convex with respect to quaternion [START_REF] Wu | Convexity analysis of optimization framework of attitude determination from vector observations[END_REF]. In the following section we are going to introduce a gradient-descent algorithm.

III. PROPOSED GDA METHOD

The Gradient Descent Algorithm (GDA) is known to be an efficient method for optimization problems [START_REF] Madgwick | Estimation of IMU and MARG orientation using a gradient descent algorithm[END_REF], [START_REF] Tian | An adaptive-gain complementary filter for real-time human motion tracking with MARG sensors in free-living environments[END_REF]. It requires the derivative information of the target function with respect to the variables to be solved. In this case, the state of the system is chosen as the quaternion q. The implementation of the GDA can be given by

q y,k = q y,k-1 -χ k ∇F (q y,k-1 ), χ k > 0 ( 15
)
where χ k is the step size of the kth iteration. The gradient of the sub-error function f (q, i) can be computed by

∇f (q, i) = J ⊤ i f (q, i) (16)
where the Jacobian matrix J i can be calculated by

J i = ∂f (q, i) ∂q = D r x,i ∂C 1 ∂q + D r y,i ∂C 2 ∂q + D r z,i ∂C 3 ∂q (17)
The details of ∂C1 ∂q , ∂C2 ∂q , ∂C3 ∂q are given by

∂C 1 ∂q =   2q 0 2q 1 -2q 2 -2q 3 -2q 3 2q 2 2q 1 -2q 0 2q 2 2q 3 2q 0 2q 1   = 2P 1 (q) ∂C 2 ∂q =   2q 3 2q 2 2q 1 2q 0 2q 0 -2q 1 2q 2 -2q 3 -2q 1 -2q 0 2q 3 2q 2   = 2P 2 (q) ∂C 3 ∂q =   -2q 2 2q 3 -2q 0 2q 1 2q 1 2q 0 2q 3 2q 2 2q 0 -2q 1 -2q 2 2q 3   = 2P 3 (q) (18)
Consequently J i can be written as

J i = 2[D r x,i P 1 (q) + D r y,i P 2 (q) + D r z,i P 3 (q)] (19)
Then the overall gradient of F (q) can be given by

∇F (q) = J ⊤ {w,v,••• ,n} F (q) ( 20
)
where

J {w,v,••• ,n} =     √ a w J w √ a v J v • • • √ a n J n     (21)
Thereby ( 20) can be further given by

∇F (q) = J ⊤ {w,v,••• ,n} F (q) = √ a w J ⊤ w , √ a v J ⊤ v , • • • , √ a n J ⊤ n     √ a w f (q, w) √ a v f (q, v) • • • √ a n f (q, n)     = i∈{w,v,••• ,n} a i J ⊤ i f (q, i) (22)
Letting (see Appendix)

P ⊤ 1 (q)D b i = M 1 D b i q P ⊤ 2 (q)D b i = M 2 D b i q P ⊤ 3 (q)D b i = M 3 D b i q (23)
, the gradient arrives at

∇F (q) = i∈{w,v,••• ,n} ai[ 1 2 J ⊤ i (q)J i (q)q -J ⊤ i (q)D b i ] = 2 i∈{w,v,••• ,n} ai   1 4 J ⊤ i (q)J i (q) -D r x,i M1(D b i ) -D r y,i M2(D b i ) -D r z,i M3(D b i )   q (24)
The operator Σ(q) is defined by

Σ(q) = i∈{w,v,••• ,n} ai   1 4 J ⊤ i (q)J i (q) -D r x,i M1(D b i ) -D r y,i M2(D b i ) -D r z,i M3(D b i )   (25)
Theorem 1. The equation      P ⊤ 1 (q)P 1 (q)q = q P ⊤ 2 (q)P 2 (q)q = q P ⊤ 3 (q)P 3 (q)q = q (26)

always holds for arbitrary unit quaternion.

Proof. See the Appendix.

Lemma 1. With the derivation of Theorem 1, we have the following equalities holding as well:

P ⊤ j (q)P k (q) + P ⊤ k (q)P j (q) q = 0 4×1 ( 27
)
where the indexes j, k = 1, 2, 3 and j = k.

Following Lemma 1, we have

1 4 J ⊤ i (q)J i (q)q = q (28)
Then the operator is simplified from a nonlinear multiplicative function with time complexity of O(n 2 ) to a linear one with complexity of O(n):

Σ(q) = i∈{w,v,••• ,n} a i I -D r x,i M 1 (D b i ) -D r y,i M 2 (D b i ) -D r z,i M 3 (D b i ) (29)
The GDA measurement update equation finally arrives at q y,k = q y,k-1 -2χ k Σ(q y,k-1 )q y,k-1 [START_REF] Kottath | PSO Aided Adaptive Complementary Filter for Attitude Estimation[END_REF] where subscript y denotes the observation model source. This simplification converts the original 4 × 3n-by-3n × 1 matrix multiplication J ⊤ {w,v,••• ,n} F (q) to an additive sum of 4 × 4 matrices, which decreases the space complexity of the algorithm.

IV. COMPLEMENTARY FILTER

A. Filter Design

A linear complementary filter can be written as the following observer [START_REF] Farrell | Aided Navigation: GPS with High Rate Sensors[END_REF]

xk = Φx k-1 + L(y k -ŷk ) ŷk = H xk (31)
where k denotes the kth time epoch, x denotes the state vector, y denotes the measurement vector. Φ, H are transition matrix and measurement matrix respectively. L is the feedback gain matrix. pk stands for the estimation of the p at epoch k.

The feedback gain matrix L is empirically diagonal but may degenerates to a constant for convenience of implementation and gain-determination when

L = βI (32)
where β denotes a constant [START_REF] Tian | An adaptive-gain complementary filter for real-time human motion tracking with MARG sensors in free-living environments[END_REF]. If the state vector is the quaternion q in our case, the linear observer can be further designed as

qk = Φq k-1 + L(q y,k -qy,k ) qy,k = H qk (33)
where

H = I (34)
Using the angular rate ω = (ω x , ω y , ω z ) ⊤ , this equation generally leads to the following quaternion kinematic equation, such that [START_REF] Titterton | Strapdown Inertial Navigation Technology[END_REF]

dq dt = 1 2 [Ω×] q (35)
where [Ω×] defines the skew symmetric matrix of angular rate in Hamilton space Ω = (0, ω x , ω y , ω z )

⊤ [Ω×] =     0 -ω x -ω y -ω z ω x 0 ω z -ω y ω y -ω z 0 ω x ω z ω y -ω x 0     (36)
Hence we approximately have [START_REF] Mahony | Complementary filter design on the special orthogonal group SO(3)[END_REF] Φ

≈ I + ∆t 2 [Ω×] (37)
where ∆t denotes the time span. Expanding the state process equation, [START_REF] Higgins | A Comparison of Complementary and Kalman Filtering[END_REF] can be further given by

qk = Φq k-1 + L (q y,k -qy,k ) ⇒ (I + L) qk = q ω,k + Lq y,k ⇒ qk = (I + L) -1 (q ω,k + Lq y,k ) ⇒ qk = (I + L) -1 q ω,k + (I + L) -1 Lq y,k ⇒ qk = (I + L) -1 q ω,k + L -1 + I -1 q y,k (38)
where

   q y,k = [I -2χ k Σ(q k-1 )]q k-1 q ω,k = I + ∆t 2 [Ω×] qk-1 (39)
Let the complementary gain G = L -1 + I -1 , (38) can be given by

qk = (I + L) -1 q ω,k + L -1 + I -1 q y,k = (I -G) q ω,k + Gq y,k = G [I -2χ k Σ(q k-1 )] + (I -G) I + ∆t 2 [Ω×]
qk-1

= I + ∆t 2 (I -G) [Ω×] -2χ k GΣ(q k-1 ) qk-1 (40)

B. Robustness Ensurance

As described before, the measurement from vector observations compensates for the gyro bias. However, when highly dynamic conditions take place, the performance of the filter will be significantly affected. For instance, in previous works, when exposed to large external acceleration or magnetic distortion, the filter is influenced at the same time. In this paper, we invest a new way for norm verification to reject sensor outliers and thus make the proposed algorithm more robust.

The Euclidean norm information can be obtained during data acquisition of the ith sensor, such that

N orm i = D b i = (D b x,i ) 2 + (D b y,i ) 2 + (D b z,i ) 2 (41)
Before the filtering process, we should have some priori knowledge of the adopted sensor. For any sensor, when it is operated with smooth motion under environments with stable external field e.g. gravity field, earth-magnetic field, the norm of the output should be around a certain constant. We call this constant the standard norm ST DN orm i . When the motion becomes drastic, the norm of the sensor output will have relatively big deviation from the standard norm. In this circumstance, the sensor is no longer trustworthy and associated item in Σ(q k-1 ) should be deleted. Hence the weights in [START_REF] Sabatini | Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing[END_REF] should be revised in this case as

ãi = a i , |N orm i -ST DN orm i | < µ i 0, |N orm i -ST DN orm i | ≥ µ i (42)
and µ i denotes a threshold for detecting drastic modes [START_REF] Li | Effective Adaptive Kalman Filter for MEMS-IMU/Magnetometers Integrated Attitude and Heading Reference Systems[END_REF]. Finally, the whole filtering process including data acquisition and state update is given in Algorithm 1.

C. Determination of Parameters

There are some parameters to be determined before the filter begins. The initial quaternion q init is the initial state of the attitude estimator and it can be obtained using initial alignment from strapdown sensors [START_REF] Suh | Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of external acceleration[END_REF]. The complementary gain G is commonly set as an diagonal matrix whose components are empirically adjusted using the estimator's response. The determination of step length χ 0 is very similar. The weights can be determined using the initial standard deviations of Algorithm 1 Generalized complementary filter using strapdown vector observations via GDA (GCF). Initialize:

Time epoch k = 0 Initial state q k=0 = q init Complementary gain G Step length χ k = χ 0 Sorted valid sensors' indexes w, v, • • • , n Weights a w , a v , • • • , a n Standard norm list {ST DN orm i |i = w, v, • • • , n} Threshold list {µ w , µ v , • • • , µ n } Output: qk . while no stop commands received do 1) k = k + 1 2) Input: a) Valid normalized strapdown sensor observations: D b w , D b v , • • • , D b n b) Normalized reference vectors: D r w , D r v , • • • , D r n c) Angular rate in (rad/s): ω = (ω x , ω y , ω z ) ⊤ d) If gyroscope is not valid: G = I 3) Calculate norms N orm i = D b i 4) Deduce outlier rejection: ãi = a i , |N orm i -ST DN orm i | < µ i 0, |N orm i -ST DN orm i | ≥ µ i 5) Normalization: D b i = D b i D b i 6) Calculate: Σ(q k-1 ) = i∈{w,v,••• ,n} ãi    I -D r x,i M 1 (D b i ) -D r y,i M 2 (D b i ) -D r z,i M 3 (D b i )    7) Perform time update: qk =    I + ∆t 2 (I -G) [Ω×] -2χ k G Σ(q k-1 )    qk-1 8
) Normalization: qk = qk qk end while different sensor [START_REF] Mortari | ESOQ: A Closed-Form Solution to the Wahba Problem[END_REF]. When the robust ensurance step is applied, the standard norm list can be given according to the regular ranges of the sensors' norms. The thresholds is chosen empirically in terms of the smoothness of the filter i.e. it decides how many 'unusual' sensor observations with unusual norms are neglected.

D. Initial Alignment

The initial alignment problem is in fact the attitude determination from strapdown vector observations in the initial stage. The following scheme is depicted for initial alignment with our proposed filter.

q init,k = q init,k-1 -Σ(q init,k-1 )q init,k-1 while q init,k-1 -q init,k-2 > κ (43)
in which k = 1, 2, • • • and κ is the threshold indicating the relative accuracy. More specifically, in steady state, we have

Σ(q init )q init = 0 (44)
Expanding it, it is obtained that the initial quaternion belongs to the following eigenvalue problem

   i∈{w,v,••• ,n} D r x,i M 1 D b i + D r y,i M 2 D b i + D r z,i M 3 D b i    q init = q init (45)
The optimal solution is the eigenvector associated with the eigenvalue that is closest to 1, which can be solved with our recent method FLAE [START_REF] Wu | Fast Linear Quaternion Attitude Estimator Using Vector Observations[END_REF].

E. Further Identities and Equivalences

Remark 1. Recalling [START_REF] Kottath | PSO Aided Adaptive Complementary Filter for Attitude Estimation[END_REF], we may rewrite it as

q y,k = q y,k-1 -2χ k Σ(q y,k-1 )q y,k-1 = q y,k-1 -2χ k      I - i∈{w,v,••• ,n} ai    D r x,i M1(D b i ) +D r y,i M2(D b i ) +D r z,i M3(D b i )         q y,k-1 = (1 -2χ k )q y,k-1 + 2χ k i∈{w,v,••• ,n} ai    D r x,i M1(D b i ) +D r y,i M2(D b i ) +D r z,i M3(D b i )   q
(46) If we treat the step size as a complimentary gain, then observation model actually leads to a filtered quaternion. This shows that the GDA is not only an optimization solver, but a smoother as well. Such identity makes the obtained attitude estimates more smooth than that directly derived from Wahba's solutions.

Theorem 2. Derivative-based optimization methods including GDA, GNA and LMA for optimal attitude determination from vector observations are essentially equivalent to each other.

Proof. The Gauss-Newton algorithm (GNA) is a classical optimization problem but it may fail when the Jacobian matrix is singular. Consequently, some other algorithms e.g. the Levenberg-Marquadt algorithm (LMA) are designed to overcome this drawback. The searching equation can be written as

q k = q k-1 -J ⊤ {w,v,•••n} J {w,v,•••n} + λI -1 J ⊤ {w,v,•••n} f (q k-1 , {w, v, • • • n}) (47) Then we have J ⊤ {w,v,••• ,n} J {w,v,••• ,n} + λI q k = J ⊤ {w,v,••• ,n} J {w,v,••• ,n} + λI q k-1 - J ⊤ {w,v,••• ,n} f (q k-1 , {w, v, • • • , n}) (48)
Note that

J ⊤ {w,v,••• ,n} J {w,v,••• ,n} + λI q k-1 =   λI + i∈{w,v,••• ,n} a i J ⊤ i J i   q k-1 = (1 + λ)q k-1 (49)
Defining the quaternion error as

∆q = J ⊤ {w,v,••• ,n} J {w,v,••• ,n} + λI (q k -q k-1 ) (50)
which is fully relevant to the original quaternion error q kq k-1 according to invertible priori matrix multiplication, we finally obtain

∆q ≈ (1 + λ)(q k -q k-1 )- J ⊤ {w,v,••• ,n} f (q k-1 , {w, v, • • • , n}) (51)
In other words, the LMA obtains the same optimization results in steady state with GDA. This shows that GDA is sufficient for optimization update and LMA-based method like [START_REF] Fourati | Complementary Observer for Body Segments Motion Capturing by Inertial and Magnetic Sensors[END_REF] would only produce advance in smoothness. Besides, notice that LMA is in fact an improved algorithm based on Gauss-Newton algorithm (GNA). This shows that related GNA methods like [START_REF] Tian | An adaptive-gain complementary filter for real-time human motion tracking with MARG sensors in free-living environments[END_REF] are equivalent to the proposed GDA as well.

As the mentioned GNA, LMA and GDA are representatives of derivative-based optimization, the equivalence connections are established.

V. HARDWARE, EXPERIMENTS AND RESULTS

A. Sensors

Employed sensors in this section are a 3-axis microelectromachnical-system (MEMS) accelerometer, a 3-axis MEMS gyroscope, a 3-axis MEMS magnetometer and a monocular camera. Each sensor has its sensing principle and mathematical model. Here, we simply introduce the details of these sensors.

A 3-axis accelerometer measures the object's specific force. Its output in the object's body frame can be expressed by A b = (a x , a y , a z ) ⊤ . A 3-axis gyroscope gives the angular rate data of the object and its output is given by ω = (ω x , ω y , ω z ) ⊤ . The magnetometer measures the Earth's geomagnetic field and its output is M b = (m x , m y , m z ) ⊤ . Fig. 1: Designed hardware platform. The platform consists of integrated AHRS, a monocular camera, a battery, an USB debugger and an embedded computer.

Camera has been widely used with the development of consumer electronics . Using a camera, we can capture numbers of images and videos. In fact, motion can be extracted from a recorded video stream since continuous pictures correspond to changes of attitude and translation. To achieve this task, characteristics of pictures are necessary. There are many feature extraction methods including scale-invariant feature transform (SIFT) [START_REF] Brown | Automatic Panoramic Image Stitching Using Invariant Features[END_REF], gradient location-orientation histogram (GLOH) [START_REF] Mikolajczyk | A Performance Evaluation of Local Descriptors[END_REF], speeded-up robust features (SURF) [START_REF] Bay | Speeded-up robust features (surf)[END_REF] and etc. Extracted features from two neiboughring images are in a degree similar. This provides an approach to determine the attitude and the translation vector with respect to the previous acquired image. Since the correlation of the 3D features are easily disturbed by noises, the random sample consensus (RANSAC) algorithm [START_REF] Ye | 6-DOF Pose Estimation of a Robotic Navigation Aid by Tracking Visual and Geometric Features[END_REF] can be used for rejecting outliers according to probabilistic functions. Using the final valid features' correspondence, the relative attitude can be obtained from (3) via the SVD method by Arun et al. [START_REF] Arun | Least-Squares Fitting of Two 3-D Point Sets[END_REF]. If the vision field is wide enough and the motion is relatively moderate, the attitude of the object can be directly obtained using the difference between the current and initial images.

B. Hardware Configuration

To verify the effectiveness of the proposed filter, an experimental platform is designed (see Fig. 1). The system integrates a commercial attitude and heading reference system (AHRS) which is composed of magnetic, angular rate and gravity (MARG) sensors. The AHRS can give high precision reference attitude angles along with raw sensor outputs and has been widely verified for its high reliability in navigation tasks such as UAV, land vehicles and robots. The reference attitude angles from the AHRS is chosen as the ground truth. A camera is attached firmly to the installed AHRS to maintain relatively identical attitude determination. It should be noted that the design of the camera is motivated by PX4FLOW [START_REF] Honegger | An open source and open hardware embedded metric optical flow CMOS camera for indoor and outdoor applications[END_REF] but has been modified for broader vision field and higher image resolution. In fact there is a micro controller on the camera board making the calculations faster. The navigation computer is formed by an STM32F4-based board with multiple interfaces. To achieve wireless and highly reliable data transmission, an Xbee Pro S3B telemetry is installed on board.

In the following sub-sections, we are going to carry out several experiments with the above sensors in order to evaluate the performances of accuracy, robustness, time consumption. of the proposed algorithm compared with representative methods:

1) The first one integrates a gyroscope, an accelerometer and a magnetometer together which generates a typical full-attitude AHRS. In such combination, the vector pairs are

D b 1 = (a x, a y , a z ) T D b 2 = (m x, m y , m z ) T , D r 1 = (0, 0, 1) T D r 2 = (m N , 0, m D ) T ( 52
)
where a x , a y , a z and m x , m y , m z are normalized vector measurements in the body frame b from the accelerometer and magnetometer respeceively. m N and m D are reference vector components of the magnetometer in the North-East-Down frame which can be referenced from the geomagnetic model according to local geodetic location.

2) The second experiment involves a gyroscope, an accelerometer, a magnetometer and a monocular camera in a hovering flight, where the vector pairs are

                         D b 1 = (a x , a y , a z ) T D b 2 = (m x , m y , m z ) T D b 3 = (p x,1 , p y,1 , p z,1 ) T . . . D b n = p x,n-2 , p y,n-2 , p z,n-2 T ,                          D r 1 = (0, 0, 1) T D r 2 = (m N, 0, m D ) T D r 3 = p r x,1 , p r y,1 , p r z,1 T . . . D r n = p r x,n-2 , p r y,n-2 , p r z,n-2 T ( 53
) in which p x,i , p y,i , p z,i are normalized coordinates of the i-th transformed feature points in the body frame. p r

x,i , p r y,i , p r z,i are normalized i-th transformed feature points reference obtained in initial image capture of the ground. With vector pair configurations shown above, we can easily fuse them with the procedure provided in Algorithm 1.

C. Case 1: AHRS with MARG Sensors

The accelerometer and magnetometer are introduced, adding a compensation of pitch, roll and yaw angles from gravityfield and magnetic-filed sensing data. The MARG sensors are calibrated for initial biases and misalignment. To ensure good results, the operating temperature of the sensors is stablized at 45 • C using a controlled thermal resistance. Constant iron and soft magnetic distortion to the magnetometer are also compensated for before all the experiments [START_REF] Wu | Calibration of three-axis strapdown magnetometers using Particle Swarm Optimization algorithm[END_REF]. To verify the performance of the proposed GCF, representative methods like Wahba's Complementary Filter (WCF) by Marantos et al. [START_REF] Marantos | UAV State Estimation Using Adaptive Complementary Filters[END_REF], Algebraic-QUaternion-Algorithm-based quaternion Kalman filter (AQUA q-KF) by Valenti et al. [START_REF] Valenti | A Linear Kalman Filter for MARG Orientation Estimation Using the Algebraic Quaternion Algorithm[END_REF], Levenberg-Marquadt-Algorithm Complementary Observer (LMA-CO) by Fourati et al. [START_REF] Fourati | Complementary Observer for Body Segments Motion Capturing by Inertial and Magnetic Sensors[END_REF] are used for comparisons. The parameters of different filters are tuned as follows 1) WCF: The parameters are w a = 0.9, w m = 0.8, [START_REF] Marantos | UAV State Estimation Using Adaptive Complementary Filters[END_REF].

c a 1 = c m 1 = 0.7, c a 2 = c m 2 = 0.3, c a 3 = 8500, c m 3 = 5500 as described in
2) AQUA q-KF: The variance matrices are Σ gyro = diag(0.0013, 0.0013, 0.0013) Σ acc = diag(0.01, 0.01, 0.02) Σ mag = diag(0.05, 0.05, 0.05) 3) LMA-CO: The gain is set to 0.1 and the positive number for LMA is set to λ = 0.000001 4) Proposed GCF: The weights are set to a acc = 0.7 and a mag = 0.3 to enhance the accelerometer's effect while the complementary gain is set to G = 0.1I. The gain of the proposed GCF is tuned to achieve relatively good attitude estimation results in the case of such experimental conditions. With the recorded data in previous experiment, the attitude estimation errors are obtained and shown in Fig. 2. In this Fig. 2: Attitude estimation errors during a flat motion from WCF, AQUA q-KF, LMA-CO and the proposed GCF. Accelerometer and magnetometer are jointly utilized.

3.5715 • AQUA q-KF 2.1736 • 1.0264 • 1.3266 • LMA-CO 1.6172 • 1.0539 • 1.7120 • Proposed GCF 1.6018 • 1.0107 • 1.0998 •
evaluation, the norm verification is not performed since the motion is not drastic. As can be seen from the figure, WCF is the worst among all filters. The estimation accuracy of AQUA q-KF and LMA-CO is close to that of the proposed filter. To further verify the performances, we calculate the Root-Mean-Squared Errors (RMSEs) of various algorithms with respect to reference angles from high precision AHRS (see Table I). We can see that for roll and pitch angles, the proposed GCF reaches almost the same accuracy with LMA-CO while GCF is the best for estimating attitude angles. This shows that the proposed GCF is efficient for quasi-static attitude estimation.

As described before, the performance of the filter is being tested out when drastic conditions occur. The magnetometer can be easily disturbed by iron or magnetic objects i.e. it is sensitive to magnetic distortion. In the next experiment, the designed hardware remains still on a table and is perturbed by a moving magnet. Fig. 3 reflects the raw data from the magnetometer along with its norm. We can see that the magnetic distortion is very large with the norm of up to 10 Gauss. Using the acquired data, attitude estimation errors are calculated and shown in Fig. 4. The threshold for norm verification of magnetometer is set to µ mag = 0.2. We can find out that the filter without outlier rejection undergoes very evident disturbances of yaw angles as magnetic distortion happens. The proposed GCF is disturbed in this case, not only for yaw but also for roll and pitch angles. However, GCF with outlier rejection shows interesting behaviour since it is hardly perturbed by the magnetic distortion. The third sub-figure of Fig. 3 shows that the filter can detect the magnetic distortion at a high level. Although most magnetometer's outputs are being disturbed, there are still some trustworthy data. Using these data, the filter maintains stable with convergent yaw estimation. The RMSEs are shown in Table II. In this sub-section, a quadrotor is used to carry the designed platform (see Fig. 5). The quadrotor is equipped with carbonfibre body and properlers and is operated under human's control via a flight controller of DJI Wookong-M. The camera is pre-calibrated for its intrinsic parameters using the geometric method proposed by Zhang [START_REF] Zhang | A Flexible New Technique for Camera Calibration[END_REF]. The 2D feature points are converted to the 3D camera frame via intrinsic parameters and instant rotation prediction from angular rate measurements. During a hovering flight, when the attitude and altitude are relatively stable, raw inertial data, raw images and reference attitude angles are recorded with the frequency of 50Hz. A certain image is chosen as the reference image. The images are processed with SIFT and RANSAC for point cloud matching. An amount of at least 4 point pairs and 12 point pairs at most is designed for the selecting of vector observations. All the point pairs are ensured not to be collinear with each other. The accelerometer and magnetometer are also adopted for sensor fusion. The attitude estimation results are calculated with the proposed complementary filter, an attitude observer proposed by Bras et al. [START_REF] Bras | A Nonlinear Attitude Observer Based on Active Vision and Inertial Measurements[END_REF], the SVD method proposed by Arun et al [START_REF] Arun | Least-Squares Fitting of Two 3-D Point Sets[END_REF] and the QUEST solution to Wahba's problem [START_REF] Shuster | Three-axis attitude determination from vector observations[END_REF]. The related attitude angles are depicted in Fig. 6. As can be seen from the figure, the proposed filter's attitude estimation is the closest to reference angles. While for other algorithms, the variance is evidently larger than the proposed GCF. The RMSEs of the attitude angles from different sources are given in Table III. As the matter of fact, the proposed GDA method provides us with a new approach for attitude determination from vector observations. Using the raw data collected, the comparisons on attitude angles with various algorithms are depicted in Fig. 7. The results show that the batch attitude determination from SVD and Wahba's solution are much more noisy with respect to the proposed GDA. This indicates that the proposed GDA can properly determinate the attitude with smooth outputs. This can be applied to related areas for reliable attitude determination from camera outputs.

E. Evaluation of Open Datasets

In nowdays autonomous driving equipments, there are always sensors like IMU, GNSS receiver to perform accurate state estimation of vehicles. The KITTI dataset [START_REF] Geiger | Vision meets robotics: The kitti dataset[END_REF], released by the Karlsruhe Institute of Technology and Toyota Technological Institute at Chicago in 2012, has been widely employed in algorithmic evaluations in representative works. It provides the users with a large variety of sensor measurements including angular rate, acceleration, velocity, position and ground-truth values. Here we use the dataset 2011_09_26_drive_0096_extract in the city scenes. We use the accelerometer and GNSS velocity measurements to form the vector observation pairs such that

D b 1 = (a F , a L , a U ) ⊤ D b 2 = (0, -1, 0) ⊤ , D r 1 = (0, 0, -1) ⊤ D r 2 = (v F , v L , v U ) ⊤
where a F , a L , a U and v F , v L , v U are normalized measurements of the acceleration and velocity respectively in the Forward-Left-Up frame. The weights of the two strapdown measurements are given equally i.e. 0.5 and 0.5 [START_REF] Zhou | Optimal , Recursive and Sub-optimal Linear Solutions to Attitude Determination from Vector Observations for GNSS / Accelerometer / Magnetometer Orientation Measurement[END_REF]. The complementary gain is set to G = 0.01I while the GDA step size is χ = 0.1. The acceletometer-GNSS fusion is enabled only when the velocity norm is over 1m/s to ensure reliable heading determination. Filtering with the angular rate, the attitude results are computed in Fig. 8. The proposed GCF gradually converge to the ground-truth over the initial time period. In fact the filter's convergence largely depends on the filter gain and GDA correction step length. Here the experimental result has proven the correctness and efficiency of the proposed filter. In later works, we will try to find how to adaptively tune the filter gain so that the attitude estimation would be more closer to the ground truth all the time.

Fig. 8: The attitude estimation results using KITTI dataset.

F. Time Consumption

This sub-section shows the execution cost regarding the time consumption of the proposed filter. Several comparisons are conducted to verify the performances. 1) Comparisons with Various Algorithms: Most of the adopted algorithms in previous sub-sections are evaluated for time consumption results. Fig. 9 shows the comparisons of time consumption between attitude algorithms for MARG fusion. Among all the algorithms, the proposed GCF owns the least time consumption while LMA-CO is the slowest. The WCF in fact uses an SVD solution to Wahba's problem [START_REF] Markley | Attitude Determination using Vector Observations and the Singular Value Decomposition[END_REF] to form a complementary filter. LMA-CO requires the calculation of Jacobian and inversion thus makes it slow. AQUA q-KF is based on Kalman filter theory which needs many matrix operations. The proposed filter, however, is free of inversion and other operations. Hence, in a word, the mathematical design of the proposed GCF makes it faster than other algorithms. When we use camera to obtain attitude determination, the amount of vector observations significantly increases. At this time, various algorithms have quite different behaviours. The related evaluated time consumption is given in Fig. 10. Fig. 11: Time consumption of the proposed filter varies with the amount of the vector observations. The legend denotes the index of the repeated simulation.

We can see that the QUEST algorithm is the fastest among all algorithms. However, QUEST, as a batch attitude determination algorithm, can only produce unfiltered results from vector observations. In real applications, we need smooth state estimates to ensure the operating quality of the system. It should be noted that the proposed GDA, as described before, can be seen as an attitude smoother (see Fig. 7). The results show that the proposed GDA is the second fastest among all the algorithms. This proves that the proposed GDA can determinate attitude with relatively fast speed and much more smooth outputs. The mean time consumption and related standard deviation of different algorithms are presented in Table IV. 2) Time Complexity Test: Numbers of simulations are also carried out to show the relationship between the amount of vector observations and time consumption. We repeated one simulation with various amount of vector observations for 15 times and the details are gathered and shown in Fig. 11. Multiple simulations show that the relationship is linear between the amount of vector observations and time consumption. Hence this algorithm owns a time complexity of O(n). The reason is that the proposed filter simplifies the computation of the Jacobian matrix and thus converts the sophisticated matrix multiplication to a sum of matrices. The low time complexity of the proposed filter makes it easy to be applied on platforms with low-configurations. The saved time can be reserved for fault detection, failsafe, reliability enhancement and etc.

VI. CONCLUSION This paper deals with the generalized sensor fusion problem.

Based on some previous works, the problem is transformed into minimizing a new error function. The minimization problem is then solved using the gradient descent algorithm (GDA). Different from existing works, we obtain some meaningfull findings of the sensor fusion that significantly simplifies the computation of the Jacobian matrix. The original huge matrix multiplication is then converted to a sum of several matrices. The proposed approach in fact gives a new method for attitude determination from vector observations. A complementary filter is designed further to fuse the angular rate and measurement quaternion from vector observations together. The filter is then named as the generalized complementary filter (GCF). Moreover, we study some mathematical properties of the proposed GCF. Results show that for quaternion-based attitude estimation from angular rate information and strapdown vector observations, the proposed GDA is equivalent to Levenberg Marquadt Algorithm (LMA).

Experiments and simulations are designed and carried out to verify the correctness and effectiveness of the proposed filter. Throughout experiments using MARG sensors, the results prove that the proposed filter can produce accurate attitude estimation in both normal and harsh cases. The algorithm is then extended to attitude estimation using inertial information and visual data from a monocular camera. The comparisons show that the filter can also achieve a satisfactory attitude accuracy with respect to ground truth and other representative methods. Execution time consumption from various sources is also investigated which proves that the proposed filter is computation-efficient and owns a time complexity of O(n) with respect to the number of vector observations that makes it easier to be implemented on low-configuration platforms.

We believe that the proposed filter can potentially benefit related applications. Related codes have been upload on https://github.com/zarathustr/GCF. Also, we think that the determination of an adaptive gain to the filter for better dynamic performance will be another task for us in the future.

APPENDIX MANDATORY PROOFS A. Linear Matrix Conversion

P ⊤ 1 (q)D b i =     D b
x,i q 0 -D b y,i q 3 + D b z,i q 2 D b

x,i q 1 + D b y,i q 2 + D b z,i q 3 D b y,i q 1 -D b x,i q 2 + D b z,i q 0 D b z,i q 1 -D b y,i q 0 -D b x,i q 3

P ⊤ 2 (q)D b i =     D b
x,i q 3 + D b y,i q 0 -D b z,i q 1 D b

x,i q 2 -D b y,i q 1 -D b z,i q 0 D b x,i q 1 + D b y,i q 2 + D b z,i q 3 D b

x,i q 0 -D b y,i q 3 + D b z,i q 2

P ⊤ 3 (q)D b i =     D b y,i q 1 -D b x,i q 2 + D b z,i q 0 D b
x,i q 3 + D b y,i q 0 -D b z,i q 1 D b y,i q 3 -D b x,i q 0 -D b z,i q 2 D b

x,i q 1 + D b y,i q 2 + D b z,i q 3 (54b)

B. Quaternion Identities

One can easily write out P ⊤ 1 (q)P 1 (q) =     q 2 0 + q 2 2 + q 2 3 q1q0 -q1q3 q1q2 q0q1 q 2 1 + q 2 2 + q 2 3 q0q3 -q0q2 -q3q1 q3q0 q 2 0 + q 2 1 + q 2 2 q2q3 q1q2 -q0q2 q2q3 q 2 0 + q 2 1 + q 2 3     (55) With the same technique, we obtain      P ⊤ 1 (q)P 1 (q) = I 4×4 + U 1 (q) P ⊤ 2 (q)P 2 (q) = I 4×4 + U 2 (q) P ⊤ 3 (q)P 3 (q) = I 4×4 + U 3 (q)

where

U 1 (q) =     -q 2 1
q 0 q 1 -q 1 q 3 q 1 q 2 q 0 q 1 -q 2 0 q 0 q 3 -q 0 q 2 -q 1 q 3 q 0 q 3 -q 2 3 q 2 q 3 q 1 q 2 -q 0 q 2 q 2 q 3 -q 2 2     (57a)

U 2 (q) =     -q 2 2
q 2 q 3 q 0 q 2 -q 1 q 2 q 2 q 3 -q 2 3 -q 0 q 3 q 1 q 3 q 0 q 2 -q 0 q 3 -q 2 0 q 0 q 1 -q 1 q 2 q 1 q 3 q 0 q 1 -q

2 1     U 3 (q) =     -q 2 3
-q 2 q 3 q 1 q 3 q 0 q 3 -q 2 q 3 -q 2 2 q 1 q 2 q 0 q 2 q 1 q 3 q 1 q 2 -q 2 1 -q 0 q 1 q 0 q 3 q 0 q 2 -q 0 q 1 -q

2 0     (57b)
Then with expansions, we have U 1 (q)q = U 2 (q)q = U 3 (q)q = 0 4×1 [START_REF] Ye | 6-DOF Pose Estimation of a Robotic Navigation Aid by Tracking Visual and Geometric Features[END_REF] which finishes the proof of Theorem 1.

Fig. 3 : 1 :

 31 Fig. 3: 1: Raw data from magnetometer. 2: Norm of the measured magnetic field. 3: Modes of the motion.

Fig. 4 :

 4 Fig. 4: Attitude errors from various sources in the presence of magnetic distortion.

Fig. 5 :Fig. 6 :

 56 Fig.5: A quadrotor is used for validation of the attitude estimation system involving MARG sensors and a monocular camera.

Fig. 7 :

 7 Fig. 7: Attitude estimation results from vector observations. The SVD solution proposed by Arun et al., QUEST algorithm proposed by Shuster et al. and the proposed GDA are adopted for comparisons.

Fig. 9 :

 9 Fig. 9: Time consumption of different algorithms for MARG fusion.

Fig. 10 :

 10 Fig. 10: Time consumption of different algorithms for attitude determination from camera outputs.

  = M 3 (D b i )q

TABLE I :

 I RMSEs of attitude angles

	Algorithms	Roll	Pitch	Yaw
	WCF	4.5075 •	2.7846 •

TABLE II :

 II RMSEs of attitude angles when magnetic distortion takes place

	Algorithms	Roll	Pitch	Yaw
	0.6637 • GCF without outlier rejection 5.7659 • GCF with outlier rejection	0.5525 • 4.0821 •	1.1507 • 99.4968

•

D. Case 2: MARG Sensors and A Monocular Camera

TABLE III :

 III RMSEs of attitude angles

	Algorithms	Roll	Pitch	Yaw
	2.1848 • 1.4628 • Sergio's Observer 0.6998 • SVD QUEST Proposed GCF 0.3798 •	2.0441 • 1.0825 • 0.3990 • 0.3377 •	2.4495 • 1.9269 • 0.6816 • 0.1731 •

TABLE IV :

 IV Mean and standard deviation of time consumption of various algorithms for each vector observation. Each algorithm is executed for 20 times to obtain the average values.

	Algorithms	Mean Time	STD
	Proposed GCF	3.4141 × 10 -5 s	2.9987 × 10 -6 s
	WCF	9.2680 × 10 -5 s	9.1729 × 10 -6 s
	LMA-CO	9.9289 × 10 -5 s	1.2806 × 10 -5 s
	AQUA q-KF	6.8710 × 10 -5 s 1.1821 × 10 -5 s
	Nonlinear Observer	6.3368 × 10 -5 s 8.2699 × 10 -6 s
	QUEST	2.9176 × 10

-5 s 3.6136 × 10 -6 s SVD 5.4259 × 10 -5 s 5.8520 × 10 -6 s

Powered by TCPDF (www.tcpdf.org)

ACKNOWLEDGMENT

This work was supported by National Natural Science Foundation of China under the grant numbers of No. 41604025 and No. 61450010 and in part the State Key Laboratory of Geodesy and Earth's Dynamics (Institute of Geodesy and Geophysics, CAS) Grant No. SKLGED2018-3-2-E and the Fundamental Research Funds for the Central Universities Grant No. ZYGX2018J080. Chengdu Bobei Inc., Beijing Sanchi Inertial Sensor Inc. and Chongqing Fengmai Innovation Inc. supported some related services. We genuinely thank them for their support.

This work was supported by National Natural Science Foundation of China under the grant number of No. 41604025 and the grant number of No. 61450010, the State Key Laboratory of Geodesy and Earth's Dynamics (Institute of Geodesy and Geophysics, CAS) Grant No. SKLGED2018-3-2-E and the Fundamental Research Funds for the Central Universities Grant ZYGX2018J080. (Corresponding author: Zebo Zhou)

The open-source codes of this paper can be downloaded from https://github.com/zarathustr/GCF.