Accepted Manuscript

Augmented Cubature Kalman Filter for nonlinear RTK/MIMU Integrated Nav-

igation with Non-additive noise

Dingjie Wang, Hanfeng Lv, Jie Wu

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

To appear in: Measurement

Received Date: 25 May 2016

Revised Date: 23 July 2016
Accepted Date: 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 non-

augmented 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
[10]
satellites and their positions . Therefore, the complementary properties of RTK and MIMU make

their integration the most promising navigation scheme with advantages in accuracy, real-time and cost-

effectiveness.

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) [8-
13]
. 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
[4]
linearization condition . 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
 [4, 16]
categories: (a) analytical approximate solution methods , (b) optimization-based iteration methods
[17-19] [20, 21]
, and (c) LHU-based filtering methods . 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
[22, 23]
methods to deal with time-varying non-additive noise . 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
[23]
system nonlinearity . 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 high-

dimensional 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
[29]
covariance matrix of the filtering process . 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
[24]
IMU raw measurements . 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 newly-

emerging 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
[24, 40]
inaccurate noise statistics . 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 (1)
 f (t)= (1 + S f ) ⋅ f (t) + δ f
b b

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
δ f = ∇ + wV
b

 (2)
 Sω = γ
S = η
 f
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;

T T
∇b = ∇bX ,∇Yb ,∇bZ  is the accelerometer zero-bias and ε b = ε bX ,ε Yb ,ε Zb  is the gyro zero-bias;

T T
η 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
  b 1 b
ε& (t)= − T ε (t) + wgb (t)
 gb

 b
∇& (t)= − 1 ∇ b (t) + w (t)
ab
 Tab
 (3)
γ& (t)= − 1 γ (t) + w (t)
 ω Tgs
gs


& 1
η (t)= − T η (t) + was (t)
 as

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.


 r& n =D −1v n  RM1+ h 0 0
 n n b −1
 
v& =Cb f − (2ωie + ωen ) × v + g ，D =  0
n n n n 1
( RM + h ) cos ϕ
0 (4)
  
1
q& bn = q nb o [0,(ω ibb − Cbnωinn )T ]T  0 0 −1 
 2

where r n = [ϕ , λ , h]T is the position vector composed of latitude ϕ , 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 the quaternion from body frame (b-frame) to n-frame, i.e.

a (1− e 2 )
r n =Cbn r b = q bn o r b o (q bn )* ; RM = (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 ( was )T  as the system noise vector, and
T
wφT ( wgb )T ( wab )T ( wgs )T

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)
In which,

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

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

+ nw
system observation matrix at k epoch; and hk : R n x
→ 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
2
R = diag{[σ lat 2
, σ lon 2
,σ alt ]} (9)
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
[41]
INS attitude and instrument errors, which improves the accuracy in estimating system states . 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 )
 (10)
 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
  E[ wk ] = 0, Cov ( wk , w j ) = Qkδ kj


 E[v k ] = 0, Cov ( v k , v j ) = Rk δ kj (11)

Cov( wk , v j ) = 0

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  =  =  (12)
 E [ wk −1 | Dk −1 ]  0 

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

   (13)
 0 Qk −1 
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

Pk / k −1 = E  x% k / k −1 x% kT/ k −1  = E ( xk − xˆ k / k −1 )( xk − xˆ k / k −1 ) 
T
 
T (15)
= ∫ (f
n x + nw
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
R

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
(16)
Rn x + nw

= ∫ hk ( xk ) N ( xk ; xˆ k / k −1, Pk / k −1 )dxk
R nx + nw

9
 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

PΖ% k = E  Ζ% k / k −1Ζ% kT/ k −1  = E  Z k − Ζˆ k / k −1 Z k − Ζˆ k / k −1 

T


( )(
 )
= E  hk ( xk ) − Ζˆ k / k −1 hk ( xk ) − Ζˆ k / k −1  + Rk
T
(
 )(  )
T (17)
= ∫ ( h ( x ) − Ζˆ
k k k / k −1 )( h ( x ) − Ζˆ
k k k / k −1 ) p ( x k | Dk −1 )dxk + Rk
nx
R
T
= ∫ ( h ( x ) − Ζˆ
k k k / k −1 )( h ( x ) − Ζˆ
k k k / k −1 ) N ( xk ; xˆ k / k −1 , Pk / k −1 )dx k + Rk
R nx

Px% k Ζ% k = E  x% k / k −1Ζ% kT/ k −1  = E ( x k − xˆ k / k −1 ) Z k − Ζˆ k / k −1 

T

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

 (  ) (18)
T
= ∫ (x k − xˆ k / k −1 ) ( h ( x ) − Ζˆ
k k k / k −1 ) N ( x ; xˆ 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

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

 
= 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)
T T T T

= 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)

Thus, the minimum of Pk is obtained,

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)

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
[29]
posterior state probability density . 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.

2n
I N (f ) = ∫ f ( x ) N ( x; 0 , I )dx ≈ ∑ ω f (ξ )
i =1
i i
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， (24)
.
    . . .
     .  . 
 .   .  .  .   .   . 
            
 0   0  1  0   0   −1  

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 ,

Pka−1 = E ( xˆ ka−1 − xka−1 )( xˆ ka−1 − xka−1 ) 

T
xˆ ka−1 = E  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) (26)

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
 1 2n
∑ χ i , k |k −1
T
χ i , k |k −1 =f ( χ ia, k −1 ) xˆ k |k −1 = xˆ ka/ k −1 = (xˆ k |k −1 )T 0  (27)
2n i =1

1 2n P 0
Pk |k −1 = ∑ χ i , k |k −1χ iT, k |k −1 − xˆ k |k −1 xˆ kT|k −1 Pka|k −1 =  k |k −1
Qk 
(28)
2 n i =1  0
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.

1 2n e
rˆke|k −1 = ∑ 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 ;

1 2n
4. Compute the mean rotation vector φ = ∑φ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

1 2n
Pq , k / k −1 = ∑[qi o q −1 ][qi o q −1 ]T
2 n i =1
(30)

Step 3. Measurement Update

1 2n
Ζ i , k |k −1 =h ( χ i , k |k −1 ) Ζˆ k / k −1 = ∑ Ζ i , k |k −1
2n i =1
(31)

1 2n
PΖ% kΖ% k =
k k
∑[ Ζ i, k |k −1 − Ζˆ k / k −1 ][Ζ i , k |k −1 − Ζˆ k / k −1 ]T +Rk
2n i =1
(32)

12
 1 2n
Px% kΖ% k =
k
∑[ χ i, k |k −1 − xˆ k |k −1 ][ Ζ i , k |k −1 − Ζˆ k / k −1 ]T
2n i =1
(33)

( )
T
xˆ k =xˆ k / k −1 +K k Ζ k − Ζˆ k / k −1 xˆ ka = (xˆ k )T 0 
P 0 (34)
Pk =Pk / k −1 − K k PΖ% k Ζ% k K kT Pka =  k
k k
0 Qk 

K k =Px% kΖ%k PΖ−% k1Ζ% k (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 (36)

13
 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.

Ey =E ( x +δ x )  =x 2 + σ 2
2
(37)
 
2
E ( y − Ey )  =E  2xδ x + (δ x ) − σ 2  =4x 2σ 2 + 2σ 4
2 2
(38)
   
3
E ( y − Ey )  =E  2xδ x + ( δ x ) − σ 2  =24x 2σ 4 + 8σ 6
3 2
(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 i − yˆ ) =4x 2σ 2 (41)
i =1

∑ ω (γ
3
i 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
four points {χ1 , χ2 , χ3 , χ4 } =  x , 2σ w  ,  x + 2σ ,0 ,  x , − 2σ w  ,  x − 2σ ,0
T 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
3
∑ ω (γ
2
i i − yˆ ) =4x 2σ 2 + σ 4 (44)
i =1 2
2

∑ ω (γ
3
i i − yˆ ) =12x 2σ 4 (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, non-

augmented 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
150
CKF
0.7 UKF
CKF-extended
0.6 UKF-extended
Up(m)

Position Error(m)
100 0.5

0.4

0.3
50
50 -100 0.2
0 0
-50 100
East(m) 0.1
North(m)
0
0 50 100 150 200 250 300 350 400
Time(sec)

(a) The trajectory of circular motion simulation (b) Error of estimated 3-dimensional positions

0.8
EKF EKF
CKF 20 CKF
0.7
UKF UKF
CKF-extended CKF-extended
0.6 UKF-extended UKF-extended
15
Velocity Error(m/s)

0.5 Attitude Error(deg)

0.4
10
0.3

0.2
5

0.1

0 0
0 50 100 150 200 250 300 350 400 0 50 100 150 200 250 300 350 400
Time(sec) Time(sec)

(c) Error of estimated 3-dimensional velocity (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 non-

additive 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 Non- Non- Augmented Augmented

augmented augmented CKF UKF
CKF 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 Table 3 OEMV-3 Card parameters

Gyro 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

vslip
the ratio ρ slip = as an indicator of sideslip effect at a single epoch. The UAV sideslip characteristics
vtotal

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
8200 Takeoff Point 160
Landing Point

140
8100

120

8000
100
Altitute (m)
North (m)

7900 80

60
7800

40

7700
20

7600 0
-6500 -6400 -6300 -6200 -6100 -6000 -5900 0 100 200 300 400 500 600 700
East (m) Time (sec)

(a) Horizontal position and altitude

50 100
North (m/s)

Roll (deg)

0 0

-50 -100
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700
Time (sec) Time (sec)
50 50
Pitch (deg)
East (m/s)

0 0

-50 -50
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700
Time (sec) Time (sec)
Down (m/s)

20 200
Yaw (deg)

0 0

-20 -200
0 100 200 300 400 500 600 700 0 100 200 300 400 500 600 700
Time (sec) Time (sec)

(b) Velocity and attitude

19
 Fig.5 Flight reference for UAV test

<0.2
<0.5
>0.5
RTK
150 Takeoff Point
Landing Point
100

Height (m) 50

-50
8200
-5800
8000
-6000
7800 -6200
-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

Representative data Heading Flight time segment Initial attitude errors (deg) Sideslip
Uncertainty case (Time) 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

40 50

Roll error/deg
20
0 0
-20
-40 -50
0 100 200 300 400 500 600 700 0 0.2 0.4 0.6 0.8
Pitch error/deg

50 50

0 0

-50 -50
0 100 200 300 400 500 600 700 0 0.2 0.4 0.6 0.8

100 100
Yaw error/deg

0 0

-100 -100
0 100 200 300 400 500 600 700 0 0.2 0.4 0.6 0.8
Time/sec Side-slip ratio

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
 3 5

Roll Error/deg
non-augmented CKF
0

Position Error (m)

augmented CKF
2
-5 non-augmented CKF
augmented CKF
1 -10
0 50 100 150 200 250 300 350
5

Pitch Error/deg
0
0 50 100 150 200 250 300 350 0

-5
4
-10
Velocity Error (m/s)

0 50 100 150 200 250 300 350

3
10

Yaw Error/deg
2
0
1 -10

0 -20
0 50 100 150 200 250 300 350 0 50 100 150 200 250 300 350
Time (sec) Time (sec)

(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
 3 10

Roll Error/deg
non-augmented CKF non-augmented CKF

Position Error (m)

5
augmented CKF augmented CKF
2
0

-5
1 0 50 100 150 200 250 300 350 400
5

Pitch Error/deg
0
0 50 100 150 200 250 300 350 400 0

-5
6
-10
Velocity Error (m/s)

0 50 100 150 200 250 300 350 400

4 20

Yaw Error/deg
0
2
-20

0 -40
0 50 100 150 200 250 300 350 400 0 50 100 150 200 250 300 350 400
Time (sec) Time (sec)

(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 non-

augmentation 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 non-

additive noise on covariance propagation naturally. As an effective way to depict the effect of non-

additive 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
 4 EKF 5

Roll Error/deg
non-augmented UKF

Position Error (m)

3 augmented UKF
0
augmented CKF
2
-5
1 0 50 100 150 200 250 300 350
5

Pitch Error/deg
0
0 50 100 150 200 250 300 350 0

-5
6
-10
Velocity Error (m/s)

0 50 100 150 200 250 300 350

4 10

Yaw Error/deg
0
2
-10

0 -20
0 50 100 150 200 250 300 350 0 50 100 150 200 250 300 350
Time (sec) Time (sec)

(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 Augmented Augmented

UKF UKF 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
4 5
non-augmented UKF
Roll Error/deg
Position Error (m)

3 augmented UKF 0
augmented CKF
-5
2
-10
1 0 50 100 150 200 250 300 350 400
10
Pitch Error/deg

0
0 50 100 150 200 250 300 350 400
0

6
-10
Velocity Error (m/s)

0 50 100 150 200 250 300 350 400

4 20
Yaw Error/deg

0
2
-20

0 -40
0 50 100 150 200 250 300 350 400 0 50 100 150 200 250 300 350 400
Time (sec) Time (sec)

(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 Augmented Augmented

UKF UKF 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 non-

augmented 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: 352-

367.

[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