MIMU integrated navigation with non-additive noise

Accepted Manuscript Augmented Cubature Kalman Filter for nonlinear RTK/MIMU Integrated Navigation with Non-additive noise Dingjie Wang, Hanfeng Lv, Jie Wu PII: DOI: Reference:

S0263-2241(16)30617-0 http://dx.doi.org/10.1016/j.measurement.2016.10.056 MEASUR 4418

To appear in:

Measurement

Received Date: Revised Date: Accepted Date:

25 May 2016 23 July 2016 28 October 2016

Please cite this article as: D. Wang, H. Lv, J. Wu, Augmented Cubature Kalman Filter for nonlinear RTK/MIMU Integrated Navigation with Non-additive noise, Measurement (2016), doi: http://dx.doi.org/10.1016/j.measurement. 2016.10.056

This is a PDF file of an unedited manuscript that has been accepted for publication. As a service to our customers we are providing this early version of the manuscript. The manuscript will undergo copyediting, typesetting, and review of the resulting proof before it is published in its final form. Please note that during the production process errors may be discovered which could affect the content, and all legal disclaimers that apply to the journal pertain.

Augmented Cubature Kalman Filter for nonlinear RTK/MIMU Integrated Navigation with Non-additive noise Dingjie Wang1, Hanfeng Lv1 , Jie Wu1

1. College of Aerospace and Engineering, National University of Defense Technology, Changsha 410073, China [email protected] (WANG Dingjie) In order to enhance the capability of autonomous operation for small unmanned aerial vehicles (UAV), a MEMS-based inertial navigation system (INS)/global navigation satellite system (GNSS) integrated navigation method is proposed. An augmented Cubature Kalman filter is derived to fulfil the data fusion of precise GNSS real-time kinematic (RTK) solution and noisy inertial measurements. In the filter, Cubature Kalman filtering is adopted to handle the strong INS model nonlinearity caused by sudden and large UAV maneuvers, and the technique of state-augmentation is used to capture meaningful odd-order moment information and reduce the adverse impacts of non-additive noise in inertial measurements. It is analyzed that the basic difference between the augmented and nonaugmented CKFs generally favors the augmented CKF, which is supported by a representative example and flight test. The results of flight test have also shown that the proposed augmented Cubature Kalman filtering method can complete more accurate navigation compared with the conventional EKF/UKF-based approaches.

Key words: Low-cost MIMU; Bayesian Estimation; non-additive noise; Cubature Kalman filter; UAV

1. Introduction Nowadays, small unmanned aerial vehicles (UAV) have played an indispensable role in various applications such as scientific research, surveying, mapping, public security maintenance, disaster monitoring and emergency rescue [1-2]. UAVs can achieve sustained flight without air-borne human pilots. However, current UAVs are generally highly dependent on the ground operators when performing 4D (Dangerous\Dirty\Deep\Dull) exploration missions. Ground operators can guide the UAVs in the sight-distance range or accomplish first-person view (FPV) flight in the long-distance range via wireless remote control. As a consequence, flight missions cannot be conducted in bad weather conditions of poor visibility (e.g., dense fog, heavy rain and snow, etc.), undermining UAVs’ operation efficiency. The demand that UAVs autonomously carry out complex operations has imposed higher requirements upon the independent navigation capability [3]. An autonomous airborne navigation system with high accuracy and reliability is critical to realize this goal. -1-

Recently, the emergence of miniature MEMS-based inertial measurement unit (MIMU) has played an important role in small UAVs’ autonomous navigation [4]. For UAVs, the use of low-cost MIMU is the only viable solution due to the strict restrictions of load weight and volume [5]. It brings many benefits for system integration such as miniature design, low power consumption, cost-effective and high reliability, reducing the air-borne navigation system weight dramatically and sparing room for more payloads and apparatus [4-7]. However, MIMU suffers from the noisy measurement so that the navigation accuracy degrades sharply when MIMU works independently for a long time [4, 9]. In order to guarantee navigation accuracy, MIMU should be calibrated on-line with the aid of accurate external measurement in the long-run (e.g. GNSS). What is worth mentioning is that, the real-time kinematic positioning of GNSS can reach the accuracy of centimeter for all-weather dynamic users in the short baseline (≤20 km), although its data update rate is typically low with relatively large and short-term noise, whose accuracy may be affected by some factors such as satellite signal loss or attenuation, and the number of satellites and their positions

[10]

. Therefore, the complementary properties of RTK and MIMU make

their integration the most promising navigation scheme with advantages in accuracy, real-time and costeffectiveness. Information fusion algorithm is of the utmost importance for an integrated navigation system. Generally speaking, there are three main factors impacting the RTK/MIMU integration performance: (a) the inaccuracy of model parameters, (b) the model nonlinearity, and (c) the uncertainty of MIMU noise. In the traditional navigation applications, extended Kalman filter (EKF) is generally used as a standard algorithm for GNSS/INS navigation system under the assumption of small heading uncertainty (SHU) [813]

. For high-dynamic vehicles such as small UAVs, however, conventional EKF method mainly suffers

from the strong model nonlinearity caused by system model linearization because of untenable SHU linearization condition

[4]

. Meanwhile, sudden motion changes and non-additive noise properties of

MIMU could lead to large initialization uncertainty and inaccurate model statistic parameters, which will deteriorate or even destroy the performance of EKF. Special attention has been paid to handling the large linearization error for traditional EKF methods in GNSS/INS navigation applications. Since SHU model requires that initial attitude errors are small, an initial attitude estimate should be determined accurately enough to guarantee error state model’s linearization [4, 16-19]; otherwise large heading uncertainty (LHU) model should be developed

[20-22]

. As a

primary step to reduce linearization error, these attitude initialization methods can be divided into three

2

categories: (a) analytical approximate solution methods [17-19]

, and (c) LHU-based filtering methods

[4, 16]

, (b) optimization-based iteration methods

[20, 21]

. These methods use clever constraints or estimation

criterion to estimate the initial attitude for SINS on the moving base. However, accuracy and real-time cannot be guaranteed in some extreme situations (i.e. sudden and large maneuver or sideslip). It is still a challenge to acquire a sound initial attitude for low-cost MIMU when the carrier is moving. Another approach to handle the nonlinearity of low-cost INS is the introduction of LHU model into traditional EKF, in which the nonlinearity is tackled by model switch from LHU to SHU as the heading error converges to a few degrees [20, 21]. The limitation of this approach is the weak observability of the augmented trigonometric function errors in LHU model. Moreover, these methods abovementioned mainly handle nonlinearity, ignoring time-varying process noise and state disturbances due to sudden motion changes. Innovation-based or residual-based adaptive EKF seems to be one of the suitable methods to deal with time-varying non-additive noise

[22, 23]

. However, AKF is unreliable when dealing

with strongly nonlinear problems, which may occur in UAV applications. As a result of better accuracy and reliability, nonlinear estimation algorithms such as unscented Kalman filter (UKF) and adaptive unscented Kalman filter (AUKF) for GNSS/INS integration have received increased attention recently [23-25]. Shin et al. propose a promising UKF algorithm to tackle the nonlinearity problem in large heading uncertainty for low-cost MIMU-based navigation in land applications [24, 25]. The introduction of unscented transformation (UT) into GNSS/MIMU integrated navigation reduces the linearization error, which handles the nonlinearity problem and overcomes the drawbacks such as low accuracy and poor stability in EKF [24, 25]. An AUKF algorithm is proposed employing online noise statistic estimator and the adaptive filtering principle into UKF to restrain the system nonlinearity

[23]

. These works presented several referenced nonlinear navigation filtering

algorithms for GNSS/MIMU integration. However, UKF is established based on the intuition that it is easier to approximate a probability distribution than it is to approximate an arbitrary nonlinear function or transformation [26, 27]. The weights of sigma points produced by UT could be minus in handling high dimensional systems (dimensionality>4). This will lead to the non-positive definite covariance matrix during the filtering process, which causes numerical instability or even divergence of filter in turn [28]. Moreover, these methods above-mentioned still need verifying by flight experiment with low-cost MEMS-based sensors.

3

As a latest development in nonlinear Kalman filter, CKF is a new nonlinear filter for highdimensional state estimation with better numerical stability and accuracy [29]. This filtering algorithm is mathematically derived based on Bayesian theory and Spherical-Radial Cubature rule. Unlike UKF, CKF’s numerical stability and accuracy benefit from all positive Cubature point weights when propagating the states and covariance matrix, which guarantees the positive definiteness of the covariance matrix of the filtering process

[29]

. Due to derivative-free Cubature rules, the CKF and its

extensions have been used to deal with strong model nonlinearity problems such as target tracking [30-33] and land-based navigation [34-39]. The CKF is applied to a GNSS/INS tightly-coupled integration with enhanced navigation performance for land vehicles as shown in [30]. A fading factor-based adaptive CKF aided by fuzzy control or artificial neural networks enhances the applicability of land-based navigation system in [31, 32]. In [33, 34], a robust strong tracking CKF method can be applied in the spacecraft attitude estimation. It is worth mentioning that CKF is initially derived with the additive noise assumption in the non-augmented form. The augmented-form of CKF is demonstrated by simulation for tracking problem in [35, 36, 37, 38], and a two-stage CKF is proposed for nonlinear systems to avoid dimension disaster in [37]. However, it may be an unnatural to describe the system noise as additive for INS-based navigation, due to the effect of both bias and scale factor noise on the IMU raw measurements

[24]

. An augmented CKF with non-additive correlated noise is developed and

validated by simulation in [39], which still needs experiment demonstration. Currently, CKF-based UAV navigation is still rarely documented in the literatures. It is still a challenge to realize optimal estimation for strongly nonlinear system with non-additive noise and state disturbances in newlyemerging UAV research field, which is the subject of this paper. Motivated by the above studies, this paper addresses the nonlinear estimation problem for UAV-borne RTK/MIMU integrated system in the presence of model nonlinearity and time-varying process noise. We propose an augmented Cubature Kalman filter (CKF) scheme to tackle the airborne RTK/MIMU integrated navigation. By augmenting state and noise vector, the noise and its effect on the covariance propagation are described as its functional mechanism, thus avoiding biased estimation caused by inaccurate noise statistics

[24, 40]

. Based on this property, we adopt the state augmentation approach to

describe the non-additive noise naturally. Combining all these terms, the proposed navigation approach can not only handle strong model nonlinearity but also resist the adverse influence of non-additive

4

process noise on filtering in a derivative-free, numerically stable and accurate manner. Moreover, the effectiveness of the proposed scheme is demonstrated via flight test. The remainder of this paper is organized as follows. Section 2 presents the nonlinear system model of RTK/MIMU integration. Section 3 develops a new augmented Cubature Kalman filter approach for integrated navigation with non-additive process noise. Flight test and conclusions are given in Section 4 and 5. The contribution of this paper is to provide an effective way to tackle the strong model nonlinearity and complicated noise characteristics for MEMS-based airborne navigation.

2. RTK/MIMU Integrated Navigation System Nonlinear modeling 2.1. State dynamic model Most of the low-cost MIMUs usually output the angular rate, ωibb , and the specific force, f b , directly. The full-dimensional navigation state (i.e. position, velocity, and attitude, etc.) of a vehicle can be obtained using these sensed measurements through integral operations. The inertial measurements are inevitably contaminated by systematic sensor errors and noise. Therefore, it is important to properly remove most of the systematic errors before the start of navigation. The inertial sensor measurements calibrated is still affected by stochastic residuals as follows. ωibb (t)= (1 + Sω ) ⋅ ωibb (t) + δωibb  b b b  f (t)= (1 + S f ) ⋅ f (t) + δ f

(1)

where ωibb and f b are the reference output of gyro and accelerometer; Sω and S f are the scale factor (SF) errors of gyro and accelerometer; δωibb and δ f b are the output error of gyro and accelerometer, which are the residuals after calibration; Assume zero-bias and SF error as well as white noise exist in MEMS sensor output residuals, the residuals are denoted as follows. δωibb = ε b + wφ  b b δ f = ∇ + wV   Sω = γ S = η  f

(2)

T

T

where wV =  wVx , wVy , wVz  is the accelerometer white noise and wφ =  wφ x , wφ y , wφ z  is the gyro white noise; ∇b = ∇bX ,∇Yb ,∇bZ 

T

is the accelerometer zero-bias and ε b = ε bX ,ε Yb ,ε Zb 

T

T

T

is the gyro zero-bias;

η b = η Xb ,ηYb ,η Zb  is the accelerometer SF error and γ b = γ bX ,γ Yb ,γ Zb  is the gyro SF error. The zero-bias

error and SF error could be modeled as a first-order Gauss-Markov process [24].

5

1 b  b ε& (t)= − T ε (t) + wgb (t) gb   b & (t)= − 1 ∇ b (t) + w (t) ∇ ab Tab   γ& (t)= − 1 γ (t) + w (t) gs  ω Tgs  1 & η (t)= − T η (t) + was (t)  as

(3)

where T⋅b represents the correlation time for inertial sensors’ bias instability; w⋅b represents the corresponding driven white noise for zero-bias process, and w⋅s represents the corresponding driven white noise for SF process. “ ⋅ ” can be “g” (gyro for short) or “a”(accelerometer for short). The corresponding statistical parameters (i.e. T , wb , ws , wV and wφ ) can be obtained by ARMA or Allan variance analysis. INS mechanization equation in the navigation frame (n-frame) is established as the system model.   RM1+ h  r& n =D −1v n   n n b n n n n −1 v& =Cb f − (2ωie + ωen ) × v + g ，D =  0   1  0 q& bn = q nb o [0,(ω ibb − Cbnωinn )T ]T  2

where r n = [ϕ , λ , h]T is

the

position

vector

composed

of

0 1 ( RM + h ) cos ϕ

0

0  0  −1 

latitude ϕ ,

(4)

longitude λ and

altitude h ; v n = [v N , vE , vD ] is the velocity vector resolved in North-East-Down directions; q bn = [ q1 , q2 , q3 , q 4 ]T represents r n =Cbn r b = q bn o r b o (q bn )* ; RM =

the quaternion from body frame (b-frame) to n-frame, i.e. a (1− e 2 )

(1 − e 2 sin 2 ϕ )3 2

is the radius of curvature in prime vertical and RN = (1− e sina 2

2

ϕ )1 2

is the

radius of curvature in meridian, in which a is the length of the Earth reference ellipsoid semi-major axis and e is eccentricity of the Earth reference ellipsoid; g is gravity vector; ωibb and f b are the noisy outputs measured by gyro and accelerometer. T Now define w (t )=  wv

wφT

( wgb )T

( wab )T

( wgs )T

T

( was )T  as the system noise vector, and

x (t )=[ϕ , λ , h, vN , vE , vD , q1 , q2 , q3 , q4 , ε X ,ε Y ,ε Z ,∇ X ,∇Y ,∇ Z ,γ X ,γ Y ,γ Z ,η X ,ηY ,η Z ]T as the system state vector. The discrete state-space nonlinear difference equation is derived by conducting discretization operations on Eq. (1) ~ (4) in matrix formulation. x k = f k −1 ( xk -1 , w k −1 )

(5)

E [ wk ] = 0 E  w k w Tj  = Qδ kj

(6)

In which,

6

where xk =x (tk ) = x (k ⋅ ∆t ) is the state vector at time point k , i.e. (tk = k ∆t ) ; fk −1 (⋅) is the nonlinear system transformation function at k − 1 epoch, and f k −1 : R n + n → R n , in which nx is the dimensionality of the x

w

x

state x k and nw is the dimensionality of the noise vector w k ; xk −1 =x(tk −1 ) = x ( (k − 1)∆t ) ; ∆t is the time interval between two consecutive MIMU output, which is usually small due to INS high data rate; E[∗] is the expectation operator, and δ (t − τ ) is Kronecker Dirac function. It should be noted that the

non-additive noise in raw measurements go through the nonlinear transformation generating nonlinear noise terms in estimated state.

2.2. System observation model Both GNSS and MIMU can output the vehicle’s position (i.e., latitude, longitude and altitude). RTK T

positioning Z k = [ϕG λG hG ] is chosen as the system observation, considering its long-term stably high accuracy. However, the GNSS carrier phase center and MIMU center are not coincident. The vector lbG from MIMU center to GNSS center is defined as the lever arm, as shown in Fig.1. rGn = rMn + Cbn lbG

(7)

where rMn = (ϕ M , λM , hM ) is the latitude, longitude and altitude of MIMU center; rGn = (ϕG , λG , hG ) is the position of GNSS center. So the system observation model is obtained as follows.

Z k = hk ( xk ) + vk

(8)

= H k xk + v k

In (8), Z k =Z (tk ) = Z (k ⋅ ∆t ) is the observation vector at k epoch (tk = k ∆t ) ; hk (⋅) = H k =  I3×3 03×(3×6 )  is the system observation matrix at k epoch; and hk : R n

x

+ nw

→ R m ,in which m is the dimensionality of

measurement vector; and white noise vk represents the measurement noise. E[v k ] = 0, E  v k v Tj  = Rδ kj

(9)

2 2 2 , σ lon ,σ alt ]} R = diag{[σ lat

Cov ( wk , v j ) = 0

In (9), σ lat ,σ lon and σ alt are the standard deviation of position errors (i.e. latitude, longitude, and altitude) for RTK positioning. Usually, RTK data rate is much lower than MIMU data rate, say 1Hz or less. It should be noted the use of velocity measurement would definitely improve the observability of INS attitude and instrument errors, which improves the accuracy in estimating system states

[41]

. Based

on our experience in dealing with actual experimental data, the “position-only” measurement mode has the same position and velocity accuracy as the full “position + velocity” mode. Considering its attitude

7

accuracy is acceptable in actual applications, we choose the “RTK position-only” mode as the measurement model in this paper. Based on the nonlinear state-space model (i.e., Eq. (5), (6), (8) and (9)), the Cubature Kalman filter can be devised for RTK/MIMU integration. The algorithm structure is shown in Fig. 2.

Fig.1 The lever arm from MIMU center to GNSS carrier phase center

Fig.2 RTK/MIMU integrated navigation algorithm schematic

3. Augmented Cubature Kalman filtering for Non-additive process noise A natural solution for UAV system model nonlinearity problem in LHU condition is to devise a nonlinear filter. As a new precise nonlinear filtering method, CKF can handle the strong system nonlinearity, which benefits from an efficient numerical integration approach called Cubature rules for multi-dimensional integrals with minimal computational effort. CKF is originally designed with additive noise assumption in the non-augmented formulation. However, the additive noise assumption may not hold true in practical applications, which causes conventional CKF some accuracy degradation. Based on this fact, the augmented CKF should be derived to tackle the nonlinear estimation problem in the presence of the non-additive process noise.

3.1. Recursive Bayesian Estimation for Nonlinear Gaussian System with Non-additive noise For the sake of clear derivation, the RTK/MIMU integrated system (nonlinear Gaussian system) is expressed as follows.

 xk = f k −1 ( xk -1 , wk −1 )   Z k = hk ( xk ) + vk In Eq. (10), the noise wk and v k are independent and their statistics are depicted in Eq. (11).

8

(10)

 E[ wk ] = 0, Cov ( wk , w j ) = Qkδ kj    E[v k ] = 0, Cov ( v k , v j ) = Rk δ kj  Cov( wk , v j ) = 0

(11)

CKF is originally derived on the assumption of additive noise, which would undergo accuracy degradation in the presence of non-additive noise. In this paper, this problem will be revealed with flight data analysis in Section 4.2, and tackled by state augmentation. The state augmentation method can describe the noise in a natural way, and the augmented state is denoted as x a (t )=[x (t )T , w (t )T ]T . Assume the old posterior density p ( xk −1 | Dk −1 ) of state xk −1 at tk −1 epoch is Gaussian, its mean and covariance are xˆ k −1 and Pk −1 . Here, Dk −1 = {Z i }(i =1 ) denotes the history of measurements up to time t k −1 . k −1

Since wk −1 is a Gaussian sequence and independent with Dk −1 , p ( xka−1 | Dk −1 ) is also Gaussian with mean and covariance as Eq. (12) and Eq. (13).

 E [ xk −1 | Dk −1 ]   xˆ k −1  xˆ ka−1 = E  xka−1 | Dk −1  =  =   E [ wk −1 | Dk −1 ]  0 

(12)

0  T P Pka−1 = E ( xˆ ka−1 − xka−1 )( xˆ ka−1 − xka−1 )  =  k −1     0 Qk −1 

(13)

According to the principle of minimum variance estimation, the predictive state of nonlinear Gaussian system in Eq. (10) is shown below. xˆ k / k −1 = E [ x k | Dk −1 ] = E [ f k −1 ( x k -1, w k −1 ) | Dk −1 ] = E  f k −1 ( x ka−1 ) | Dk −1  =

∫

f k −1 ( xka−1 ) p ( xka−1 | Dk −1 )dxka−1

Rn x + nw

(14)

f k −1 ( x ka−1 ) N ( x ka−1; xˆ ka−1 , Pka−1 )dx ka−1

∫

=

R nx + nw

where N ( ⋅; ⋅, ⋅) is the conventional symbol for a Gaussian density. Similarly, we obtain the error covariance T Pk / k −1 = E  x% k / k −1 x% kT/ k −1  = E ( xk − xˆ k / k −1 )( xk − xˆ k / k −1 )   

∫ (f

= R

T

k −1

( xka−1 ) − xˆ k / k −1 )( f k −1 ( x ka−1 ) − xˆ k / k −1 ) N ( xka−1 ; xˆ ka−1 , Pka−1 )dx ka−1

(15)

n x + nw

Assume the predictive density p ( x k | Dk −1 ) follows Gaussian distribution when xˆ k / k −1 and Pk / k −1 known, the mean of predicted measurement is given by Ζˆ k / k −1 = E [ Z k | Dk −1 ] = E [ hk ( x k ) + v k | Dk −1 ]

∫

= E [ hk ( xk ) | Dk −1 ] =

hk ( xk ) p ( x k −1 | Dk −1 )dx k

Rn x + nw

=

∫

hk ( xk ) N ( xk ; xˆ k / k −1, Pk / k −1 )dxk

R nx + nw

9

(16)

Meanwhile, vk is uncorrelated with Ζˆ k / k −1 and x% k / k −1 , as wk and vk are uncorrelated white noise. The associated auto-covariance and cross-covariance are given by T PΖ% k = E  Ζ% k / k −1Ζ% kT/ k −1  = E  Z k − Ζˆ k / k −1 Z k − Ζˆ k / k −1   

(

)(

)

= E  hk ( xk ) − Ζˆ k / k −1 hk ( xk ) − Ζˆ k / k −1  + Rk  

( = ∫ ( h ( x ) − Ζˆ k

R

=

k

T

k / k −1

)( )( h ( x ) − Ζˆ

k / k −1

) )

k / k −1

)( h ( x ) − Ζˆ

k / k −1

)

k

k

T

p ( x k | Dk −1 )dxk + Rk

T

N ( xk ; xˆ k / k −1 , Pk / k −1 )dx k + Rk

(17)

nx

∫ ( h ( x ) − Ζˆ k

k

k

k

R nx T Px% k Ζ% k = E  x% k / k −1Ζ% kT/ k −1  = E ( x k − xˆ k / k −1 ) Z k − Ζˆ k / k −1   

(

)

= E ( xk − xˆ k / k −1 ) hk ( xk ) − Ζˆ k / k −1    =

∫ (x

k

− xˆ k / k −1

( ) ( h ( x ) − Ζˆ k

k

T

) ) N ( x ; xˆ

(18)

T

k / k −1

k

k / k −1

, Pk / k −1 )dxk

R nx

where Ζ% k / k −1 =Z k − Ζˆ k / k −1 is the innovation sequence. For linear or nonlinear system whose state follows the Gaussian distribution, the minimum variance estimate is the linear function of measurement. So the posterior state estimate xˆ k can be given by

xˆ k = xˆ k / k −1 + K k Ζ% k / k −1

(19)

where K k is the Kalman gain matrix. Accordingly, x% k =x k − xˆ k =x% k / k −1 − K k Ζ% k / k −1 is the state estimate error. The estimate error covariance is expressed as T Pk = E  x% k x% kT  = E ( x% k / k −1 − K k Ζ% k / k −1 )( x% k / k −1 − K k Ζ% k / k −1 )    T T T T = E  x% k / k −1 x% k / k −1  − E  x% k / k −1Ζ% k / k −1  K k − K k E  Ζ% k / k −1 x% k / k −1  + K k E  Ζ% k / k −1Ζ% kT/ k −1  K kT (20) = Pk / k −1 − Px% k Ζ% k K kT − K k Px%Tk Ζ% k + K k PΖ% k K kT

Based on minimum mean square error criterion, the gain matrix K k can be obtained by minimizing the trace of the error covariance matrix Pk in Eq. (20), i.e.

K k =Px%kΖ% k PΖ−% k1Ζ%k

(21)

Pk = Pk / k −1 − Px% k Ζ% k K kT =Pk / k −1 − K k Px%Tk Ζ% k =Pk / k −1 − K k PΖ% k K kT

(22)

Thus, the minimum of Pk is obtained,

For a nonlinear Gaussian system with non-additive noise as shown in Eq. (10), the state estimation time update is expressed in Eq. (14) ~ (16). The measurement update consists of Eq. (17) ~ (22).

3.2. Augmented Cubature Kalman filtering for strong nonlinearity of low-cost MIMU navigation

10

According to Section 3.1, there are no special restrictions on the system linearity in deriving the recursive Bayesian estimation algorithm. In fact, the Bayesian filter is an optimal estimation method for both linear and nonlinear system. However, it is very difficult to obtain accurate solution for optimal recursive Bayesian filtering in practice, due to complexity of multi-dimensional integral for the posterior state probability density

[29]

. Therefore, an efficient numerical integration approach called

Cubature rules is proposed for the implementation of the recursive Bayesian estimation, by which the Cubature Kalman filtering is realized for multi-dimensional nonlinear Gaussian system [29]. The importance of three-degree Cubature rules over higher-degree rules has been emphasized in the literatures [29]. This criterion transforms the Gaussian integral to a simplified form as follows.

I N (f ) =

2n

∫ f ( x ) N ( x; 0 , I )dx ≈ ∑ ω f (ξ ) i

i

i =1

Rn

(23)

1 ξ i = n [1]i , ωi = , i = 1, 2,K 2n 2n In the formula, [1] ∈ R n denotes the following set with 2n points:  1   0   0   −1  0   0             0 1 0 0 1 −           0   .   .  .  .   .   .   L， L，  ，  ，  ，  ，    ， . . . . .       .      .  .   .   .   .   .               1  0   0   −1    0   0 

(24)

Based on Cubature points {ξi ,ωi } , the Bayesian solutions in Eq. (15) ~ (18) can be approximated efficiently as the implementation of Cubature Kalman filtering. a a Suppose the augmented state and its error covariance for UAV at time t k −1 are xˆ k −1 and Pk −1 ,

xˆ ka−1 = E  xka−1 

T Pka−1 = E ( xˆ ka−1 − xka−1 )( xˆ ka−1 − xka−1 )   

(25)

The augmented CKF-based RTK/MIMU integrated navigation is presented here. Step 1. Cubature points generation The point set {χia,k −1 | i = 1,L, 2n} is generated by the following algorithm.

χ ia, k −1 = xˆ ka−1 + Pka−1ξi

(i = 1,L , 2 n)

where n represents the dimensionality of the augmented state;

Pka−1 is the square-root matrix of the

covariance matrix Pka−1 ; ξ i is obtained according to the Eq. (23) and (24). Step 2. Time Update

11

(26)

χ i , k |k −1 =f ( χ ia, k −1 ) xˆ k |k −1 = Pk |k −1 =

1 2n ∑ χ i , k |k −1 2n i =1

xˆ ka/ k −1 = (xˆ k |k −1 )T

1 2n ∑ χ i , k |k −1χ iT, k |k −1 − xˆ k |k −1 xˆ kT|k −1 2 n i =1

0 

T

0 P Pka|k −1 =  k |k −1 Qk   0

(27)

(28)

where xˆ k |k −1 is the non-augmented predicted state at time tk , with corresponding covariance matrix Pk |k −1 ; xˆ ka/ k −1 and Pka|k −1 are the counterparts in the augmented form.

In Eq. (27), special attention should be paid to the position and attitude quaternion components in χ i ,k | k −1 . For position components, transformation from geodetic coordinates (latitude, longitude, altitude) to Earth-centered Earth-fixed reference (ECEF, e-frame) coordinates (X, Y, Z) should be completed before the averaging.

rˆke|k −1 =

1 2n e ∑ rˆi , k |k −1 2n i =1

(29)

For quaternions {qi | i = 1, 2,L,2n} , the mean and error covariance of attitude quaternion should be obtained using the following algorithm: 1. Choose an arbitrary qi (e.g., i = 1 ) as the initial value of the mean quaternion q ; 2. Compute the difference between qi and q using qφ ,i =qi o q −1 ; 3. Transform the attitude difference qφ , i to the corresponding rotation vector φi ; 4. Compute the mean rotation vector φ =

1 2n ∑φi ; 2n i =1

5. Transform φ to the corresponding mean quaternion qφ ; 6. Update the mean quaternion q =qφ o q ; 7. Repeat from step 2 to step 6 until φ is below the threshold (e.g. 1e-7). The corresponding predicted covariance matrix Pq , k / k −1 of attitude quaternion error is

Pq , k / k −1 =

1 2n ∑[qi o q −1 ][qi o q −1 ]T 2 n i =1

(30)

Step 3. Measurement Update

Ζ i , k |k −1 =h ( χ i , k |k −1 ) Ζˆ k / k −1 = PΖ% kΖ% k = k

k

1 2n ∑ Ζ i , k |k −1 2n i =1

1 2n ∑[ Ζ i, k |k −1 − Ζˆ k / k −1 ][Ζ i , k |k −1 − Ζˆ k / k −1 ]T +Rk 2n i =1

12

(31)

(32)

Px% kΖ% k = k

1 2n ∑[ χ i, k |k −1 − xˆ k |k −1 ][ Ζ i , k |k −1 − Ζˆ k / k −1 ]T 2n i =1

(

xˆ k =xˆ k / k −1 +K k Ζ k − Ζˆ k / k −1

)

xˆ ka = (xˆ k )T

0 

P Pka =  k 0

0 Qk 

Pk =Pk / k −1 − K k PΖ% k Ζ% k K kT k

k

(33)

T

K k =Px% kΖ%k PΖ−% k1Ζ% k

(34)

(35)

where xˆ k |k is the non-augmented estimated state at time tk , with corresponding estimated covariance matrix Pk |k ; xˆ ka/ k and Pka|k are the counterparts in the augmented form.

3.3. Discussion on the difference between augmentation and non-augmentation CKF is a straightforward extension of Cubature rules to Bayesian recursive estimation [29]. For both non-augmented CKF and augmented CKF, the prediction stage consists of two consecutive Cubature transformations (CT): the first CT is closely related to the system equation and the second one is to the observation equation. The distinction between the two CKFs is the second CT. For non-augmented CKF, the Cubature points generated by the first CT only contain the statistical information of f ( x ) , ignoring the effect of system noise. In order to incorporate the process noise into the predicted covariance matrix, the non-augmented CKF has to regenerate a new set of Cubature points before the second CT. For augmented CKF, the Cubature points generated by the first CT contain the information of both f ( x ) and system noise. As a result, these points can be used directly to propagate through the observation equation. One advantage brought by augmented CKF is to reduce the computation burden of Cubature point generation. Moreover, part of the odd-moment information can be captured and propagated in the filtering recursion, whereas the non-augmented CKF cannot capture the odd-moment information. It is difficult to give a strict mathematical proof for the meaningful odd-moment information capture capability in augmented CKF in general case. Therefore, a scalar example is used to investigate the difference of two formulations in three-order moments capture, similar to the analysis in the literature [40]. Assume one-dimensional random variable y is connected with another one-dimensional random variable x ~N ( x ,σ 2 ) as follows.

y =x 2

13

(36)

The random variable x can be written as x =x +δ x , in which δ x~N ( 0,σ 2 ) . The first, second and third moments of random variable y are listed here.

2 Ey =E ( x +δ x )  =x 2 + σ 2  

(37)

2

2 2 E ( y − Ey )  =E  2xδ x + (δ x ) − σ 2  =4x 2σ 2 + 2σ 4    

(38)

3

3 2 E ( y − Ey )  =E  2xδ x + ( δ x ) − σ 2  =24x 2σ 4 + 8σ 6    

(39)

In this case, the non-augmented Cubature set has two points {χ1 , χ 2} = { x + σ , x − σ } . The transformed points are {χ1 , χ 2} = { x + σ , x − σ } . The statistical first, second and third moments of y are

2

yˆ = ∑ ωiγ i =x 2 + σ 2 = Ey

(40)

i =1 2

∑ ω (γ i

2

i

− yˆ ) =4x 2σ 2

(41)

i =1

2

∑ ω (γ i

3

i

− yˆ ) =0

(42)

i =1

As for augmented Cubature set, due to the state augmentation of the additive noise w~N ( 0,σ w2 ) , it has

{

T

T

T

four points {χ1 , χ2 , χ3 , χ4 } =  x , 2σ w  ,  x + 2σ ,0 ,  x , − 2σ w  ,  x − 2σ ,0

T

} . The transformed set

is {γ 1, γ 2 , γ 3 , γ 4 } = { x 2 , x 2 , x 2 + 2 2 xσ + 2σ 2 , x 2 − 2 2 xσ + 2σ 2} , which can be used to compute the first, second and third moments of y .

4

yˆ = ∑ ωiγ i =x 2 + σ 2 = Ey

(43)

i =1 4

∑ ω (γ i

i

i =1

3 2 − yˆ ) =4x 2σ 2 + σ 4 2

2

∑ ω (γ i

3

i

− yˆ ) =12x 2σ 4

(44)

(45)

i =1

By comparing the Eq. (39), (42) and (45), it can be seen that the augmented Cubature set after nonlinear transformation does capture part of the meaningful third-order moment information, while the non-augmented Cubature set cannot capture it. Therefore, it is anticipated that the augmented CKF has better performance in accuracy compared with the conventional non-augmented CKF.

4. Simulation 14

In order to make a comprehensive analysis of the proposed algorithm, some simulation data is applied to the five filtering methods. In the simulation, the vehicle is in the clockwise uniform circular motion on a slope of 30 degrees. The radius of the circle is 100m and the tangential velocity is 10m/s. The simulation trajectory is illustrated in Fig.3 (a). Such a simulation scenario can guarantee the observability of filter states [41]. The navigation comparison during the uniform circular motion period is the main concern in this simulation. Some noises are introduced to simulate a RTK/MIMU integrated navigation system. For MIMU simulation, stochastic errors such as bias and scale factor are introduced into the IMU (i.e. accelerometers and gyros) 50Hz raw measurements. It is assumed that the gyro bias repeatability is 3.0deg/s, the bias instability is 0.007deg/sec, which is generated by a 1st-order Markovian process, and angular random walk and scale factor are white noise with standard deviation 2.0deg/√hr and 500ppm. It is assumed that the accelerometer is of bias repeatability 50mg, bias instability 0.2mg, velocity random walk 0.2m/sec/√hr and scale factor 300ppm white noise. For 1Hz RTK positioning simulation, it is assumed that the positioning errors are white noise with standard deviation 0.05m in horizontal directions and 0.1m in vertical directions. In order to evaluate the performance among different algorithms, filter tuning parameters such as P0 , Q and R should be set identically for these five filtering methods. The vehicle navigation starts from a kinematic status with a sound position and attitude initialization. The process noise covariance Q corresponds to inertial sensors simulation. The measurement noise matrix R corresponds to RTK positioning noise simulation, in which the standard deviation related to the RTK positions is [0.05m,0.05m,0.1m]T. The initial position, velocity and position standard deviations are set as 1.0m, 0.5m/s and 15deg in P0 , which is substantially larger than the true standard deviations in order to maintain stability. The simulation data is executed to examine the performance difference among the EKF, nonaugmented CKF, augmented CKF, non-augmented UKF and augmented UKF methods. The same data is implemented with these methods separately. The 3-dimensional position, velocity and attitude comparisons with the simulation reference data are shown in Fig. 3 (b) ~ (d) and Table 1, which are the average of the absolute estimation errors. Here the norm of the misalignment angles is adopted as the indicator of overall errors of attitude (i.e., yaw, pitch and roll).

15

0.8

EKF CKF UKF CKF-extended UKF-extended

150 0.7

Position Error(m)

Up(m)

0.6

100

50 50

0

-50

-100

0

100

0.5 0.4 0.3 0.2 0.1

East(m) North(m)

0 0

(a) The trajectory of circular motion simulation

50

100

150

200 Time(sec)

250

300

350

400

(b) Error of estimated 3-dimensional positions

0.8 EKF CKF UKF CKF-extended UKF-extended

0.7

Attitude Error(deg)

Velocity Error(m/s)

0.6

EKF CKF UKF CKF-extended UKF-extended

20

0.5 0.4 0.3 0.2

15

10

5

0.1 0

0 0

50

100

150

200 Time(sec)

250

300

350

400

0

(c) Error of estimated 3-dimensional velocity

50

100

150

200 Time(sec)

250

300

350

400

(d) Error of estimated 3-dimensional attitude

Fig.3 Navigation performance comparison among five methods in inclined circular motion simulation

All the five methods can converge quickly as shown in Fig. 3 (b) ~ (d), and the estimation errors of position, velocity and attitude are given in Table 1. It can be seen that from Table 1 that there is an obvious improvements in accuracy for augmented UKF and CKF, compared with the conventional EKF and non-augmented UKF/CKF methods. Moreover, it should be noted that all the tuning parameters such as Q and R are set same for all five filtering methods. However, their system models are totally different. Specific speaking, the EKF uses the linearized error-state model with additive noise to estimate real-time navigation states, while UKF/CKF uses nonlinear state model. The difference between two system modeling lies in whether the process noises are coupled with the state or the nonlinear process noises exist. As a consequence, the Q and R in CKF/UKF are different from those in EKF. This fact explains why non-augmented CKF/UKF outperforms EKF in attitude accuracy significantly, while the errors in position and velocity are a little larger than EKF. Augmented

16

CKF/UKF benefits from state augmentation in that they describe the effect of process noise in a nonadditive way, so the same Q and R as EKF are suitable.

As shown in Table 1, the augmented CKF has almost the same accuracy as the augmented UKF. A comprehensive analysis of the algorithm can be achieved by the simulation results under some ideal conditions, but it still needs experiments demonstrating which is presented in Section 5. Table 1 Navigation comparison among the five methods in inclined circular motion simulation

RMSE

EKF

Nonaugmented CKF

Nonaugmented UKF

Augmented CKF

Augmented UKF

Position (m)

0.0852

0.1035

0.1036

0.0753

0.0757

Velocity (m/s)

0.0514

0.0537

0.0537

0.0274

0.0277

Attitude (deg)

3.6017

2.2424

2.2441

1.8218

1.8065

5. Field Test A UAV flight test is conducted to verify the aforementioned analysis in Section 3.3 and test the performance of the proposed algorithm. A prototype system of RTK/MIMU integrated navigation is established to collect both GNSS raw measurements and inertial data during the flight test. In the prototype system, the low-cost MIMU ADIS16405 is used for inertial measurement with its noise characteristics listed in the Table 2, and the GNSS receiver, which is based on NovAtel OEMV-3 Card (main feature parameters listed in Table 3) with HX-BS581 (Harxon) GNSS multi-frequency antenna, is used for GNSS measurement. The field test and its equipment are shown in Fig. 4. The field test is carried out in an open area without GNSS blockages (The Moon Island, Changsha), about 10km away from the base station. Clockwise and anti-clockwise circular movements are chosen to enhance the observability of the filter state, which could improve the navigation accuracy. During the test, the max velocity of UAV can reach 115km/h, and the max sideslip velocity is about 48km/h.

17

Table 2 ADIS16405 IMU noise parameters Gyro

Table 3 OEMV-3 Card parameters

Accelerometer

Frequency Point

Triple-frequency: L1/L2/L3

Bias

3.0deg/sec

50mg

Time to First Fix

60sec(Cold Start); 35sec(Warm Start)

Bias Instability

0.007deg/sec

0.2mg

Power Consumption

2.1W

Random Walk

2.0deg/√hr

0.2m/sec/√hr

Data rate

20Hz (50Hz for optional)

(a) Small flying-wing

(b) flying UAV in field test

(c) Airborne rover equipment

(d) Base station

(e) RTK/MIMU integration prototype Fig.4 Flight test equipment and experiment

5.1. UAV Kinematics Analysis

18

Ideally, the true state (i.e. positions, velocity and attitude, etc.) of UAV should be acquired by another independent high-accuracy GNSS/INS reference system. However, the test UAV is too small to hold another reference system. It is necessary to select another accurate and reasonable reference to evaluate MEMS-based navigation performance. Considering the high accuracy of Rauch-Tung-Striebel (RTS) smoothing technique even in the GNSS outages, it is reasonable to choose the RTS smoothing results of GNSS post-processing kinematic (PPK) and MIMU solutions as the reference value. The whole flight reference is shown in Fig.5. Based on the reference, the kinematic characteristics can be analyzed for flight vehicle. Here, we define the ratio ρ slip =

vslip vtotal

as an indicator of sideslip effect at a single epoch. The UAV sideslip characteristics

are illustrated with different color points in Fig.6, in which the green points represent ρ slip < 0.2 , and yellow points are 0.2 ≤ ρ slip < 0.5 , and ρ slip ≥ 0.5 is shown in red points. The statistics shows that the mean of ρ slip is -0.1087 with standard deviation 0.2396 for the whole flight test. It can be seen that the red points are mainly distributed in the flight segments of large and rapid maneuvers. Reference Takeoff Point Landing Point

8200

160

140 8100 120 8000 Altitute (m)

North (m)

100

7900

80

60 7800 40 7700 20

7600 -6500

0 -6400

-6300

-6200 East (m)

-6100

-6000

-5900

0

100

200

300 400 Time (sec)

500

600

700

Roll (deg)

50 0 -50

0

100

200

300 400 Time (sec)

500

600

700 Pitch (deg)

50 0 -50

0

100

200

300 400 Time (sec)

500

600

700

20

Yaw (deg)

Down (m/s)

East (m/s)

North (m/s)

(a) Horizontal position and altitude

0 -20

0

100

200

300 400 Time (sec)

500

600

700

100 0 -100

0

100

200

300 400 Time (sec)

500

600

700

0

100

200

300 400 Time (sec)

500

600

700

0

100

200

300 400 Time (sec)

500

600

700

50 0 -50

200 0 -200

(b) Velocity and attitude

19

Fig.5 Flight reference for UAV test <0.2 <0.5 >0.5 RTK Takeoff Point Landing Point

150

Height (m)

100

50

0

-50 8200 -5800

8000

-6000 -6200

7800 -6400 East (m)

7600

-6600

North (m)

Fig.6 Sideslip analysis for UAV in 3-Dimension Trajectory

Based on the non-holonomic constraints vslip = 0 [4], the attitude errors are calculated by the in-flight coarse alignment algorithm at every epoch in the test, shown in Fig.6. It shows that these coarse attitude errors, especially the heading errors, are above 30 degrees when ρ slip >0.5 . Both Fig.5 and Fig.6 show that the non-holonomic constraints of land vehicles hardly hold true for aerial vehicles. The in-flight coarse alignment will inevitably lead to significant initial attitude errors due to large sideslip. Large initial coarse alignment errors will cause significant linearization errors, which jeopardize the following MIMU fine alignment and integrated navigation. Consequently, model nonlinearity causes performance degradation or even divergence of the EKF. In such cases, CKF seems an effective option for RTK/MIMU integration. In this paper, CKF is evaluated with non-augmentation and augmentation in terms of both large and small initial misalignment conditions (i.e. SHU and LHU). Two representative flight segment data, i.e. SHU flight test data and LHU flight test data, are selected from the whole test according to the initial attitude errors. SHU flight segment starts from the red star (shown in Fig.7) to the end, which corresponds to 374sec epoch (i.e. 175340 GNSS second) with small sideslip ratio and small initial attitude errors, while LHU flight data begin with the red triangle (shown in Fig.7), which corresponds to 304sec epoch (i.e. 175270 GNSS second) with large sideslip ratio and large initial attitude errors. These two representative flight data listed in Table 3 are used to evaluate the performance of the proposed method in both SHU and LHU cases. All the tuning parameters are set same in the experiment to compare all these algorithms clearly.

20

Table 3 Representative flight segment selection Heading Uncertainty case

Flight time segment (Time)

Initial attitude errors (deg)

Sideslip ratio

R1: SHU flight test

Small

374s~694s

2.3185(yaw);1.0559(pitch);1.2606(roll)

0.1088

R2: LHU flight test

Large

304s~694s

33.2472(yaw);2.9161(pitch);-2.4738(roll)

0.5409

Yaw error/deg

Pitch error/deg

Roll error/deg

Representative data

40 20 0 -20 -40

50 0

0 100 200 300 400 500 600 700

-50

50

50

0

0

-50

0 100 200 300 400 500 600 700

-50

100

100

0

0

-100

0 100 200 300 400 500 600 700 Time/sec

-100

0

0.2

0.4

0.6

0.8

0

0.2

0.4

0.6

0.8

0

0.2 0.4 0.6 Side-slip ratio

0.8

Fig.7 Initial attitude errors for the whole flight test

5.2. Experimental Results: non-augmented CKF vs. augmented CKF In Section 3, it is anticipated that the augmented CKF has better performance in accuracy compared with the conventional non-augmented CKF. Here the performance of both filtering approaches under the SHU and SHU cases is evaluated based on two representative flight data aforementioned in Table 3.

5.2.1 SHU case The representative SHU flight data is chosen as the experiment data. The initial velocity of UAV is about 41.41km/h with 5.59km/h sideslip at the 175340 sec epoch. Fig.8 shows the performance comparison of the conventional non-augmented CKF and proposed augmented CKF in the SHU case.

21

Roll Error/deg

5

non-augmented CKF augmented CKF

2

1

0

0

50

100

150

200

250

300

Pitch Error/deg

Position Error (m)

3

350

2 1 0

0

50

100

150 200 Time (sec)

250

300

350

non-augmented CKF augmented CKF

-10

0

50

100

150

200

250

300

350

0

50

100

150

200

250

300

350

0

50

100

150 200 Time (sec)

250

300

350

5 0 -5 -10

3 Yaw Error/deg

Velocity Error (m/s)

4

0 -5

10 0 -10 -20

(a) Error of estimated 3-dimensional positions and velocity of

(b) Error of estimated attitude of conventional CKF and

conventional CKF and augmented CKF

augmented CKF

Fig.8 Integrated navigation performance comparison in SHU case: non-augmented vs. augmented

As is illustrated in Fig.8, the two CKF approaches can both converge but the augmented CKF performs better in unbiasedness and accuracy. Table 4 gives the navigation state accuracy comparison between these two methods for the SHU flight test. It can be learned from Fig.8 and Table 4 that there are improvements in the root mean square (RMS) of the navigation errors when adopting augmented CKF. The anticipation in the Section 3.3 is validated via SHU flight test. Table 4 Navigation RMSs of the conventional CKF and augmented CKF in SHU case

RMSE(>30s)

Non-augmented CKF

Augmented CKF

Roll (deg)

1.0490

0.6021

Pitch (deg)

2.0328

0.9959

Yaw (deg)

3.8618

1.3415

Position (m)

0.2996

0.2102

Velocity (m)

0.3519

0.2579

5.2.2 LHU case The representative LHU flight data is chosen as the experiment data here. The initial velocity of UAV is about 90.22km/h with 48.80km/h sideslip at the 175270 sec epoch. Fig.9 shows the performance comparison of the conventional non-augmented CKF and proposed augmented CKF in the LHU case.

22

10 Roll Error/deg

non-augmented CKF augmented CKF

2

1

0

0

50

100

150

200

250

300

350

Pitch Error/deg

Position Error (m)

3

400

4 Yaw Error/deg

Velocity Error (m/s)

6

2

0

0

50

100

150

200 Time (sec)

250

300

350

400

non-augmented CKF augmented CKF

5 0 -5

0

50

100

150

200

250

300

350

400

0

50

100

150

200

250

300

350

400

0

50

100

150

200 Time (sec)

250

300

350

400

5 0 -5 -10 20 0 -20 -40

(a) Error of estimated 3-dimensional positions and velocity of

(b) Error of estimated attitude of conventional CKF and

conventional CKF and augmented CKF

augmented CKF

Fig.9 Integrated navigation performance comparison in LHU case: non-augmented vs. augmented

Table 5 gives the navigation state accuracy comparison between these two methods for the LHU flight test. It can be learned from Fig.9 and Table 5 that there are obvious improvements in the RMSs of the navigation errors when adopting augmented CKF. Thus the anticipation in the Section 3.3 is also validated via flight test in LHU case. Table 5 Navigation RMSs of the conventional CKF and augmented CKF in LHU case

RMSE(>30s)

Non-augmented CKF

Augmented CKF

Roll (deg)

1.1563

0.7236

Pitch (deg)

1.9625

1.0505

Yaw (deg)

3.6820

1.3889

Position (m)

0.3072

0.2217

Velocity (m)

0.3629

0.2780

Combining Fig.8, Fig.9, Table 4 and Table 5 together, it can be learned that both methods can realize accurate navigation, and the accuracy is little affected by the heading uncertainty. This is because that the derivative-free CKF adopts Cubature rules to match the true mean and covariance of state vector correctly up to the third order, which reduces the linearization errors without establishing the Jacobians or Hessians with small heading uncertainty assumption. Moreover, augmentation is better than nonaugmentation in estimation accuracy in both LHU and SHU cases. The improvement with state augmentation brings two main benefits. Firstly, state augmentation describes the effect of the nonadditive noise on covariance propagation naturally. As an effective way to depict the effect of nonadditive system noise, this modification reflects the fact that the system noise is generated in the CKF

23

and goes through the nonlinear system process model, which favors the estimation accuracy. Secondly, the augmented CKF can capture meaningful odd-order moment information to improve the estimation accuracy, as illustrated in Section 3.3. The augmented CKF draws Cubature points only once in a recursion so that odd-order moment information is captured and propagated well within one recursion. The flight test results in both cases agree well with our anticipation.

5.3. Experimental Results: augmented CKF vs. conventional approaches According to Section 4.2, the augmented CKF has better performance in accuracy compared with the conventional non-augmented CKF in the non-additive process noise case. Here, the performance comparison is investigated among the proposed method and other conventional approaches (i.e. EKF/non-augmented UKF/augmented UKF), based on the representative flight data listed in Table 3.

5.3.1 SHU case Fig.10 shows the performance comparison of the conventional filtering methods and proposed augmented CKF in the SHU case. As is illustrated in Fig.10, the four filtering approaches can all converge but the augmented CKF and augmented UKF perform better in unbiasedness and accuracy. Table 6 gives the navigation state accuracy comparison between four methods for the SHU flight test. For the four methods, we can see that the positioning RMS of augmented CKF is about 0.2m with equivalent accuracy in augmented UKF, whereas the RMS of non-augmented UKF and EKF are greater than 0.3m and 0.6m. In addition, non-augmented CKF/UKF outperforms EKF in position and velocity accuracy significantly, while the errors in attitude are a little larger than EKF. As analyzed in Section 4, this is because the Q and R in EKF may not be the suitable tuning parameters for non-augmented CKF/UKF due to totally different state system model (i.e., non-additive noise). This fact, from another perspective, demonstrated the validity of augmentation. As illustrated in Fig.10 and Table 6, the augmented CKF improves the estimation accuracy of position, velocity and attitude angle significantly compared with EKF. The reason for the accuracy degradation of EKF is the abandon of high-order error terms, and this problem will be worsened when strong model nonlinearity occurs in LHU case, which can be seen in Section 4.3.2. Moreover, both Fig.10 and Table 6 show that there are obvious improvements in the RMSs of the navigation errors when adopting augmented CKF compared with non-augmented UKF.

24

EKF non-augmented UKF augmented UKF augmented CKF

5 Roll Error/deg

3 2

0

0

50

100

150

200

250

300

350

6

4 Yaw Error/deg

Velocity Error (m/s)

0

-5

1 Pitch Error/deg

Position Error (m)

4

2

0

0

50

100

150 200 Time (sec)

250

300

350

0

50

100

150

200

250

300

350

0

50

100

150

200

250

300

350

0

50

100

150 200 Time (sec)

250

300

350

5 0 -5 -10 10 0 -10 -20

(a) Error of estimated 3-dimensional positions and velocity of

(b) Error of estimated attitude of conventional methods and

conventional methods and augmented CKF

augmented CKF

Fig.10 Integrated navigation performance comparison in SHU case: conventional methods vs. augmented CKF Table 6 Navigation RMSs of the conventional methods and augmented CKF in SHU case

RMSE(>30s)

EKF

Non-augmented UKF

Augmented UKF

Augmented CKF

Roll (deg)

0.6340

1.0490

0.6014

0.6021

Pitch (deg)

1.0564

2.0325

0.9596

0.9959

Yaw (deg)

1.5488

3.8623

1.3037

1.3415

Position (m)

0.6520

0.2988

0.2074

0.2102

Velocity (m/s)

0.4296

0.3520

0.2556

0.2579

5.3.2 LHU case Fig.11 shows the performance comparison of the conventional filtering methods and the proposed augmented CKF in the LHU case. Table 7 gives the navigation state accuracy comparison between the four methods for the LHU flight test. As is illustrated in Fig.11 and Table 7, the result is consistent with that in SHU case, highlighting the superior performance of the augmented CKF in LHU case. Table 7 shows the positioning RMS of augmented CKF is about 0.20m with equivalent accuracy in augmented UKF, whereas the RMS of non-augmented UKF and EKF are greater than 0.30m and 0.68m. Compared with EKF, the augmented CKF experiences a significant improvement in the estimation accuracy of position, velocity and attitude. This accuracy promotion is due to the ability of CKF to address model nonlinearity. Compared with non-augmented UKF, there are obvious improvements in the RMSs of the navigation errors when adopting augmented CKF. This is because the augmented Cubature points evaluated could capture and propagate odd-order moment information well within one recursion and the augmented state describes the non-additive noise properly. Moreover, the phenomenon occurs again 25

that position errors of non-augmented CKF/UKF are much lower than EKF while the attitude errors are higher. As analyzed before, the Q and R determined in EKF can be directly used to system model with non-additive noise, but need to be tuned subtly for the additive noise description. This fact, from another perspective, demonstrated the validity of state augmentation. As is illustrated in Fig.10 and Fig.11, the performance of the augmented CKF and that of augmented UKF are almost same. However, scaled UT parameters (α , β , κ ) need to be adjusted carefully to generate a set of sigma points, while CKF is easy to obtain Cubature points according to the Eq. (23) [25]. Moreover, for high-dimensional state estimation, the presence of negative weights of UKF sigma points may lead to the non-positive definite covariance matrix, which could be disastrous in practical terms [24]; while CKF improves the numerical stability by adopting all positive weights in Cubature points. As a result, for the sake of implementation and numerical stability, the augmented CKF is the best integrated navigation algorithm among all these methods. EKF non-augmented UKF augmented UKF augmented CKF

Roll Error/deg

3

5

2

0

0

50

100

150

200

250

300

350

400

6

4 Yaw Error/deg

Velocity Error (m/s)

0 -5 -10

1 Pitch Error/deg

Position Error (m)

4

2

0

0

50

100

150

200 Time (sec)

250

300

350

400

0

50

100

150

200

250

300

350

400

0

50

100

150

200

250

300

350

400

0

50

100

150

200 Time (sec)

250

300

350

400

10 0

-10 20 0 -20 -40

(a) Error of estimated 3-dimensional positions and velocity of

(b) Error of estimated attitude of conventional methods and

conventional methods and augmented CKF

augmented CKF

Fig.11 Integrated navigation performance comparison in LHU case: conventional methods vs. augmented CKF Table 7 Navigation RMSs of the conventional CKF and augmented CKF in LHU case

RMSE(>30s)

EKF

Non-augmented UKF

Augmented UKF

Augmented CKF

Roll (deg)

0.7941

1.1565

0.7304

0.7236

Pitch (deg)

1.1255

1.9622

1.0109

1.0505

Yaw (deg)

1.4961

3.6829

1.3564

1.3889

Position (m)

0.6816

0.3073

0.2192

0.2217

Velocity (m/s)

0.4720

0.3630

0.2758

0.2780

26

All the flight test results show that the proposed augmented Cubature Kalman filtering method can improve the RTK/MIMU navigation performance in unbiasedness and accuracy effectively. Its accuracy is higher than the conventional EKF and non-augmented UKF methods, and consistent with the augmented UKF. However, CKF is much easier to implement than UKF in the algorithm complexity, and eases the burden on the curse of dimensionality compared with general augmented UKF. Moreover, the result comparison of SHU and LHU cases indicates that the proposed algorithm can realize RTK/MIMU integrated navigation in either large or small attitude errors of MIMU seamlessly without model changes.

6. Conclusion In this paper, a CKF-based integrated navigation scheme is proposed for low-cost MIMU aided by RTK to address large misalignment model nonlinearity and non-additive noise problem in small UAV navigation. An augmented CKF is derived to fulfil the data fusion. In the filter, cubature transformation is adopted to handle the strong MIMU model nonlinearity caused by sudden and large UAV maneuvers, and the technique of state-augmentation is used to capture meaningful odd-order moment information and reduce the impacts of non-additive noise in inertial measurements. Based on this scheme, several conclusions can be safely drawn via the representative flight tests. 1. The non-holonomic constraints of land vehicles hardly hold true for aerial vehicles; 2. The proposed augmented CKF performs better in unbiasedness and accuracy than the nonaugmented CKF when dealing with the nonlinear Gaussian system with non-additive process noise; 3. The accuracy of the proposed method is equivalent to that of the augmented UKF with advantages in numerical stability, easy implementation and little filter parameters adjustments; 4. The proposed method can complete more accurate navigation compared with the conventional EKF/UKF-based approaches. Moreover, it can realize RTK/MIMU integrated navigation in either large (LHU) or small attitude errors (SHU) seamlessly without model changes.

Acknowledgments The first author is grateful to Mr. MENG Liangsheng, the senior engineer of the Flight Dynamics and Control Group (National University of Defense Technology, China), for his valuable work in system integration and this makes the results of this paper stand the test of practice.

References

27

[1] Maria de Fatima Bento. Unmanned Aerial Vehicles: An Overview. InsideGNSS, 2008, 1, 54-61. [2] Xusheng Lei, Jingjing Li. An adaptive navigation method for small unmanned aerial rotorcraft under complex environment. Measurement, 2013, 46(10), 4166-4171. [3] Zhu Lihua, Cheng Xiaohong, Yuan Fuh-Gwo. A 3D collision avoidance strategy for UAV with physical constraints. Measurement, 2016,77, 40-49. [4] Dingjie WANG, Liang CHEN, Jie WU. Novel In-flight Coarse Alignment of Low-cost Strapdown Inertial Navigation System for Unmanned Aerial Vehicle Applications. Transactions of the Japan Society for Aeronautical and Space Sciences. 2016, 59(1): 10-17. [5] Sergio de La Parra, and Javier Angel. Low cost navigation system for UAV’s. Aerospace Science and Technology. 2005, 9(6), 504-516. [6] Xu Li, Xiang Song, Chingyao Chan. Reliable vehicle sideslip angle fusion estimation using low-cost sensors. Measurement, 2014, 51(5), 241-258. [7] H. Sheng, T. Zhang. MEMS-based low-cost strap-down AHRS research. Measurement, 2015, 59, 63-72. [8] Dah-Jing Jwo, Ta-Shun Cho. Critical remarks on the linearized and extended Kalman filters with geodetic navigation examples. Measurement, 2016, 43(9), 1077-1089. [9] Jan Wendel, Oliver Meister, Christian Schlaile, and Gert F. Trommer. An integrated GPS/MEMS-IMU navigation system for an autonomous helicopter. Aerospace Science and Technology. 2006, 10(6), 527-533. [10] Shaghayegh Zihajehzadeh, Darrell Loh, Tien Jung Lee, Reynald Hoskinson, Edward J. Park. A cascaded Kalman filter-based GPS/MEMS-IMU integration for sports applications. Measurement, 2015, 73, 200-210. [11] Tao Zhang, Xiaosu Xu. A new method of seamless land navigation for GPS/INS integrated system. Measurement, 2012, 45(4), 691-701. [12] Xiyuan Chen, Chong Shen, Weibin Zhang, Masayoshi Tomizuka, Yuan Xu, Kuanlin Chiu. Novel hybrid of strong tracking Kalman filter and wavelet neural network for GPS/INS during GPS outages. Measurement, 2013, 46(10), 3847-3854. [13] Lin Zhao, Haiyang Qiu, Yanming Feng. Analysis of a robust Kalman filter in loosely coupled GPS/INS navigation system. Measurement, 2016, 80, 138-147. [14] D.H. Titterton, John L. Weston. Strapdown Inertial Navigation Technology (2nd Edition). Peter Peregrinus Ltd., London, 2004, 277-307. [15] Savage, P. G. Strapdown Analytics (2nd Edition). Strapdown Associates, Maple Plain, MN, 2007, 228-241. [16] Ma, L., Wang, K. and Shao, M. Initial alignment on moving base using GPS measurements to construct new vectors. Measurement. 2013, 46(8), 2405-2410. [17] Silson, P.M.G. Coarse alignment of a ship’s strapdown inertial attitude reference system using velocity loci. IEEE Transactions on Instrument and Measurement. 2011, 60(6), 1930-1941.

28

[18] Wu, Y. and Pan, X. Velocity/Position Integration Formula Part I: Application to In-Flight Coarse Alignment. IEEE Transactions on Aerospace and Electronic Systems, 2013, 49(2), 1006-1023. [19] Wu M., Wu Y., Hu X. and Hu D. Optimization-based alignment for inertial navigation systems: Theory and algorithm. Aerospace Science and Technology. 2011, 15(1), 1-17. [20] Rogers, R.M. IMU In-Motion Alignment without Benefit of Attitude Initialization. Navigation, Journal of the Institute of Navigation. 1997, 44(3), 301-311. [21] Songlai Han, Jinling Wang. A novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applications. The Journal of Navigation, 2010, 63(4): 663-680. [22] A. H. Mohamed, and K. P. Schwarz. Adaptive Kalman Filtering for INS/GPS. Journal of Geodesy, 1999, 73(4), 193-203. [23] Jianhua Cheng, Daidai Chen, Rene Jr. Landry, Lin Zhao, and Dongxue Guan. An Adaptive Unscented Kalman Filtering Algorithm for MEMS/GPS Integrated Navigation Systems. Journal of Applied Mathematics. 2014, 155-184. [24] Shin, E. and El-Sheimy, N. An Unscented Kalman Filter for In-Motion Alignment of Low-Cost IMUs. Position Location and Navigation Symposium. 2004, 273-279. [25] Shin, E. and El-Sheimy, N. Unscented Kalman Filter and Attitude Errors of Low-Cost Inertial Navigation Systems. Navigation, Journal of the Institute of Navigation. 2007, 54 (1), 1-9. [26] S. J. Julier and J. K. Uhlman. Unscented filtering and nonlinear estimation. Proceedings of the IEEE. 2004, 92(3): 401-422. [27] Julier, S., Uhlmann, J. Hugh, F. A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators. IEEE Transaction on Automatic Control, 2000, 3, 477-482. [28] Yuanxin Wu, Dewen, Hu, Meiping Wu, et al. A numerical-integration perspective on Gaussian filters. IEEE Transactions on Signal Processing. 2006, 54(8): 2910-2921. [29] Ienkaran Arasaratnam, and Simon Haykin. Cubature Kalman Filters. IEEE Transactions on Automatic Control, 2009, 54(6), 1254-1269. [30] Yingwei Zhao. Performance evaluation of Cubature Kalman filter in a GPS/IMU tightly-coupled navigation system. Signal Processing, 2016, 119: 67-79. [31] Min Liu, Jizhou Lai, Zhimin Li, Jianye Liu. An adaptive cubature Kalman filter algorithm for inertial and land-based navigation system. Aerospace Science and Technology, 2016, 51(2): 52-60. [32] Hassana Maigary Georges, Dong Wang, Zhu Xiao. GNSS/Low-cost MEMS-INS Integration Using Variational Bayesian Adaptive Cubature Kalman Smoother and Ensemble Regularized ELM. Mathematical Problems in Engineering, 2015, 2015: 1-13. [33] Wei Huang, Hongsheng Xie, Chen Shen, Jinpeng Li. A robust strong tracking cubature Kalman filter for spacecraft attitude estimation with quaternion constraint. Acta Astronautica, 2016, 121: 153-163.

29

[34] Zhao Liqiang, Wang Jianlin, Yu Tao, Jian Huan, Liu Tangjiang. Design of adaptive robust square-root cubature Kalman filter with noise statistic estimator. Applied Mathematics and Computation, 2015, 256: 352367. [35] Li Pengfei, Yu Jianping, Wan Mingjie. The augmented form of Cubature Kalman filter and Quadrature Kalman filter for additive noise. 2009 IEEE Youth Conference on Information, Computing and Telecommunication Proceedings, 2009: 295-298. [36] Huang Jianjun, Zhong Jiali, Jiang Feng. A CKF Based Spatial Alignment of Radar and Infrared Sensors. 2010 IEEE 10th International Conference on Signal Processing Proceedings (ICSP2010), 2010, 1-3: 2386-2390. [37] Lu Zhang, Meilei Lv, Zhuyun Niu. Two-stage Cubature Kalman Filter for Nonlinear System with Random Bias. Proceeding of 2014 International Conference on Multisensor Fusion and Information Integration for Intelligent Systems (MFI), 2014, 1-4. [38] Wenling Li, Yingmin Jia, Junping Du, and Jun Zhang. Distributed Multiple-Model Estimation for Simultaneous Localization and Tracking With NLOS Mitigation. IEEE TRANSACTIONS ON VEHICLAR TECHNOLOGY, 2013, 62(6): 2824-2830. [39] Qian Hua-ming, Huang Wei. A generalized augmented Gaussian approximation filter for nonlinear systems with non-additive correlated noises. 2013 Third International Conference on Instrumentation, Measurement, Computer, Communication and Control, 2013:1618-1623. [40] Yuanxin Wu, Dewen Hu, Xiaoping Hu. Unscented Kalman filtering for additive noise case: augmented vs. non-augmented. IEEE Signal Processing Letters, 2005, 12(5), 357-360. [41] Paul D. Groves. Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems. Artech House, London, 2008, 375-378.

30