AccurateAttitudeDeterminationBasedonAdaptiveUKFandRBF ...downloads.hindawi.com/journals/mpe/2020/1638678.pdfMEMS Inertial Measurement Unit (MIMU) and magnetic sensor have wide application

Research ArticleAccurateAttitudeDeterminationBasedonAdaptiveUKFandRBFNeural Network Using Fusion Methodology for Micro-IMUApplied to Rotating Environment

Lei Wang Zhi Min Meng and Ying Guan

Research Centre of Weaponry Science and Technology Shenyang Ligong University Shenyang Liaoning 110159 China

Correspondence should be addressed to Lei Wang uuanda163com

Received 5 April 2020 Revised 1 July 2020 Accepted 7 July 2020 Published 31 July 2020

Academic Editor Carlos-Renato Vazquez

Copyright copy 2020 Lei Wang et al is is an open access article distributed under the Creative Commons Attribution Licensewhich permits unrestricted use distribution and reproduction in any medium provided the original work is properly cited

Focusing on the issue of attitude tracking for low-cost and small-size Micro-Electro-Mechanical System (MEMS) InertialMeasurement Unit (IMU) in high dynamic environment an Adaptive Unscented Kalman Filter (AUKF) method combiningsensor fusion methodology with Artificial Neural Network (ANN) is proposed e different control strategies are adopted byfusing multi-MEMS inertial sensors under various dynamic situations e AUKF attitude determination approach utilizing theMEMS sensor and Global Positioning System (GPS) can provide reliable estimation in these situations In particular the adaptivescale factor is used to adaptively weaken or enhance the effects on new measurement data according to the predicted residualvector in the estimation process In order to solve the problem that the newmeasurement data is not available in case of GPS faultan attitude algorithm based on Radial Basis Function (RBF)-ANN feedback correction is proposed for AUKF e estimateddeviation of predicted system state can be provided based on RBF-ANN in GPS-denied environment e corrected predictedsystem state is used for the estimation process in AUKF An experimental platform was setup to simulate the rotation of thespinning projectile e experimental results show that the proposed method has better performance in terms of attitude es-timation than other representative methods under various dynamic situations

1 Introduction

MEMS Inertial Measurement Unit (MIMU) and magneticsensor have wide application for attitude determination incivilian and military fields such as Unmanned Aerial Vehicle(UAV) systems [1 2] land vehicle systems [3 4] projectilespinning shell [5 6] underwater system [7 8] and others[9 10] In these specific applications the work of differentattitude determination algorithms is classified according to thefollowing categories Among them representative researchstudies include attitude determination utilizing the differentMEMS sensors [11ndash14] different Kalman filter methods forattitude estimation [15 16] sensor fusion algorithms for at-titude determination [17 18] and intelligent control methodstrategy combined with filters [19 20]

On one side differentMEMS inertial sensors are affordablefor attitude calculation e gyroscope is the principal sensoravailable for attitudemeasurement using quaternion algorithm

which quickly degrades over time [11] Assuming that theobject does not move or moves at a constant speed the rollangle and pitch angle can be estimated by the strapdowntriaxial of the accelerometer [14] Magnetometers are widelyused for estimating the yaw angle in AHRS [15] or estimatingthe ballistic roll in the projectile [16] e attitude determi-nation approaches utilizing different sensors separately aredifficult to provide reliable estimation due to sensor errors anddynamic environmental variations

In addition considerable work has been carried out onattitude estimation using adaptive filters and fusion algo-rithms e Extended Kalman Filter (EKF) [4] and AdaptiveEKF (AEKF) [7 14] are developed for nonlinear system stateestimation in navigation system Unscented Kalman Filter(UKF) [5 9] and Adaptive UKF (AUKF) [15 16] are widelyused for sensor fusion algorithms e methods mentionedabove can achieve accuracy and adaptability for attitudeestimation For low-cost MEMS IMU sensors Chiella and

HindawiMathematical Problems in EngineeringVolume 2020 Article ID 1638678 17 pageshttpsdoiorg10115520201638678

Teixeira [15] proposed a quaternion-based robust adaptiveunscented Kalman Filter for attitude estimation In the filterthe adaptive strategy based on covariance matching canadjust the measurement covariance matrix online to dealwith the slow time-varying disturbance in the sensors Xuet al [16] proposed a multiple-model unscented Kalmanfilter for attitude estimation which is a quaternion-basedattitude estimator that fuses related strap-down MagneticAngular Rate and Gravity (MARG) sensor arrays

In order to overcome the drawbacks of the abovementionedmethods and take full advantage of fusion and adaptive filteringscheme some fusionmethodologies combining complementaryfiltering or Kalman filtering are proposed Koksal et al [17]combines Kalman filter and complementary filter to provideaccurate attitude estimation and control performance forquadrotor UAV Noordin et al [18] proposed a sensor fusionalgorithm for UAV attitude estimation based on nonlinearcomplementary filter (NCF) To obtain reliable attitude esti-mation gyroscope is fused to the accelerometer and magne-tometer to correct drift error of the gyroscope Otherrepresentative research studies are the combination of filteringand intelligent control scheme Ning et al [19] propose a robustKalmanfilter combinedwithRBF-ANNalgorithm to isolate andreduce the influence of the dynamic model error and GNSSobservation gross error Hossein and Jafer [20] adjusts the fuzzyweighting coefficients to correct the orientation estimation byusage of SINSGPS and AHRS data

e above works on attitude determination algorithm lie inthe following problems e traditional EKF and other im-proved EKF have the drawbacks such as low estimation ac-curacy and poor stability which will cause the biasedestimation or even divergence due to which the statisticalproperties of system and measurement noise cannot be pre-dicted accurately especially for low-cost MEMS-based sensors[21] Some complicated nonlinear calculationwill cause a heavyburden in real-time micronavigation system which willweaken the feasibility for application [2 22] erefore theadvanced filtering or adaptive filtering methods will play a keyrole in these applications [6 13 17 23]

is paper concentrates on developing the customizedattitude determination algorithm of low-cost MEMS sensorin high dynamic environmente corresponding techniquecan be widely applied in measuring the attitude of theprojectile and spinning shell of low dynamic rotatingCombined with the proposed adaptive Kalman filteringmethod and intelligent control scheme it is suitable fordealing with time-varying noise of low-cost small-sizemicro-IMU and dynamic interference e novel aspects ofthis paper can be summarized as follows

(1) According to dynamic rotating environment a newattitude estimation scheme of AUKF is designedMeanwhile different control strategies are alsoadopted to fuse MEMS multi-inertial sensors underdifferent dynamic conditions AUKF attitude de-termination method based on MEMS sensor andGPS can provide reliable estimation under highdynamic environment change Using the adaptivescale factor the influence on the new measurement

data is weakened or enhanced during the process ofthe estimation according to the predicted residualvector

(2) e proposed method can accommodate sensorfailure particularly GPS failure in adverse envi-ronment When GPS signals are temporarilyblocked the attitude algorithm based on RBF-ANNfeedback correction can continue to provide reliableestimated deviation information e correctedpredicted system state will be used in the estimationprocess in AUKF

(3) Considering the space limitations in spinning pro-jectile the small size MEMS IMU is designed ecustomized prototype of the DSP controller is de-veloped for it e corresponding programs of theproposed algorithm can be compiled and run on inreal time

e remainder of this paper is organized as followsSection 2 presentsMEMS IMU configuration and design themodel of MEMS sensor and related calibration work eAUKF algorithm and RBF feedback correction are devel-oped in detail in Section 3 en experimental results anddiscussion are stated in Section 4

2 Modeling and Related Work

21 Sensor Configuration and Design According to applica-tion background of the spinning projectile a small size lowpower consuming and low-cost MEMS IMU is developedMicroinertial navigation system consists of a DSP micro-controller AD converter flash powermodulemicro-IMU andmagnetometeremicro-IMU is composed of three single-axisgyroscopes and accelerometers It is shown in Figure 1

In Figure 1 a six layer MEMS IMU PCB board is designedwith size of 45times 45mm e output analog signals of the ac-celerometer and magnetometer are sampled simultaneously by8-channel 16bit analog-to-digital converter e simple andeffective communication between DSP microcontroller andgyroscope is realized through SPI interface Angular rate data isoutput in 12-bit 2rsquos complement format at a maximum rate of2000 samples per second Considering the real-time perfor-mance the proposed algorithm ismigrated fromMATLAB toCcodeese codes can be compiled in CCS33 environment andloaded into DSP 28335 microcontroller for real-time operation

22 Sensor Model Low-cost small size MEMS inertialsensors are widely used in these applications due to thedimension constraint cost consideration in projectile andspinning shell [5 6] Gyroscopes and accelerometers arecommonly known as inertial sensors and their orthogonaltriads generally form an IMU used as a core means of anavigation systeme precision of gyro is constraint to timevarying bias drift and noise e true output of the gyro canbe defined as follows

ωbib S

minus1g

1113958ωbib minus bg1113874 1113875 (1)

2 Mathematical Problems in Engineering

whereωbib is the vector of true angular rate

1113958ωbib is the vector of

measured angular rate and bg corresponds to the vector ofoutput bias Used indices are e-Earth frame n-Navigationframe i-inertial frame and b-body frame e upper indexdefines the referenced coordinate system whereas the lowerindex refers to the described value Sg denotes the matrix ofscale factors and misalignments

Sg

kgx σxy σxz

σyx kgy σyz

σzx σzy kgz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (2)

e main diagonal elements kg are scale factors thataccounts for the sensitivities of the individual sensors byscaling the outputs e variable σ represents the non-orthogonality and misalignment of the triaxial gyroscopesensors e biases scale factors nonorthogonality ormisalignment errors are main systematic errors or deter-ministic sources for MEMS inertial sensors

An accelerometer sensor is a dynamic MEMS sensorused to measure acceleration forces up to three orthogonalaxes e accelerometer in the body frame is defined as

fb

MaSa1113957fb minus ba1113872 1113873 (3)

where fb is the vector of true accelerometer 1113957fb is the vectorof measured acceleration ba corresponds to the vector ofoffsets Sa represents the matrix of scale factors and Ma

transforms a vector from the nonorthogonal coordinatesystem to the orthogonal one

Ma

1 0 0

ζxy 1 0

ζzx ζzy 1

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

Sa

kax 0 0

0 kay 0

0 0 kaz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(4)

e main diagonal elements ka are scale factors thevariable ζ represents the nonorthogonality and misalign-mente three angles namely pitch roll and yaw denotedby the symbols θ ϕ and ψ Correspondingly the pitch androll angle could be computed as

θacc minusarctanfb

x

g1113888 1113889

ϕacc arctanfb

y

g middot cos θ⎛⎝ ⎞⎠

(5)

where fbx and fb

y are the component variables of fb in bodycoordinate

e magnetic field components on local geography co-ordinate (west magnetic field southmagnetic field and verticalmagnetic field) can be measured by the geomagnetic vectormeasurement system which mainly contains a three-axismagnetometer e performance of the magnetometer sensoris distorted by hard iron and soft iron effects Intrinsic andcross sensor calibration of the three-axis magnetometer isindispensable to eliminate scale factor cross coupling and biaserrors e output of the magnetometer is represented as

mb

Cminus1e

1113958mb minus ε1113872 1113873 (6)

where mb is the calibrated output of magnetometer the matrix1113958mb represents measured output in body frame Ce representseffect of sensor imperfection andmagnetic disturbance and thevector ε represents the noise of magnetic disturbance Definemn is the local magnetic vector in n-frame e relationship ofmagnetic vectors in b-frame and n-frame is represented by

mb

Cbnm

n (7)

Define mp is the horizontal plane projection of themagnetic field the relationship between mp and magneticvectors in b-frame mb is

mpx

mpy

mpz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

cos θ sinϕ sin θ cosϕ cos θ

0 cosϕ minussinϕ

minussin θ sinϕ cos θ cosϕ cos θ

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ middot

mbx

mby

mbz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (8)

Correspondingly yaw angle ψm is calculated as

ψm arctanm

py

mpx

1113888 1113889 (9)

23 Sensor Calibration Method Before MEMS IMU wasused for attitude estimation in the actual application there

y

x

z

(a)

45mm

(b)

MEMS inertial sensors

(c)

Figure 1 e prototype of MEME IMU and micronavigation system

Mathematical Problems in Engineering 3

were measurement errors and installation errors which willaffect the measurement output erefore the calibrationmethod is one effective way to compensate for the deter-ministic and other errors of the MEMS inertial sensorConsidering the application in high dynamic environmentthe coupling effect between sensitive axes will significantlyaffect the measurement accuracy of micro-IMU Accordingto the spinning characteristics in the projectile applicationthat the rotation of one sensitive axis is highly dynamic andthe rotation of the other sensitive axis is low dynamic thecoupling coefficient of the model is estimated by thecompounded calibration method [23] e high dynamicrolling three-axis turntable is used to simulate differentballistic attitude angle variation of projectile in flight stateExperiments are performed respectively Figure 2 shows thecompounded calibration scheme for micro-IMU

e calibration process is composed of the initial cali-bration and compounded calibration e optimal bias ofgyro is estimated and provided to AUKF as the initialcalibration process e bias bg scale factor kgi(i x y z)

and the misalignment σjk were estimated in the initialcalibration process using the least square algorithm edominant deterministic error result is shown in Table 1

e estimated adjustment value Δωb in compoundedprocess is used for compensating the coupling effect ofdifferent rotating axis in high dynamic environment ecompounded calibration eliminates the coupling effect be-tween the high dynamic rotation axis and the low dynamicrotation axis e compensation accuracy of high dynamicrotation is improved e preprocessing data after calibra-tion and compensation can be used for attitude estimation

especially in high dynamic environment e sensor char-acteristics of stochastic errors in low MEMS gyro acceler-ometer and magnetometer could be identified using Allanvariance [24] e root mean square random drift error ofAllan variance is calculated and provided to AUKF as theinitial measurement noise

3 Methodology

31 System State and Observation Model According to theoutput characteristic of MEMS sensor the filter algorithmcan effectively improve the accuracy of sensor measurementoutput Considering the nonlinear characteristic of thesystem the adaptive UKF fusion algorithm is preferred tosolve the attitude estimation in the case of dynamic envi-ronment changes

Establishing the perfect state equation and measurementequation is the key to improving the solution Consideringthe effect of the model error the quaternion vector q and theoutput error of gyroscope bt are chosen as the state vectore operator ldquootimes rdquo denotes the quaternion multiplication

X q bg1113960 1113961T

7times1(10)

Hereq is the quaternion vector q q0 q1 q2 q31113858 1113859ebias of gyroscope bg bgx bgy bgz1113960 1113961 which is the biasdrift in three sensitive axes x y and z e coordinatetransformation from the navigation frame to the body fixedframe is based on the three angles θ ϕ and ψ e attitudematrix Cn

b from the navigation frame and body frame bythree Euler angles is given by

Cnb

cos θ cosψ sinϕ sin θ cosψ minus cos ϕ sinψ cos ϕ sin θ cosψ + sinϕ sinψ

cos θ sinψ sinϕ sin θ sinψ + cos ϕ cosψ cos ϕ sin θ sinψ minus sinϕ cosψ

minussin θ sinϕ cos θ cos ϕ cos θ

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (11)

e single spline algorithm based on the quaternionvector in equation (12) is suitable for attitude angle esti-mation in a low dynamic environment

_X7times1 f q bt( 1113857 12

A4times4 04times3

03times4 B3times3

⎡⎢⎣ ⎤⎥⎦ middot X7times1 + W7times1 (12)

A4times4

0 minus1113957ωbnbx minus1113957ωb

nby minus1113957ωbnbz

1113957ωbnbx 0 1113957ωb

nbz minus1113957ωbnby

1113957ωbnby minus1113957ωb

nbz 0 1113957ωbnbx

1113957ωbnbz 1113957ωb

nby minus1113957ωbnbx 0

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

B3times3 minus1τlowast

1 0 00 1 00 0 1

⎡⎢⎢⎢⎢⎢⎣⎤⎥⎥⎥⎥⎥⎦

W7times1 01times4 ωrx ωry ωrz1113960 1113961T

(13)


where τ is the time constant of a first order GaussndashMarkovprocess and ωr is Gaussian white process e body anglerrate with respect to the navigation frame 1113957ωnb is given by thedifference between the body angular rate 1113957ωib and thenavigation frame rotation ωin

1113957ωbnb 1113957ωb

ib minus Cnb( 1113857

Tωnin ωb

ib + δωnib minus C

nb( 1113857

Tωnin (14)

where the vector ωnin can be written as ωn

in ωnie + ωn

en evector ωn

ie is the Earth rate in the navigation frame e

vector ωnen is the turn rate of the navigation frame with

respect to the velocity of the craft e output error δωbib of

gyro is replaced by the constant deviation b0 and thewhole range of the constant deviation b0 is estimated asthe definite error through the off-line calibrationexperiment

In a high dynamic environment the cubic spline algo-rithm based on the quaternion vector is used for the esti-mation of attitude angle

q(t + h) q(t)otimes r(h) (15)

r(h) cos(|Φ|2) + (Φ|Φ|)sin(|Φ|2) Φ is the equiv-alent rotating vector in sampling period from t to t+ h h isthe time step in sampling time e corresponding angularrate of gyro in sampling period ωb

nb is expressed using thestraight line fitting method e expression of matrix inequation (15) could be rewritten as

q0(t + h)

q1(t + h)

q2(t + h)

q3(t + h)

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

cos|Φ|

2minusΦx

|Φ|sin

|Φ|

2minusΦy

|Φ|sin

|Φ|

2

Φx

|Φ|sin

|Φ|

2cos

|Φ|

2Φz

|Φ|sin

|Φ|

2

Φy

|Φ|sin

|Φ|

2minusΦz

|Φ|sin

|Φ|

2cos

|Φ|

2

minusΦz

|Φ|sin

|Φ|

2

minusΦy

|Φ|sin

|Φ|

2

Φx

|Φ|sin

|Φ|

2

Φz

|Φ|sin

|Φ|

2Φy

|Φ|sin

|Φ|

2minusΦx

|Φ|sin

|Φ|

2cos

|Φ|

2

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

q0(t)

q1(t)

q2(t)

q3(t)

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(16)

The setting data used assimulating ballisticattitude variations

Calculating the desired adjustmentvalue matrix for training of FNN

The experiments ofsimulating different

ballistic attitudes

Compensated data for attitudecalculation

The transition data ofinitial calibration process

Compounded calibration

Data preprocessing

The least square algorithm

Initial calibration

Static amp angular rate method

Collecting the raw data

Reference datafrom Mti

Three axes rotation tablein high dynamic rolling

The fixture of micro-IMU

Mti

Micro-IMU

Slip ring

Figure 2 Compounded calibration experiments of micro-IMU

Table 1 Deterministic errors for MEMS gyro

AxisGyro deterministic error in indoor

temperatureBiases (degs g Gauss) Scale factor

Gyro x minus20511 09976Gyro y minus25848 10033Gyro z minus00043 10021


e differential equation for the rotating vector can beobtained by neglecting the high order items

_Φ 1113957ωbnb +

12Φ times 1113957ωb

nb +112Φ times Φ times 1113957ωb

nb1113872 1113873 (17)

e polynomial fitting equation of the angular rate isexpressed as follows

ωbnb a + 2bτ + 3cτ2 0le τ le h (18)

where a b and c are fitting parameters for quadraticfunction polynomial Defining Φ(tk) and Φ(tkminus1) are theattitude quaternions of tk and tkminus1 moment respectively theTaylor series expansion equation of the attitude quaternionis expressed in

Φ tk( 1113857 Φ tkminus1( 1113857 + _hΦ tkminus1( 1113857 +h2

2euroΦ tkminus1( 1113857 +

h3

3Φmiddotmiddotmiddot

tkminus1( 1113857 + middot middot middot

(19)

e attitude algorithm with incremental angle could beconstructed as follows

θi 1113946(i3)h

(iminus13)hω tk + τ( 1113857dτ i 1 2 3 (20)

where θ1 θ2 and θ3 are the angular increments of[tk tk + (13)h] [tk + (13)h tk + (23)h] and[tk + (23)h tk+1]

θ1 13

ah +19

bh2

+127

ch3

θ2 13

ah +39

bh2

+727

ch3

θ3 13

ah +59

bh2

+1927

ch3

(21)

e equivalent rotating vector is expressed according toTaylor series expansion equation

Φ(h) Φ(0) + _Φ(o)h +12Φ_(0)h

2+16Φt(0)h

3+

124Φ(4)

(0)h4

+1120Φ(5)

(0)h5

ah + bh2

+ ch3

+16

(a times b)h3

+14

(a times c)h4

+110

(b times c)h5

θ1 +θ2 +θ3 +3380θ1 timesθ3

+5780θ2 times θ2 minusθ3( 1113857

(22)As can be seen from the Section 21 the accelerometer

can be used for calculating the pitch and roll angles bymeasuring the gravity vectors if the vehicle is stationary or inlow dynamic environment Triaxis magnetometers are usedfor calculating the yaw angle Its accuracy is heavily affectedby local magnetic environments According to the attitude

algorithm of the accelerometer and magnetometer the at-titude angle ϕacc θacc and ψmag can be treated as themeasurement of the attitude system in the low dynamicenvironment

ϕacc

θacc

ψmag

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

arctan2q2q3 + 2q0q1

2q20 + 2q23 minus 11113888 1113889

minusarcsin 2q1q3 minus 2q0q2( 1113857

arctan2q1q2 + 2q0q3

2q20 + 2q21 minus 11113888 1113889

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

+ nablam (23)

nablam nablaϕ nablaθ nablaψ1113960 1113961T where nablaϕ nablaθ and nablaψ are the

measurement noise of roll pitch and yaw angleE[nablam] 0E[nablamknablaT

mk] Rδkj and R diag σ2ϕ σ2θ σ2ψ1113966 1113967where σ2ϕ σ2θ and σ2ψ are standard deviation of attitudeerrors eir values depend on the precision of the singleMEMS sensor

In the high dynamic environment the aforementionedattitude estimation approaches utilizing only the singlesensor are difficult to provide reliable estimation due tosensor errors e dynamic accuracy of the attitude anglethat are calculated in virtue of the accelerometer andmagnetometer is affected largely due to the motion of bodyin the high dynamic environment In such a case themeasurement vectors ϕacc θacc and ψmag are unreliable dueto the effects of sensor errorse specific force referenced inn-frame can be treated as the measurement of low-costmicro-IMU attitude system erefore the measurementequation can be replaced as

Z Fn

Fnx

Fny

Fnz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ Cnbf

b+ v (24)

where the measurement vector Fn is the specific force ref-erenced in n-frame and v is the measurement noise eattitude error matrix Cn

b is expressed in terms of quaternionin equation (11)

Cnb

1 minus 2 q22 + q23( 1113857 2 q1q2 minus q0q3( 1113857 2 q1q3 + q0q2( 1113857

2 q1q2 + q0q3( 1113857 1 minus 2 q21 + q23( 1113857 2 q2q3 minus q0q1( 1113857

2 q1q3 minus q0q2( 1113857 2 q2q3 + q0q1( 1113857 1 minus 2 q21 + q22( 1113857

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(25)

e measurement vector Fn is calculated as

Fn

_Vn

+ 2ωnie + ωn

en( 1113857 times Vn

minus gn (26)

Here the vector _Vn is given by calculating the derivative

of velocity afforded by GPS

32 AdaptiveUKFFusionAlgorithm Due to the fact that thestatistical characteristics of the noise are real-time changedit is essential to adjust noise estimation in the filter algo-rithm Based on the nature of UKF algorithm unscentedtransformation (UT) is an approximate method to change


the mean and covariance of random variables when theyundergo nonlinear transformation [21 25] e adaptiveperformance of AUKF is the filter process of the mea-surement noise variance of the real-time estimation ebasic steps of the AUKF algorithm are as follows

(i) Step 1 generation of sigma points and weightse estimated state and corresponding covariance ofthe system can be expressed as follows

1113954xkminus1 E xkminus1( 1113857

1113954Pkminus1 E 1113954xkminus1 minus xkminus1( 1113857 1113954xkminus 1 minus xkminus 1( 1113857T

1113960 1113961

⎧⎨

⎩ (27)

Sigmal points xikminus1 | i 1 2 n kge 11113966 1113967 are gener-ated according to the following equations

xikminus1 1113954xkminus1 +

(n + λ)Pkminus1

1113969

1113874 1113875i i 1 2 middot middot middot n

xi+nkminus1 1113954xkminus1 minus

(n + λ)Pkminus1

1113969


(28)

Correspondingly the weights of mean and covarianceare presented

Wmi

λn + λ

i 0

λ2(n + λ)

ine 0

⎧⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎩

Wci

λn + λ

+ 1 + β minus α2 i 0

λ2(n + λ)

ine 0

(i 0 1 2 n 2n)

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎩

(29)

where λ α2(n + k) minus n and it is a scalar parameterrelated to the transformation scale factor Constantk 3minus n is chosen to ensure the positive semidefinite ofthe postcovariance β is a parameter related to state apriori distribution for reducing the higher order effectsand the general value forGaussian distribution is 2escaling factor α is used such that sigma points aresampled within the range of plusmn ϱ It should be of littlepositive value as much as possible α 0001 when thesystem is seriously nonlinear e use of α guaranteesthat nonlinearities outside of the uncertainty regioncannot affect the solution

(ii) Step 2 time update

xik|kminus1 f xikminus11113872 1113873 i 1 2 2n

1113954xk|kminus1 11139442n

i0W

mi x

ik|kminus1

Pk|kminus1 11139442n

i0W

ci x

ik | kminus1 minus 1113954xk | kminus11113872 1113873 x

ik | kminus1 minus 1113954xk | kminus 11113872 1113873

T

(30)

where 1113954xk|kminus1 and Pk|kminus1 are predicted nonaugmentedsystem state and predicted error covariance matrixat time tkminus1

(iii) Step 3 measurement update

Zik|kminus1 h x

ik | kminus11113872 1113873 i 1 2 2n (31)

1113954Zk|kminus1 11139442n

i0W

mi Z

ik|kminus1 (32)

Pzzk 1113944

2n

i0W

ci Z

ik|kminus1 minus 1113954Zk|kminus11113872 1113873 Z

ik|kminus1 minus 1113954Zk|kminus 11113872 1113873

T+ 1113954Rk

Pxzk 1113944

2n

i0W

ci x

ik|kminus1 minus 1113954xk|kminus11113872 1113873 Z


T

(33)

where

Kk Pxzk P

zzk( 1113857

minus 1 (34)

1113954xk|k 1113954xk|kminus1 + Kk Zk minus 1113954Zk | kminus11113872 1113873 (35)

Pk|k Pk|kminus1 minus KkPzzk K

Tk (36)

1113954xak|kminus1 1113954xT

k|kminus 1 01113960 1113961T (37)

Pak|kminus1

Pk|kminus1 00 Qk

1113890 1113891 (38)

where 1113954xk|k and Pk|k are estimated nonaugmentedsystem state and predicted error covariance matrixat time tk while 1113954xa

k|k and 1113954Pa

k|k are estimated aug-mented system state and estimated error covariancematrix at time tk

(iv) Step 4 the tuning of the adaptive scale factorDefine the predicted residual vector is εk

εk Zk minus 1113954Zk|kminus1 (39)

By placing the condition that output residual vectors atdifferent sampling time maintain orthogonal to eachother the residual vector is satisfied

E εk+jεTk1113872 1113873 0 k 1 2 j 1 2 (40)

e adaptive scale factor μk is adapted to enhance orweaken the effects of previous measurements on cur-rent estimation in filter algorithm In this way theadaptive scale factor in AUKF is used to adaptivelyadjust the influence between system prediction Pzz

k andobservation 1113954P

zz

k e adaptive factor μk is introduced torealize a reasonable balance between system predictionand observation

Pzzk μk

1113954Pzz

k + 1113954Rk 1 minus μk( 1113857 (41)


where the adaptive factor 0le μk le 1 Considering theuncertainty of the model errors the estimated covariancematrix 1113954P

zz

k can be estimated by predicted residual vector εk1113954P

zz

k εkεTk (42)

According to equations (31) (35) and (39) the fol-lowing expressions are given

xk minus 1113954xk|k xk minus 1113954xk|kminus1 minus Kk Zk minus 1113954Zk|kminus11113872 1113873 1113957xk|kminus1 minus Kkεk

(43)

whereεk Zk minus 1113954Zk|kminus1 h x

ik | kminus11113872 1113873 minus 1113954Zk|kminus1

1113957xk|kminus1 xk minus 1113954xk|kminus1(44)

Considering the condition that the measurement modelh(xi

k | kminus1) is linearized by the first order Taylor series thefollowing derivations [26] is presented as follows

E xk minus 1113954xk | k1113872 1113873εTk1113960 1113961 E 1113957xk | kminus1 minus Kkεk1113872 1113873εT

k1113960 1113961 (45)

Define G(Pk|kminus1 μk) is the goal function It is expressedin equation (46) according to the relationship inequations (34) (41) and (42)

G Pk|kminus1 μk1113872 1113873 E 1113957xk|kminus1εTk minus Kk

1113954Pzz

k1113960 1113961 (46)

e adaptive scale factor model can be calculated as

μk min G Pk | kminus1 μk1113872 11138731113872 1113873 (47)

Based on (34)ndash(38) the estimated residual covariancematrices Pzz

k and Pxzk can be formulated by adding the

adaptive scale factor μk in (48) and (49) Correspondingvectors 1113954Kk 1113954xk|k and 1113954Pk|k can be described in equation(50)ndash(52)

1113954Pzz

k 1μk

1113944

2n

i0W

ci Z



T+ 1113954Rk

(48)

1113954Pxz

k 1μk

1113944

2n

i0W

ci x



T

(49)1113954Kk 1113954P

xz

k1113954P

xz

k1113872 1113873minus 1

(50)1113954xk|k 1113954xk|kminus1 + 1113954Kk Zk minus 1113954Zk | kminus11113872 1113873

(51)

1113954Pk|k 1μk

1113954Pk|kminus1 minus 1113954Kk1113954P

zz

k1113954K

T

k

(52)Depending on the predicted residual statistics theadaptive scale factor is used to adaptively inflate thecovariance matrix of the 1113954xk|k and 1113954Pk|k Meanwhile the

gain matrix 1113954Kk is also changed in real time to achievestrong tracking of AUKF It is reasonable to assume

Pzzk asymp μk

1113954Pzz

k (53)

Define Pzzk Nk and 1113954P

zz

k Mk It can be described as

Mkμk Nk (54)

Here the adaptive scale factor is introduced

μk 1 μ0 lt 1

μ0 μ0 ge 11113896 (55)

where μ0 tr[Nk]tr[Mk] in case of the practicalapplications the adaptive scale factor can be set tosatisfy the condition μk le 1 Its specific implementationis as followsIt is noted that model disturbance usually exists in thefirst stage of AUKF In order to reduce model dis-turbances on current estimates in filer algorithm theweight of model prediction in the estimation processshould be lowe scale μk is approximately set to 1 Onthe contrary as the measurement model error isgradually reduced over time the scale μk is close to 0 forweakening the effects of previousmeasurements In thisway the filter is caused to enhance the weight of thenew measurement data according to the predictedresidual vector εk

33 RBFFeedbackCorrection inAUKF Considering the caseof GPS failure the reliable newmeasurement data will not beupdated In order to continue to provide reliable informa-tion in GPS-denied environment an attitude algorithmbased on RBF-ANN feedback correction is adopted forAUKF RBF-ANN shows good performance in classificationmodeling It consists of three layers one input layer onehidden layer and one output layer e output of thenetwork is a linear combination of input radial basisfunction and neuron parameters Its diagram is shown inFigure 3

When GPS works normally the estimated deviation ofpredicted nonaugmented system state δ1113954xk|k is used forcorrecting the predicted nonaugmented system state 1113954xk|k inAUKF according to equation (51)

δ1113954xk|k 1113954xk|k minus 1113954xk|kminus1 1113954Kk Zk minus 1113954Zk|kminus11113872 1113873 (56)

When GPS signals are temporarily blocked the esti-mated deviation of system state δ1113954xk|k will be provided basedon RBF-ANNe corrected predicted nonenhanced systemstate δ1113954xk|k is used in the estimation process in AUKF

Considering the error effect resulted from low-costmicro-IMU the processed data of accelerometer gyro andmagnetometer after calibration process are selected as theinput of RBF-ANN e estimated deviation of the systemstate δ1113954xk|k is taken as the output of RBF-ANN In case GPSworks normally the processed outputs data of micro-IMUand the expecting estimated deviation of predicted system


state δ1113954xek|k in equation (56) are used as the training data of

RBF-ANN e activation function of RBF is chosen as

ϕi(x) exp minusx minus c2i

2σ221113888 1113889 (57)

where the input data is modeled as a real vector x isin R φ(middot)

is the activation function of hidden layer which can bedefined as the Gaussian function and ci is the clusteringcenter vector of input data which is determined by K-meansalgorithm e value σ2 is the standard deviation of acti-vation function

σ2 dmax2m

radic (58)

where dmax is the maximum distance between differentcluster centers and m is the quantity of cluster which is alsothe neural quantity of the hidden layer According tominimum error sum of squares the optimal value m isselected In the optimization process K-mean clusteranalysis method is adopted e output is expressed asfollows

yi 1113944N

i1Wiφ x minus ci( 1113857 1113944

N

i1Wi exp minus

x minus c2i2σ22

1113888 1113889 (59)

where N is the number of hidden units Wi is the connectingweight coefficient between the kth hidden unit and theoutput yi is differentiable with respect to the weight Wi

Input sample dataset is introduced as(Xi Yi) | Xi isin RN Yi isin RN i 1 2 N1113864 1113865 Here Yi is theestimated deviation of predicted nonaugmented system stateδ1113954xk|k e matrix G is Green function which is calculated inequation (60) and the function is as follows

G11 middot middot middot G1m

⋮ ⋱ ⋮

Gm1 middot middot middot Gmm

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

W1

⋮

Wm

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

Y1

⋮

Ym

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (60)

Considering that the matrix G is usually not a non-singular matrix the weight matrix W is updated using apseudoinverse method

W G+D G

TG1113872 1113873

minus 1G

TD (61)

where G+ is pseudoinverse matrix and D is an expectingmatrix composed of detecting motion errors Yi e RBFnetwork increases the speed of training by using a localtransfer function so that a few neurons have a nonzeroresponse and become active to each input value RBF net-work can avoid falling into local minimumwhen the trainingis in process e variable RBF networks are used to rep-resent time-varying dynamics with both the structure andparameters which are tuned in real time

ΔWkj η dk minus WTkΦ1113872 1113873φj (62)

According to the low dynamic and high dynamic en-vironment the different control strategies of low and highdynamic environments are adopted to solve dynamic ac-curacy of attitude calculation e proposed method can

estimate the attitude angles utilizing affordable sensorsunder different dynamic situations e switching criterionis determined according to roll angle rates of body eattitude algorithm of single sample quaternion vector andattitude algorithm based on accelerometer gyro andmagnetometer are adopted in low dynamic environmentAlternatively cubic spline algorithm based on quaternionvector based on Micro-IMU and GPS is designed in the highdynamic environment Considering the situation of GPSfailure an attitude algorithm based on RBF-ANN feedbackcorrection is adopted to guarantee the reliable measuremente estimated deviation of system state δ1113954xk|k will be pro-vided based on RBF-ANN Furthermore the adaptive scalefactor μk is set to reduce or enhance the influence of previousmeasurements on the current estimation in AUKF In thisway a reasonable balance between system prediction andobservation is achievede scheme of the proposed methodis shown in Figure 4

is scheme is actually a combined adaptive filteringapproach and RBF algorithm architecture It comprises twoparts AUKF based on multisensor fusion module and RBFestimation method e multisensor fusion module consistsof micro-IMU magnetometer and single antenna GPS edominant errors are compensated by the calibration processbefore fusion algorithm Different fusion strategies provideefficient solutions to enhance the accuracy of the attitudeestimation

4 Experimental Results

41 Attitude Determination Experiment in Low DynamicEnvironment To verify the effect of the proposed algorithmin the low dynamic environment the attitude testing ex-periments are implemented in tracking human body mo-tions as shown in Figure 5

e different algorithms are implemented in DSP de-velopment prototype e attitudes estimated separately bydifferent sensors and filter algorithms are demonstrated inFigure 6 Data is denoted as ldquoGyrordquo ldquoAccMagrdquo ldquoCFrdquoldquoEKFrdquo ldquoUKFrdquo and ldquoReferencerdquo respectively derived fromgyroscope integration based on quaternion algorithm andattitude algorithm based on the accelerometer and mag-netometer Complementary Filter Extended Kalman Filteralgorithm Unscented Kalman filter and referenced Mti-G

X1

X2

Xn

φ1

φ2

φn

sum

w1

w2

wn

Figure 3 Radial feedforward neuron structure diagram


e result shows that the single spline quaternion al-gorithm based on gyro has short-term accuracy but theerror will be accumulated over time due to gyro bias drifte attitude algorithm of the accelerometer is used toprovide the long-term accuracy estimation of the roll andpitche yaw angle is later calculated by the magnetometerIt is also affected by sensor error and vibration noise

e complementary filter (CF) method can provideaccurate Euler estimate in low dynamic condition It utilizescharacters of accelerometers that has long-term accuracythe gyroscope has only short-term accuracy and magne-tometer that has low noise and high bandwidth Accordingto the characteristics of the MEMS sensor the cut-offbandwidths of low-pass filter and high-pass filter are chosenas the weight coefficient

e attitude algorithms based on EKF and UKF havesimilar estimation performances e UKF has better ac-curacy than EKF due to undergoing the nonlinear mea-surement equation in filter e STD of the pitch roll andyaw angle is 172deg 311deg and 575deg respectively at all times inEKF e STD of attitude angles is 055deg 087deg and 093degrespectively in UKF e estimation of the yaw angle isaffected by the error of pitch and roll angle calculated by theaccelerometer and gyro

42 Attitude Determination Experiment in High DynamicEnvironment Considering the characteristics of spinningprojectile the testing experiments are performed to simulatethe situation that the rotation of the symmetry axis is in highdynamic and rest the rotations of other orthogonal axes arein low dynamic A portable single rotating table is designedwhich can be mounted in a mobile vehicle e single axisrotating table driving by the servo motor is used to simulatethe motion of the roll attitude for the spinning projectileMeanwhile the attitude changes for the pitch and yaw angleare achieved when the testing vehicle equipped with portabletable moves quickly in different routes e experimentalplatform consists the prototype of low-cost IMU a single-antenna GPS receiver prototype of DSPmicrocontroller theMTi-G servo motor slip ring DC to AC inverter DCregulated power laptop and testing vehicle e experi-mental system is shown in Figure 7

In experiment system the Mti-G device from the XsensMotion Technologies is used to provide reference attitudewhich consists MEMS gyro accelerometer and magne-tometer e sampling frequency is 100Hz to collect rawdata e measurements of the GPS receiver are provided at10Hz sampling frequency e single antenna is mounted inthe long pole which is held up outside the testing vehiclee micro-IMU GPS receiver and MTi-G are mounted inthe single axis rotating table which is connected to the servomotor directly e reference frames of micro-IMU andMTi-G are consistent during the rotating motion At thebeginning of the experiment the single axis rotating table isfitted into the floor of the testing vehicle e rated speed ofthe servo motor is 3000 rpm e experimental collectingreal-time data are transmitted to the laptop PC with the USBport from converting the RS232 serial port by slip ring

between rotating and nonrotating mechanism Experimentaltests were performed in the urban section

In order to verify the effect of the different filter algo-rithms and the proposed method several testing experi-ments are implemented in different rotating motions andtraveling paths A set of comparative experiments are carriedout to verify the accuracy of different algorithms e resultsare shown in Figure 8

Figure 8(a) illustrates the estimation result of attitudealgorithm by the accelerometer and magnetometer In thisscenario the rotating angular rate of servomotor varies from360degs to 575degs For the roll angle it can track with areference angle in the range of less than 360degs With theincrease of servo-motor speed the dynamic condition has agreat influence on the estimation accuracy e propagationof error occurred during the estimation of roll angle emaximum error of roll angle is 5deg In addition the pitchangle calculated by the attitude algorithm of the acceler-ometer and magnetometer deviates greatly from the refer-ence pitch angle in Figure 8(b) e performancedegradation can be attributed to the reason that the error ofpitch angle is caused due to the sensor errors and vibrationnoises e estimated pitch angle is affected by large errors(up to 70 during the peak angle) e results show that theinaccuracy calculation of the algorithm has a great influenceon the measurement precision As it is visible the calculatedroll and pitch can be acceptable in low dynamic condition Inhigh dynamic condition the measurement precision wasaffected greatly due to the inevitable disturbances and dy-namic motions So the calculated results of attitude algo-rithm by the accelerometer and magnetometer cannot beaffordable as the measurement vector in AUKF

To produce as many as possible driving maneuvers tovalidate the performance in complex driving conditions theexperiment was conducted on an overpass over the roadetesting trajectory is shown in Figure 9 It can produce largechanges in both the pitch and yaw angle during the ex-periment e corresponding experiments were carried outon the cases that GPS worked normally the vehicle was inturning with large sideslip angle and GPS signals wereaffected and lost by obstruction

e results of the on-vehicle test are presented from 0 sto 100 s During the testing period all attitude data werecollected and recorded in the embedded navigation sys-tem e GPS satellite signal is obstructed artificially tosimulate external environmental disturbance in part pathTo evaluate the effect of the proposed algorithm the realexperimental tests in the high dynamic environment arecarried out During the testing experiments the servomotor of single rotating table is performed actions au-tomatically to predesigned motion program e maxi-mum roll angle rate can reach 1080degs to simulate therotating of the spinning shell In the meantime thechanges of the pitch and yaw angle is simulated by thetesting vehicle riding in different routes e results arepresented in Figure 10 Data denoted as ldquoUKFrdquo ldquoAUKF-RBFrdquo ldquoReferencerdquo and ldquoGPS-Outagerdquo respectively de-rived from the conventional UKF adaptive UKF based onRBF network and GPS outage


As shown in Figure 10(a) UKF and AUKF-RBF algo-rithms can both track the reference roll angle within anglerate range of 1080degs e quaternion vector by cubic splinealgorithm is estimated in the filter Using quaternionupdating algorithm the roll angle is obtained Although itsaccuracy is affected by the model disturbances on currentestimates and the random error of IMU the performance isstill acceptable in UKF and AUKF-RBF algorithms Duringthe whole stage the maximum of the errors of roll angle inUKF and AUKF-RBF are 5deg and 2deg respectively AUKF-RBFalgorithm is effective and accurate in full tracking routethrough the tuning of the adaptive scale factor Mean SquareError (MSE) of AUKF-RBF algorithm is 042deg in dynamiccondition

It can also be found that the performance degradesduring the GPS outage(ie from 40 to 50 s) As the GPS-failure interval the performance of the proposed methodbecomes worse in comparison with that at the same intervalwithout GPS failure Owing to lack of enough measurementdata the estimation of the roll angle has no convergence anda trend of divergence in high rotating environment inFigure 10(b) To solve the loss of measurement data in GPS

outage section the proposed RBF-ANN algorithm cancontinue to offer data availabilitye estimated deviation ofsystem state δ1113954xk|k in equation (56) is replaced which isprovided by RBF-ANN e corrected system state 1113954xk|k canbe used for estimation process in AUKF-RBF and UKF

In Figures 10(c) and 10(e) the estimated pitch and yawangle are obtained Considering the fact that the micro-IMUis mounted by a cantilever beam in single axis rotating tablemechanical vibration of rotating motion may affect theaccuracy of the estimated pitch and yaw angle As it is visiblethe estimates of pitch and yaw angle based on UKF andAUKF can follow the reference attitude angle with the ac-ceptable error e errors for pitch and yaw angle are within079deg and 08deg in UKF algorithm e errors for pitch andyaw angle with adaptive estimation are within 039deg and043deg in AUKF-RBF algorithm During the intervals of10ndash20 s and 20ndash30 s the estimated yaw angle greatly changedin the opposite direction at sample time t 83 s t 25 s andt 30 s e estimated yaw angle is affected by large sideslipangle in Figures 10(e) and 10(g) Correspondingly the es-timated errors come out at these sampling times inFigure 10(f) ese errors increase by an average of 05deg ascompared with the errors in the steady section InFigure 10(h) the yaw angle is estimated depending on RBFfeedback and tuning of adaptive scale factor in AUKF andthe fluctuation of error decreased faster in the GPS outage

e convergence of RBF in UKF and AUKF is shown inFigure 11

As can be seen from above the training time in AUKFalgorithm takes about 825 s Compared with UKF algo-rithm the costing time of training process takes about1325 s Obviously in the process of training RBF network itspends less time than UKF algorithm

e attitude angles are estimated in both UKF andAUKF-RBF algorithms e AUKF-RBF algorithm has

GPSreceiver

MEMSaccelerometer

Filter algorithmbased on noise

removal

Calibration amperror

compensation


compensation

Attitudedetermination-

basedaccelerometer

Roll amp pitchestimation

MEMSgyroscope

GPS outage or unavailable sideslip

Yawestimation

Low dynamic environment

Magnetometer

High dynamic environmentThe acceleration calculation

in navigation frame

State updating

AUKF

Y

N

Attitude determination-basedon equivalent rotation

vector algorithm

Measurement update

Initialization (generation of sigmapoints and weights)

Time update

The adaptivescale factor

tuning

State update

The calculation of predicted error

GPS signal isavailable

RBF-ANN

GPS works normally


compensation


basedmagnetometer

N+

YAttitude

data

f bf b~

mb mb~

Vn Vn

ωbib

~

ωbib

~

δXekkndash1

δXkkndash1

Zk Zk|kndash1

Xkkndash1Xkk

Figure 4 e scheme of combining AUKF fusion algorithm with RBF-ANN for low-cost micro-IMU and GPS

Micro-IMU Mti-G

Figure 5 Attitude testing experiment in low dynamicenvironment


100

50

0

ndash50

Roll

angl

e (de

gree

)

0 10 20 30 40 50 60Time (s)

GyroEKFAccMag

UKFCFReference

50

0

ndash5052 53 54 55

(a)

Roll

angl

e err

or (d

egre

e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

40

30

20

10

0

ndash10

ndash20

ndash30

ndash40

ndash50

(b)

30

0

ndash30

ndash60

ndash90

Pitc

h an

gle (

degr

ee)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

0

ndash40

ndash8025 26 27 28

(c)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

20

15

10

5

0

ndash5

ndash10

ndash15

ndash20

Pitc

h an

gle e

rror

(deg

ree)

(d)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

Yaw

angl

e (de

gree

)

180

150

120

90

60

30

ndash30

ndash60

ndash90

0

8040

0ndash40

11 12 13 14 15

(e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

0

ndash50

ndash100

Yaw

angl

e err

or (d

egre

e)

(f )

Figure 6 Attitude results in low dynamic environment


better estimation precision than UKF during the GPS outagesection RBF correction algorithm is regarded to be reliableand can be utilized by AUKF e estimated precision ofRBF depends on the accuracy of training datae estimatedattitude angles in AUKF-RBF algorithm provided morereliable results as training samples than UKF algorithm

during the GPS nonoutage stage Compared with the per-formance of GPS normal working sections (ie 10ndash20 s) andGPS outage section (ie 40ndash50 s) the RMS of the proposedAUKF-RBF is shown in Table 2

It can also be found that the performance degradesduring the GPS outage e estimation error in GPS outage

GPS antenna

(a)

MTi-G DC power

Slip ring

Servo motorServo driver

Micro IMU

GPS signal repeater

GPS receiver

(b)

Figure 7 Experimental platform of microembedded navigation system

200

100

0

ndash100

ndash200

Roll

angl

e (de

gree

)

0 2 4 6 8 10 12 14 16 18 20Time (s)

AccMagReference

(a)

AccMagReference

0 2 4 6 8 10 12 14 16 18 20Time (s)

50

0

ndash50

Pitc

h an

gle (

degr

ee)

(b)

Figure 8 (a) Roll angle and (b) pitch angle in high dynamic condition

End

Start

GPS outage

Figure 9 Testing trajectory of the attitude determination system


0 20 40 60 80 100Time (s)

UKFAUKF-RBFReference

200

150

100

50

0

ndash50

ndash100

ndash150

ndash200

Roll

angl

e (de

gree

)150

100

50

0

ndash50

ndash100

ndash150

0 87654321

60 62 64 66 68

150

100

50

0

ndash50

ndash100

ndash150

(a)

0 20 40 60 80 100Time (s)

UKFAUKF-RBFGPS-outage

Roll

angl

e err

or (d

egre

e)

50

0

ndash50

ndash100ndash5

0

5

ndash150

(b)

UKFAUKF-RBF

ReferenceGPS-outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

4

2

0

ndash2

ndash4

Pitc

h an

gle (

degr

ee)

(c)


0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

Pitc

h an

gle e

rror

(deg

ree)

3

2

1

0

ndash1

ndash2

ndash3

(d)

UKFAUKF-RBF

ReferenceGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

ndash150

ndash155

ndash160

ndash165

ndash170

ndash175

ndash180

Yaw

angl

e (de

gree

)

GPS outage

GPS normal working

(e)

UKFAUKF-RBFGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

Yaw

angl

e err

or (d

egre

e)

GPS outage

3

4

2

1

0

ndash1

ndash2

ndash3

ndash4

(f )

Figure 10 Continued


section is larger than the error in GPS normal workingsection e reason is that the reliable measurements areprovided by GPS and accelerometer in the GPS normalworking section On the contrary the measurements wereestimated by RBF in GPS outage e corresponding pro-grams of the proposed algorithm is compiled and run onCCS 33 environment for the DSP controller in real timeecalculating time takes 896ms in a sampling period Forbrevity we will concentrate on comparing the performancesof proposed in the whole stage e AUKF-RBF algorithmcan not only provide accurate estimation GPS normalworking section but also guarantee reliable estimation in theGPS-denied section e attitude angle estimation errorstatistics of various methods in testing experiment aresummarized in Table 3

Compared with performance of the abovementionedmethods the proposed AUKF-RBF method provides moreaccurate and reliable estimation whether in normal workingcondition or GPS failure It has good estimation perfor-mance in the whole section which suffers from vibrationnoises due to inevitable disturbances and high rotatingmotion e MSE of roll pitch and yaw based on theproposed method is 04212deg 03879deg and 04254degrespectively

5 Conclusions

e attitude determination method combining AUKF withRBF-ANN is proposed Considering the situations in lowdynamic and high dynamic environment the differentcontrol strategies fusing the MEMS multisensors areadopted Using the adaptive scale factor the influence on thenew measurement data is weakened or enhanced during theprocess of the estimation in AUKF e attitude algorithmbased on RBF-ANN feedback correction can continue toprovide reliable estimated deviation information in GPSoutage A portable experiment system to simulate high


Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)

Figure 10 Attitude determination results in high dynamic environment

00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal

Figure 11 e convergence process of RBF-ANN

Table 2 Contrast of mean square error of GPS failure and GPSnormal

Roll Pitch YawUKF-GPS outage 20048 09882 08042UKF-GPS normal 19740 06633 05834AUKF-RBF- GPS outage 04185 03816 03658

Table 3 Mean square error of the attitude angle in the whole stage

Roll Pitch YawUKF 19857 07913 08049AUKF-RBF 04212 03879 04254


rotating motion of spinning projectile simulate is setup eperformance of the proposed method has been evaluatedand verified through testing experiments for comparing withother representative methods for attitude estimation Ex-perimental results indicate that the proposed method ex-hibits reliable and satisfactory performance under variousdynamic conditions as well as GPS-friendly or temporarilyGPS-denied conditions e calculating time of the pro-posed method in DSP controller takes 896ms in a samplingperiod e proposed method has good estimation perfor-mance in the whole section e MSE of roll pitch and yawbased on the proposed method is 04212deg 03879deg and04254deg respectively

Data Availability

e datasets used to support the findings of this study areavailable from the corresponding author on reasonablerequest

Conflicts of Interest

e authors declare that there are no conflicts of interestregarding the publication of this paper

Acknowledgments

e work was supported by Natural Science Foundation ofLiaoning Province (Grant no 20180550714) Correspondingexperimental tests are carried out at Research Centre ofWeaponry Science and Technology in Shenyang LigongUniversity e authors would like to thank their colleaguesfor their support in implementing the experiments

References

[1] L Zhang Z Xiong J Lai and J Liu ldquoOptical flow-aidednavigation for UAV a novel information fusion of integratedMEMS navigation systemrdquoOptik vol 127 no 1 pp 447ndash4512016

[2] B Kada K Munawar M S Shaikh M A Hussaini andU M Al-Saggaf ldquoUAV attitude estimation using nonlinearfiltering and low-cost mems sensorsrdquo IFAC-PapersOnLinevol 49 no 21 pp 521ndash528 2016

[3] X Li C Y Chan and Y Wang ldquoA reliable fusion meth-odology for simultaneous estimation of vehicle sideslip andyaw anglesrdquo IEEE Transactions on Vehicular Technologyvol 65 no 6 pp 4440ndash4458 2018

[4] L Chang F Zha and F Qin ldquoIndirect Kalman filtering basedattitude estimation for low-cost attitude and heading refer-ence systemsrdquo IEEEASME Transactions on Mechatronicsvol 22 no 4 pp 1850ndash1858 2017

[5] F C Liu Z Su and H Zhao ldquoAttitude measurement forhigh-spinning projectile with a hollowMEMS IMU consistingof multiple accelerometers and gyrosrdquo Sensors vol 19 no 8pp 1ndash25 2019

[6] J J Zhang J Li and X R Che ldquoe optimal design ofmodulation angular rate for MEMS-based rotary semi-SINSrdquoMicromachines vol 10 no 111 pp 2ndash12 2019

[7] N Davari and A Gholami ldquoAn Asynchronous adaptive directKalman filter algorithm to improve underwater navigation

system performancerdquo IEEE Sensors Journal vol 17 no 4pp 1061ndash1068 2017

[8] R Costanzi F Fanelli N Monni A Ridolfi and B AllottaldquoAn attitude estimation algorithm for mobile robots underunknown magnetic disturbancesrdquo IEEEASME Transactionson Mechatronics vol 21 no 4 pp 1900ndash1911 2016

[9] M S Challa J G Moore and D J Rogers ldquoA simple attitudeunscented kalman filter theory and evaluation in a magne-tometer-only spacecraft scenariordquo IEEE Access vol 4pp 1845ndash1858 2016

[10] M Zhong J Guo and Z Yang ldquoOn real time performanceevaluation of the inertial sensors for INSGPS integratedsystemsrdquo IEEE Sensors Journal vol 16 no 17 pp 6652ndash66612016

[11] J Wu Z Zhou R Li L Yang and H Fourati ldquoAttitudedetermination using a single sensor observation analyticquaternion solutions and property discussionrdquo IET ScienceMeasurement amp Technology vol 11 no 6 pp 731ndash739 2017

[12] Y-C Lai and S-S Jan ldquoAttitude estimation based on fusion ofgyroscopes and single antenna GPS for small UAVs under theinfluence of vibrationrdquo GPS Solutions vol 15 no 1pp 67ndash77 2011

[13] Y Wang A Hussain and M Soltani ldquoA MEMS-basedadaptive AHRS for marine satellite tracking antennardquo IFAC-PapersOnLine vol 48 no 16 pp 121ndash126 2015

[14] A Bethany I LGMark and Z Ryan ldquoBallistic roll estimationusing EKF frequency tracking and adaptive noise cancella-tionrdquo IEEE Transactions on Aerospace and Electronic Systemsvol 49 no 4 pp 2546ndash2553 2013

[15] A C B Chiella and B O S Teixeira ldquoQuaternion-basedrobust attitude estimation using an adaptive unscented Kal-man filterrdquo Sensors vol 19 no 10 pp 2ndash19 2019

[16] X L Xu X C Tian L L Zhou and Y B Li ldquoA decision-treebased multiple-model UKF for attitude estimation using low-cost MEMS MARG sensor arraysrdquo Measurement vol 135pp 355ndash367 2018

[17] N Koksal M Jalamaa and B Fidan ldquoAdaptive linear quadraticattitude tracking control of a quadrotor UAV based on IMUsensor data fusionrdquordquo Sensors vol 19 no 46 pp 2ndash23 2019

[18] A Noordin M A M Basri and Z Mohamed ldquoSensor fusionfor attitude estimation and PID control of quadrotor UAVrdquoInternational Journal of Electrical and Electronic Engineeringamp Telecommunicationsvol 7 no 4 pp 183ndash189 2018

[19] Y P Ning J Wang and H Z Han ldquoAn optimal radial basisfunction neural network enhanced adaptive robust Kalmanfilter for GNSSINS integrated systems in complex urbanareasrdquo Sensors vol 18 no 9 pp 2ndash21 2018

[20] N Hossein and K Jafar ldquoFuzzy adaptive integration schemefor low cost SINSGPS navigation systemrdquo Mechanical Sys-tems and Signal Processing vol 99 pp 434ndash449 2018

[21] D Wang H Lv and J Wu ldquoIn-flight initial alignment forsmall UAV MEMS-based navigation via adaptive unscentedKalman filtering approachrdquo Aerospace Science and Technol-ogy vol 61 pp 73ndash84 2017

[22] M M Teshnizi and A Shirazi ldquoAttitude estimation andsensor identification utilizing nonlinear filters based on a low-cost MEMS magnetometer and sun sensorrdquo IEEE Aerospaceand Electronic Systems Magazine vol 30 no 12 pp 20ndash332015

[23] L Wang Y Guan and X Hu ldquoCompounded calibrationbased on FNN and attitude estimation method using intel-ligent filtering for low cost MEMS sensor applicationrdquoMathematical Problems in Engineering vol 2019 Article ID4514873 13 pages 2019


[24] M M Carrasco and A L da Silva ldquoDetermination of anattitude estimation unit with inertial sensors modelled byAllan variancerdquo IEEE Latin America Transactions vol 13no 8 pp 2500ndash2505 2015

[25] S Julier J Uhlmann and H F Durrant-Whyte ldquoA newmethod for the nonlinear transformation of means and co-variances in filters and estimatorsrdquo IEEE Transactions onAutomatic Control vol 45 no 3 pp 477ndash482 2000

[26] Y Yang H He and G Xu ldquoAdaptively robust filtering forkinematic geodetic positioningrdquo Journal of Geodesy vol 75no 2-3 pp 109ndash116 2001


Teixeira [15] proposed a quaternion-based robust adaptiveunscented Kalman Filter for attitude estimation In the filterthe adaptive strategy based on covariance matching canadjust the measurement covariance matrix online to dealwith the slow time-varying disturbance in the sensors Xuet al [16] proposed a multiple-model unscented Kalmanfilter for attitude estimation which is a quaternion-basedattitude estimator that fuses related strap-down MagneticAngular Rate and Gravity (MARG) sensor arrays

In order to overcome the drawbacks of the abovementionedmethods and take full advantage of fusion and adaptive filteringscheme some fusionmethodologies combining complementaryfiltering or Kalman filtering are proposed Koksal et al [17]combines Kalman filter and complementary filter to provideaccurate attitude estimation and control performance forquadrotor UAV Noordin et al [18] proposed a sensor fusionalgorithm for UAV attitude estimation based on nonlinearcomplementary filter (NCF) To obtain reliable attitude esti-mation gyroscope is fused to the accelerometer and magne-tometer to correct drift error of the gyroscope Otherrepresentative research studies are the combination of filteringand intelligent control scheme Ning et al [19] propose a robustKalmanfilter combinedwithRBF-ANNalgorithm to isolate andreduce the influence of the dynamic model error and GNSSobservation gross error Hossein and Jafer [20] adjusts the fuzzyweighting coefficients to correct the orientation estimation byusage of SINSGPS and AHRS data

e above works on attitude determination algorithm lie inthe following problems e traditional EKF and other im-proved EKF have the drawbacks such as low estimation ac-curacy and poor stability which will cause the biasedestimation or even divergence due to which the statisticalproperties of system and measurement noise cannot be pre-dicted accurately especially for low-cost MEMS-based sensors[21] Some complicated nonlinear calculationwill cause a heavyburden in real-time micronavigation system which willweaken the feasibility for application [2 22] erefore theadvanced filtering or adaptive filtering methods will play a keyrole in these applications [6 13 17 23]

is paper concentrates on developing the customizedattitude determination algorithm of low-cost MEMS sensorin high dynamic environmente corresponding techniquecan be widely applied in measuring the attitude of theprojectile and spinning shell of low dynamic rotatingCombined with the proposed adaptive Kalman filteringmethod and intelligent control scheme it is suitable fordealing with time-varying noise of low-cost small-sizemicro-IMU and dynamic interference e novel aspects ofthis paper can be summarized as follows

(1) According to dynamic rotating environment a newattitude estimation scheme of AUKF is designedMeanwhile different control strategies are alsoadopted to fuse MEMS multi-inertial sensors underdifferent dynamic conditions AUKF attitude de-termination method based on MEMS sensor andGPS can provide reliable estimation under highdynamic environment change Using the adaptivescale factor the influence on the new measurement

data is weakened or enhanced during the process ofthe estimation according to the predicted residualvector

(2) e proposed method can accommodate sensorfailure particularly GPS failure in adverse envi-ronment When GPS signals are temporarilyblocked the attitude algorithm based on RBF-ANNfeedback correction can continue to provide reliableestimated deviation information e correctedpredicted system state will be used in the estimationprocess in AUKF

(3) Considering the space limitations in spinning pro-jectile the small size MEMS IMU is designed ecustomized prototype of the DSP controller is de-veloped for it e corresponding programs of theproposed algorithm can be compiled and run on inreal time

e remainder of this paper is organized as followsSection 2 presentsMEMS IMU configuration and design themodel of MEMS sensor and related calibration work eAUKF algorithm and RBF feedback correction are devel-oped in detail in Section 3 en experimental results anddiscussion are stated in Section 4

2 Modeling and Related Work

21 Sensor Configuration and Design According to applica-tion background of the spinning projectile a small size lowpower consuming and low-cost MEMS IMU is developedMicroinertial navigation system consists of a DSP micro-controller AD converter flash powermodulemicro-IMU andmagnetometeremicro-IMU is composed of three single-axisgyroscopes and accelerometers It is shown in Figure 1

In Figure 1 a six layer MEMS IMU PCB board is designedwith size of 45times 45mm e output analog signals of the ac-celerometer and magnetometer are sampled simultaneously by8-channel 16bit analog-to-digital converter e simple andeffective communication between DSP microcontroller andgyroscope is realized through SPI interface Angular rate data isoutput in 12-bit 2rsquos complement format at a maximum rate of2000 samples per second Considering the real-time perfor-mance the proposed algorithm ismigrated fromMATLAB toCcodeese codes can be compiled in CCS33 environment andloaded into DSP 28335 microcontroller for real-time operation

22 Sensor Model Low-cost small size MEMS inertialsensors are widely used in these applications due to thedimension constraint cost consideration in projectile andspinning shell [5 6] Gyroscopes and accelerometers arecommonly known as inertial sensors and their orthogonaltriads generally form an IMU used as a core means of anavigation systeme precision of gyro is constraint to timevarying bias drift and noise e true output of the gyro canbe defined as follows

ωbib S

minus1g

1113958ωbib minus bg1113874 1113875 (1)





Sg

kgx σxy σxz

σyx kgy σyz

σzx σzy kgz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (2)



fb

MaSa1113957fb minus ba1113872 1113873 (3)



Ma

1 0 0

ζxy 1 0

ζzx ζzy 1

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

Sa

kax 0 0

0 kay 0

0 0 kaz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(4)


θacc minusarctanfb

x

g1113888 1113889

ϕacc arctanfb

y


(5)

where fbx and fb



mb

Cminus1e

1113958mb minus ε1113872 1113873 (6)


mb

Cbnm

n (7)


mpx

mpy

mpz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦


0 cosϕ minussinϕ


⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ middot

mbx

mby

mbz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (8)


ψm arctanm

py

mpx

1113888 1113889 (9)


y

x

z

(a)

45mm

(b)


(c)








3 Methodology



X q bg1113960 1113961T

7times1(10)



Cnb




⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (11)


_X7times1 f q bt( 1113857 12

A4times4 04times3

03times4 B3times3


A4times4



1113957ωbnbx 0 1113957ωb



nbz 0 1113957ωbnbx

1113957ωbnbz 1113957ωb


⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦


1 0 00 1 00 0 1

⎡⎢⎢⎢⎢⎢⎣⎤⎥⎥⎥⎥⎥⎦


(13)



1113957ωbnb 1113957ωb


Tωnin ωb


nb( 1113857

Tωnin (14)


in ωnie + ωn

en evector ωn









q0(t + h)

q1(t + h)

q2(t + h)

q3(t + h)

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

cos|Φ|

2minusΦx

|Φ|sin

|Φ|

2minusΦy

|Φ|sin

|Φ|

2

Φx

|Φ|sin

|Φ|

2cos

|Φ|

2Φz

|Φ|sin

|Φ|

2

Φy

|Φ|sin

|Φ|

2minusΦz

|Φ|sin

|Φ|

2cos

|Φ|

2

minusΦz

|Φ|sin

|Φ|

2

minusΦy

|Φ|sin

|Φ|

2

Φx

|Φ|sin

|Φ|

2

Φz

|Φ|sin

|Φ|

2Φy

|Φ|sin

|Φ|

2minusΦx

|Φ|sin

|Φ|

2cos

|Φ|

2

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

q0(t)

q1(t)

q2(t)

q3(t)

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(16)




ballistic attitudes




Data preprocessing


Initial calibration






Mti

Micro-IMU

Slip ring








_Φ 1113957ωbnb +



nb1113872 1113873 (17)






h3



(19)


θi 1113946(i3)h

(iminus13)hω tk + τ( 1113857dτ i 1 2 3 (20)


θ1 13

ah +19

bh2

+127

ch3

θ2 13

ah +39

bh2

+727

ch3

θ3 13

ah +59

bh2

+1927

ch3

(21)


Φ(h) Φ(0) + _Φ(o)h +12Φ_(0)h

2+16Φt(0)h

3+

124Φ(4)

(0)h4

+1120Φ(5)

(0)h5

ah + bh2

+ ch3

+16

(a times b)h3

+14

(a times c)h4

+110

(b times c)h5

θ1 +θ2 +θ3 +3380θ1 timesθ3

+5780θ2 times θ2 minusθ3( 1113857




ϕacc

θacc

ψmag

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

arctan2q2q3 + 2q0q1

2q20 + 2q23 minus 11113888 1113889


arctan2q1q2 + 2q0q3

2q20 + 2q21 minus 11113888 1113889

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

+ nablam (23)





Z Fn

Fnx

Fny

Fnz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ Cnbf

b+ v (24)



Cnb




⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(25)


Fn

_Vn

+ 2ωnie + ωn


minus gn (26)









1113960 1113961

⎧⎨

⎩ (27)



(n + λ)Pkminus1

1113969



(n + λ)Pkminus1

1113969


(28)


Wmi

λn + λ

i 0

λ2(n + λ)

ine 0

⎧⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎩

Wci

λn + λ


λ2(n + λ)

ine 0

(i 0 1 2 n 2n)

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎩

(29)




1113954xk|kminus1 11139442n

i0W

mi x

ik|kminus1


i0W

ci x



T

(30)



Zik|kminus1 h x

ik | kminus11113872 1113873 i 1 2 2n (31)

1113954Zk|kminus1 11139442n

i0W

mi Z

ik|kminus1 (32)

Pzzk 1113944

2n

i0W

ci Z



T+ 1113954Rk

Pxzk 1113944

2n

i0W

ci x



T

(33)

where

Kk Pxzk P

zzk( 1113857

minus 1 (34)



Tk (36)

1113954xak|kminus1 1113954xT

k|kminus 1 01113960 1113961T (37)

Pak|kminus1

Pk|kminus1 00 Qk

1113890 1113891 (38)


k|k and 1113954Pa





E εk+jεTk1113872 1113873 0 k 1 2 j 1 2 (40)



zz


Pzzk μk

1113954Pzz

k + 1113954Rk 1 minus μk( 1113857 (41)



zz


zz

k εkεTk (42)



(43)







k1113960 1113961 (45)



1113954Pzz

k1113960 1113961 (46)






1113954Pzz

k 1μk

1113944

2n

i0W

ci Z



T+ 1113954Rk

(48)

1113954Pxz

k 1μk

1113944

2n

i0W

ci x



T

(49)1113954Kk 1113954P

xz

k1113954P

xz

k1113872 1113873minus 1


(51)

1113954Pk|k 1μk


zz

k1113954K

T

k



Pzzk asymp μk

1113954Pzz

k (53)


zz


Mkμk Nk (54)


μk 1 μ0 lt 1

μ0 μ0 ge 11113896 (55)











2σ221113888 1113889 (57)



σ2 dmax2m

radic (58)


yi 1113944N

i1Wiφ x minus ci( 1113857 1113944

N

i1Wi exp minus

x minus c2i2σ22

1113888 1113889 (59)




⋮ ⋱ ⋮


⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

W1

⋮

Wm

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

Y1

⋮

Ym

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (60)


W G+D G

TG1113872 1113873

minus 1G

TD (61)









X1

X2

Xn

φ1

φ2

φn

sum

w1

w2

wn





















GPSreceiver

MEMSaccelerometer


removal


compensation


compensation


basedaccelerometer


MEMSgyroscope


Yawestimation


Magnetometer


in navigation frame

State updating

AUKF

Y

N


vector algorithm

Measurement update


Time update


tuning

State update



RBF-ANN

GPS works normally


compensation


basedmagnetometer

N+

YAttitude

data

f bf b~

mb mb~

Vn Vn

ωbib

~

ωbib

~

δXekkndash1

δXkkndash1

Zk Zk|kndash1

Xkkndash1Xkk


Micro-IMU Mti-G



100

50

0

ndash50

Roll

angl

e (de

gree

)

0 10 20 30 40 50 60Time (s)

GyroEKFAccMag

UKFCFReference

50

0

ndash5052 53 54 55

(a)

Roll

angl

e err

or (d

egre

e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

40

30

20

10

0

ndash10

ndash20

ndash30

ndash40

ndash50

(b)

30

0

ndash30

ndash60

ndash90

Pitc

h an

gle (

degr

ee)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

0

ndash40

ndash8025 26 27 28

(c)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

20

15

10

5

0

ndash5

ndash10

ndash15

ndash20

Pitc

h an

gle e

rror

(deg

ree)

(d)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

Yaw

angl

e (de

gree

)

180

150

120

90

60

30

ndash30

ndash60

ndash90

0

8040

0ndash40

11 12 13 14 15

(e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

0

ndash50

ndash100

Yaw

angl

e err

or (d

egre

e)

(f )






GPS antenna

(a)

MTi-G DC power

Slip ring


Micro IMU

GPS signal repeater

GPS receiver

(b)


200

100

0

ndash100

ndash200

Roll

angl

e (de

gree

)

0 2 4 6 8 10 12 14 16 18 20Time (s)

AccMagReference

(a)

AccMagReference

0 2 4 6 8 10 12 14 16 18 20Time (s)

50

0

ndash50

Pitc

h an

gle (

degr

ee)

(b)


End

Start

GPS outage



0 20 40 60 80 100Time (s)


200

150

100

50

0

ndash50

ndash100

ndash150

ndash200

Roll

angl

e (de

gree

)150

100

50

0

ndash50

ndash100

ndash150

0 87654321

60 62 64 66 68

150

100

50

0

ndash50

ndash100

ndash150

(a)

0 20 40 60 80 100Time (s)


Roll

angl

e err

or (d

egre

e)

50

0

ndash50

ndash100ndash5

0

5

ndash150

(b)

UKFAUKF-RBF

ReferenceGPS-outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

4

2

0

ndash2

ndash4

Pitc

h an

gle (

degr

ee)

(c)


0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

Pitc

h an

gle e

rror

(deg

ree)

3

2

1

0

ndash1

ndash2

ndash3

(d)

UKFAUKF-RBF

ReferenceGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

ndash150

ndash155

ndash160

ndash165

ndash170

ndash175

ndash180

Yaw

angl

e (de

gree

)

GPS outage

GPS normal working

(e)


0 10 20 30 40 50 60 70 80 90 100Time (s)

Yaw

angl

e err

or (d

egre

e)

GPS outage

3

4

2

1

0

ndash1

ndash2

ndash3

ndash4

(f )

Figure 10 Continued




5 Conclusions



Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)


00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal








Data Availability




Acknowledgments


References

































Sg

kgx σxy σxz

σyx kgy σyz

σzx σzy kgz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (2)



fb

MaSa1113957fb minus ba1113872 1113873 (3)



Ma

1 0 0

ζxy 1 0

ζzx ζzy 1

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

Sa

kax 0 0

0 kay 0

0 0 kaz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(4)


θacc minusarctanfb

x

g1113888 1113889

ϕacc arctanfb

y


(5)

where fbx and fb



mb

Cminus1e

1113958mb minus ε1113872 1113873 (6)


mb

Cbnm

n (7)


mpx

mpy

mpz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦


0 cosϕ minussinϕ


⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ middot

mbx

mby

mbz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (8)


ψm arctanm

py

mpx

1113888 1113889 (9)


y

x

z

(a)

45mm

(b)


(c)








3 Methodology



X q bg1113960 1113961T

7times1(10)



Cnb




⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (11)


_X7times1 f q bt( 1113857 12

A4times4 04times3

03times4 B3times3


A4times4



1113957ωbnbx 0 1113957ωb



nbz 0 1113957ωbnbx

1113957ωbnbz 1113957ωb


⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦


1 0 00 1 00 0 1

⎡⎢⎢⎢⎢⎢⎣⎤⎥⎥⎥⎥⎥⎦


(13)



1113957ωbnb 1113957ωb


Tωnin ωb


nb( 1113857

Tωnin (14)


in ωnie + ωn

en evector ωn









q0(t + h)

q1(t + h)

q2(t + h)

q3(t + h)

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

cos|Φ|

2minusΦx

|Φ|sin

|Φ|

2minusΦy

|Φ|sin

|Φ|

2

Φx

|Φ|sin

|Φ|

2cos

|Φ|

2Φz

|Φ|sin

|Φ|

2

Φy

|Φ|sin

|Φ|

2minusΦz

|Φ|sin

|Φ|

2cos

|Φ|

2

minusΦz

|Φ|sin

|Φ|

2

minusΦy

|Φ|sin

|Φ|

2

Φx

|Φ|sin

|Φ|

2

Φz

|Φ|sin

|Φ|

2Φy

|Φ|sin

|Φ|

2minusΦx

|Φ|sin

|Φ|

2cos

|Φ|

2

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

q0(t)

q1(t)

q2(t)

q3(t)

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(16)




ballistic attitudes




Data preprocessing


Initial calibration






Mti

Micro-IMU

Slip ring








_Φ 1113957ωbnb +



nb1113872 1113873 (17)






h3



(19)


θi 1113946(i3)h

(iminus13)hω tk + τ( 1113857dτ i 1 2 3 (20)


θ1 13

ah +19

bh2

+127

ch3

θ2 13

ah +39

bh2

+727

ch3

θ3 13

ah +59

bh2

+1927

ch3

(21)


Φ(h) Φ(0) + _Φ(o)h +12Φ_(0)h

2+16Φt(0)h

3+

124Φ(4)

(0)h4

+1120Φ(5)

(0)h5

ah + bh2

+ ch3

+16

(a times b)h3

+14

(a times c)h4

+110

(b times c)h5

θ1 +θ2 +θ3 +3380θ1 timesθ3

+5780θ2 times θ2 minusθ3( 1113857




ϕacc

θacc

ψmag

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

arctan2q2q3 + 2q0q1

2q20 + 2q23 minus 11113888 1113889


arctan2q1q2 + 2q0q3

2q20 + 2q21 minus 11113888 1113889

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

+ nablam (23)





Z Fn

Fnx

Fny

Fnz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ Cnbf

b+ v (24)



Cnb




⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(25)


Fn

_Vn

+ 2ωnie + ωn


minus gn (26)









1113960 1113961

⎧⎨

⎩ (27)



(n + λ)Pkminus1

1113969



(n + λ)Pkminus1

1113969


(28)


Wmi

λn + λ

i 0

λ2(n + λ)

ine 0

⎧⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎩

Wci

λn + λ


λ2(n + λ)

ine 0

(i 0 1 2 n 2n)

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎩

(29)




1113954xk|kminus1 11139442n

i0W

mi x

ik|kminus1


i0W

ci x



T

(30)



Zik|kminus1 h x

ik | kminus11113872 1113873 i 1 2 2n (31)

1113954Zk|kminus1 11139442n

i0W

mi Z

ik|kminus1 (32)

Pzzk 1113944

2n

i0W

ci Z



T+ 1113954Rk

Pxzk 1113944

2n

i0W

ci x



T

(33)

where

Kk Pxzk P

zzk( 1113857

minus 1 (34)



Tk (36)

1113954xak|kminus1 1113954xT

k|kminus 1 01113960 1113961T (37)

Pak|kminus1

Pk|kminus1 00 Qk

1113890 1113891 (38)


k|k and 1113954Pa





E εk+jεTk1113872 1113873 0 k 1 2 j 1 2 (40)



zz


Pzzk μk

1113954Pzz

k + 1113954Rk 1 minus μk( 1113857 (41)



zz


zz

k εkεTk (42)



(43)







k1113960 1113961 (45)



1113954Pzz

k1113960 1113961 (46)






1113954Pzz

k 1μk

1113944

2n

i0W

ci Z



T+ 1113954Rk

(48)

1113954Pxz

k 1μk

1113944

2n

i0W

ci x



T

(49)1113954Kk 1113954P

xz

k1113954P

xz

k1113872 1113873minus 1


(51)

1113954Pk|k 1μk


zz

k1113954K

T

k



Pzzk asymp μk

1113954Pzz

k (53)


zz


Mkμk Nk (54)


μk 1 μ0 lt 1

μ0 μ0 ge 11113896 (55)











2σ221113888 1113889 (57)



σ2 dmax2m

radic (58)


yi 1113944N

i1Wiφ x minus ci( 1113857 1113944

N

i1Wi exp minus

x minus c2i2σ22

1113888 1113889 (59)




⋮ ⋱ ⋮


⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

W1

⋮

Wm

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

Y1

⋮

Ym

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (60)


W G+D G

TG1113872 1113873

minus 1G

TD (61)









X1

X2

Xn

φ1

φ2

φn

sum

w1

w2

wn





















GPSreceiver

MEMSaccelerometer


removal


compensation


compensation


basedaccelerometer


MEMSgyroscope


Yawestimation


Magnetometer


in navigation frame

State updating

AUKF

Y

N


vector algorithm

Measurement update


Time update


tuning

State update



RBF-ANN

GPS works normally


compensation


basedmagnetometer

N+

YAttitude

data

f bf b~

mb mb~

Vn Vn

ωbib

~

ωbib

~

δXekkndash1

δXkkndash1

Zk Zk|kndash1

Xkkndash1Xkk


Micro-IMU Mti-G



100

50

0

ndash50

Roll

angl

e (de

gree

)

0 10 20 30 40 50 60Time (s)

GyroEKFAccMag

UKFCFReference

50

0

ndash5052 53 54 55

(a)

Roll

angl

e err

or (d

egre

e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

40

30

20

10

0

ndash10

ndash20

ndash30

ndash40

ndash50

(b)

30

0

ndash30

ndash60

ndash90

Pitc

h an

gle (

degr

ee)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

0

ndash40

ndash8025 26 27 28

(c)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

20

15

10

5

0

ndash5

ndash10

ndash15

ndash20

Pitc

h an

gle e

rror

(deg

ree)

(d)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

Yaw

angl

e (de

gree

)

180

150

120

90

60

30

ndash30

ndash60

ndash90

0

8040

0ndash40

11 12 13 14 15

(e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

0

ndash50

ndash100

Yaw

angl

e err

or (d

egre

e)

(f )






GPS antenna

(a)

MTi-G DC power

Slip ring


Micro IMU

GPS signal repeater

GPS receiver

(b)


200

100

0

ndash100

ndash200

Roll

angl

e (de

gree

)

0 2 4 6 8 10 12 14 16 18 20Time (s)

AccMagReference

(a)

AccMagReference

0 2 4 6 8 10 12 14 16 18 20Time (s)

50

0

ndash50

Pitc

h an

gle (

degr

ee)

(b)


End

Start

GPS outage



0 20 40 60 80 100Time (s)


200

150

100

50

0

ndash50

ndash100

ndash150

ndash200

Roll

angl

e (de

gree

)150

100

50

0

ndash50

ndash100

ndash150

0 87654321

60 62 64 66 68

150

100

50

0

ndash50

ndash100

ndash150

(a)

0 20 40 60 80 100Time (s)


Roll

angl

e err

or (d

egre

e)

50

0

ndash50

ndash100ndash5

0

5

ndash150

(b)

UKFAUKF-RBF

ReferenceGPS-outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

4

2

0

ndash2

ndash4

Pitc

h an

gle (

degr

ee)

(c)


0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

Pitc

h an

gle e

rror

(deg

ree)

3

2

1

0

ndash1

ndash2

ndash3

(d)

UKFAUKF-RBF

ReferenceGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

ndash150

ndash155

ndash160

ndash165

ndash170

ndash175

ndash180

Yaw

angl

e (de

gree

)

GPS outage

GPS normal working

(e)


0 10 20 30 40 50 60 70 80 90 100Time (s)

Yaw

angl

e err

or (d

egre

e)

GPS outage

3

4

2

1

0

ndash1

ndash2

ndash3

ndash4

(f )

Figure 10 Continued




5 Conclusions



Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)


00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal








Data Availability




Acknowledgments


References



































3 Methodology



X q bg1113960 1113961T

7times1(10)



Cnb




⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (11)


_X7times1 f q bt( 1113857 12

A4times4 04times3

03times4 B3times3


A4times4



1113957ωbnbx 0 1113957ωb



nbz 0 1113957ωbnbx

1113957ωbnbz 1113957ωb


⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦


1 0 00 1 00 0 1

⎡⎢⎢⎢⎢⎢⎣⎤⎥⎥⎥⎥⎥⎦


(13)



1113957ωbnb 1113957ωb


Tωnin ωb


nb( 1113857

Tωnin (14)


in ωnie + ωn

en evector ωn









q0(t + h)

q1(t + h)

q2(t + h)

q3(t + h)

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

cos|Φ|

2minusΦx

|Φ|sin

|Φ|

2minusΦy

|Φ|sin

|Φ|

2

Φx

|Φ|sin

|Φ|

2cos

|Φ|

2Φz

|Φ|sin

|Φ|

2

Φy

|Φ|sin

|Φ|

2minusΦz

|Φ|sin

|Φ|

2cos

|Φ|

2

minusΦz

|Φ|sin

|Φ|

2

minusΦy

|Φ|sin

|Φ|

2

Φx

|Φ|sin

|Φ|

2

Φz

|Φ|sin

|Φ|

2Φy

|Φ|sin

|Φ|

2minusΦx

|Φ|sin

|Φ|

2cos

|Φ|

2

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

q0(t)

q1(t)

q2(t)

q3(t)

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(16)




ballistic attitudes




Data preprocessing


Initial calibration






Mti

Micro-IMU

Slip ring








_Φ 1113957ωbnb +



nb1113872 1113873 (17)






h3



(19)


θi 1113946(i3)h

(iminus13)hω tk + τ( 1113857dτ i 1 2 3 (20)


θ1 13

ah +19

bh2

+127

ch3

θ2 13

ah +39

bh2

+727

ch3

θ3 13

ah +59

bh2

+1927

ch3

(21)


Φ(h) Φ(0) + _Φ(o)h +12Φ_(0)h

2+16Φt(0)h

3+

124Φ(4)

(0)h4

+1120Φ(5)

(0)h5

ah + bh2

+ ch3

+16

(a times b)h3

+14

(a times c)h4

+110

(b times c)h5

θ1 +θ2 +θ3 +3380θ1 timesθ3

+5780θ2 times θ2 minusθ3( 1113857




ϕacc

θacc

ψmag

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

arctan2q2q3 + 2q0q1

2q20 + 2q23 minus 11113888 1113889


arctan2q1q2 + 2q0q3

2q20 + 2q21 minus 11113888 1113889

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

+ nablam (23)





Z Fn

Fnx

Fny

Fnz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ Cnbf

b+ v (24)



Cnb




⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(25)


Fn

_Vn

+ 2ωnie + ωn


minus gn (26)









1113960 1113961

⎧⎨

⎩ (27)



(n + λ)Pkminus1

1113969



(n + λ)Pkminus1

1113969


(28)


Wmi

λn + λ

i 0

λ2(n + λ)

ine 0

⎧⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎩

Wci

λn + λ


λ2(n + λ)

ine 0

(i 0 1 2 n 2n)

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎩

(29)




1113954xk|kminus1 11139442n

i0W

mi x

ik|kminus1


i0W

ci x



T

(30)



Zik|kminus1 h x

ik | kminus11113872 1113873 i 1 2 2n (31)

1113954Zk|kminus1 11139442n

i0W

mi Z

ik|kminus1 (32)

Pzzk 1113944

2n

i0W

ci Z



T+ 1113954Rk

Pxzk 1113944

2n

i0W

ci x



T

(33)

where

Kk Pxzk P

zzk( 1113857

minus 1 (34)



Tk (36)

1113954xak|kminus1 1113954xT

k|kminus 1 01113960 1113961T (37)

Pak|kminus1

Pk|kminus1 00 Qk

1113890 1113891 (38)


k|k and 1113954Pa





E εk+jεTk1113872 1113873 0 k 1 2 j 1 2 (40)



zz


Pzzk μk

1113954Pzz

k + 1113954Rk 1 minus μk( 1113857 (41)



zz


zz

k εkεTk (42)



(43)







k1113960 1113961 (45)



1113954Pzz

k1113960 1113961 (46)






1113954Pzz

k 1μk

1113944

2n

i0W

ci Z



T+ 1113954Rk

(48)

1113954Pxz

k 1μk

1113944

2n

i0W

ci x



T

(49)1113954Kk 1113954P

xz

k1113954P

xz

k1113872 1113873minus 1


(51)

1113954Pk|k 1μk


zz

k1113954K

T

k



Pzzk asymp μk

1113954Pzz

k (53)


zz


Mkμk Nk (54)


μk 1 μ0 lt 1

μ0 μ0 ge 11113896 (55)











2σ221113888 1113889 (57)



σ2 dmax2m

radic (58)


yi 1113944N

i1Wiφ x minus ci( 1113857 1113944

N

i1Wi exp minus

x minus c2i2σ22

1113888 1113889 (59)




⋮ ⋱ ⋮


⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

W1

⋮

Wm

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

Y1

⋮

Ym

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (60)


W G+D G

TG1113872 1113873

minus 1G

TD (61)









X1

X2

Xn

φ1

φ2

φn

sum

w1

w2

wn





















GPSreceiver

MEMSaccelerometer


removal


compensation


compensation


basedaccelerometer


MEMSgyroscope


Yawestimation


Magnetometer


in navigation frame

State updating

AUKF

Y

N


vector algorithm

Measurement update


Time update


tuning

State update



RBF-ANN

GPS works normally


compensation


basedmagnetometer

N+

YAttitude

data

f bf b~

mb mb~

Vn Vn

ωbib

~

ωbib

~

δXekkndash1

δXkkndash1

Zk Zk|kndash1

Xkkndash1Xkk


Micro-IMU Mti-G



100

50

0

ndash50

Roll

angl

e (de

gree

)

0 10 20 30 40 50 60Time (s)

GyroEKFAccMag

UKFCFReference

50

0

ndash5052 53 54 55

(a)

Roll

angl

e err

or (d

egre

e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

40

30

20

10

0

ndash10

ndash20

ndash30

ndash40

ndash50

(b)

30

0

ndash30

ndash60

ndash90

Pitc

h an

gle (

degr

ee)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

0

ndash40

ndash8025 26 27 28

(c)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

20

15

10

5

0

ndash5

ndash10

ndash15

ndash20

Pitc

h an

gle e

rror

(deg

ree)

(d)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

Yaw

angl

e (de

gree

)

180

150

120

90

60

30

ndash30

ndash60

ndash90

0

8040

0ndash40

11 12 13 14 15

(e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

0

ndash50

ndash100

Yaw

angl

e err

or (d

egre

e)

(f )






GPS antenna

(a)

MTi-G DC power

Slip ring


Micro IMU

GPS signal repeater

GPS receiver

(b)


200

100

0

ndash100

ndash200

Roll

angl

e (de

gree

)

0 2 4 6 8 10 12 14 16 18 20Time (s)

AccMagReference

(a)

AccMagReference

0 2 4 6 8 10 12 14 16 18 20Time (s)

50

0

ndash50

Pitc

h an

gle (

degr

ee)

(b)


End

Start

GPS outage



0 20 40 60 80 100Time (s)


200

150

100

50

0

ndash50

ndash100

ndash150

ndash200

Roll

angl

e (de

gree

)150

100

50

0

ndash50

ndash100

ndash150

0 87654321

60 62 64 66 68

150

100

50

0

ndash50

ndash100

ndash150

(a)

0 20 40 60 80 100Time (s)


Roll

angl

e err

or (d

egre

e)

50

0

ndash50

ndash100ndash5

0

5

ndash150

(b)

UKFAUKF-RBF

ReferenceGPS-outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

4

2

0

ndash2

ndash4

Pitc

h an

gle (

degr

ee)

(c)


0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

Pitc

h an

gle e

rror

(deg

ree)

3

2

1

0

ndash1

ndash2

ndash3

(d)

UKFAUKF-RBF

ReferenceGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

ndash150

ndash155

ndash160

ndash165

ndash170

ndash175

ndash180

Yaw

angl

e (de

gree

)

GPS outage

GPS normal working

(e)


0 10 20 30 40 50 60 70 80 90 100Time (s)

Yaw

angl

e err

or (d

egre

e)

GPS outage

3

4

2

1

0

ndash1

ndash2

ndash3

ndash4

(f )

Figure 10 Continued




5 Conclusions



Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)


00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal








Data Availability




Acknowledgments


References































1113957ωbnb 1113957ωb


Tωnin ωb


nb( 1113857

Tωnin (14)


in ωnie + ωn

en evector ωn









q0(t + h)

q1(t + h)

q2(t + h)

q3(t + h)

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

cos|Φ|

2minusΦx

|Φ|sin

|Φ|

2minusΦy

|Φ|sin

|Φ|

2

Φx

|Φ|sin

|Φ|

2cos

|Φ|

2Φz

|Φ|sin

|Φ|

2

Φy

|Φ|sin

|Φ|

2minusΦz

|Φ|sin

|Φ|

2cos

|Φ|

2

minusΦz

|Φ|sin

|Φ|

2

minusΦy

|Φ|sin

|Φ|

2

Φx

|Φ|sin

|Φ|

2

Φz

|Φ|sin

|Φ|

2Φy

|Φ|sin

|Φ|

2minusΦx

|Φ|sin

|Φ|

2cos

|Φ|

2

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

q0(t)

q1(t)

q2(t)

q3(t)

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(16)




ballistic attitudes




Data preprocessing


Initial calibration






Mti

Micro-IMU

Slip ring








_Φ 1113957ωbnb +



nb1113872 1113873 (17)






h3



(19)


θi 1113946(i3)h

(iminus13)hω tk + τ( 1113857dτ i 1 2 3 (20)


θ1 13

ah +19

bh2

+127

ch3

θ2 13

ah +39

bh2

+727

ch3

θ3 13

ah +59

bh2

+1927

ch3

(21)


Φ(h) Φ(0) + _Φ(o)h +12Φ_(0)h

2+16Φt(0)h

3+

124Φ(4)

(0)h4

+1120Φ(5)

(0)h5

ah + bh2

+ ch3

+16

(a times b)h3

+14

(a times c)h4

+110

(b times c)h5

θ1 +θ2 +θ3 +3380θ1 timesθ3

+5780θ2 times θ2 minusθ3( 1113857




ϕacc

θacc

ψmag

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

arctan2q2q3 + 2q0q1

2q20 + 2q23 minus 11113888 1113889


arctan2q1q2 + 2q0q3

2q20 + 2q21 minus 11113888 1113889

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

+ nablam (23)





Z Fn

Fnx

Fny

Fnz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ Cnbf

b+ v (24)



Cnb




⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(25)


Fn

_Vn

+ 2ωnie + ωn


minus gn (26)









1113960 1113961

⎧⎨

⎩ (27)



(n + λ)Pkminus1

1113969



(n + λ)Pkminus1

1113969


(28)


Wmi

λn + λ

i 0

λ2(n + λ)

ine 0

⎧⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎩

Wci

λn + λ


λ2(n + λ)

ine 0

(i 0 1 2 n 2n)

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎩

(29)




1113954xk|kminus1 11139442n

i0W

mi x

ik|kminus1


i0W

ci x



T

(30)



Zik|kminus1 h x

ik | kminus11113872 1113873 i 1 2 2n (31)

1113954Zk|kminus1 11139442n

i0W

mi Z

ik|kminus1 (32)

Pzzk 1113944

2n

i0W

ci Z



T+ 1113954Rk

Pxzk 1113944

2n

i0W

ci x



T

(33)

where

Kk Pxzk P

zzk( 1113857

minus 1 (34)



Tk (36)

1113954xak|kminus1 1113954xT

k|kminus 1 01113960 1113961T (37)

Pak|kminus1

Pk|kminus1 00 Qk

1113890 1113891 (38)


k|k and 1113954Pa





E εk+jεTk1113872 1113873 0 k 1 2 j 1 2 (40)



zz


Pzzk μk

1113954Pzz

k + 1113954Rk 1 minus μk( 1113857 (41)



zz


zz

k εkεTk (42)



(43)







k1113960 1113961 (45)



1113954Pzz

k1113960 1113961 (46)






1113954Pzz

k 1μk

1113944

2n

i0W

ci Z



T+ 1113954Rk

(48)

1113954Pxz

k 1μk

1113944

2n

i0W

ci x



T

(49)1113954Kk 1113954P

xz

k1113954P

xz

k1113872 1113873minus 1


(51)

1113954Pk|k 1μk


zz

k1113954K

T

k



Pzzk asymp μk

1113954Pzz

k (53)


zz


Mkμk Nk (54)


μk 1 μ0 lt 1

μ0 μ0 ge 11113896 (55)











2σ221113888 1113889 (57)



σ2 dmax2m

radic (58)


yi 1113944N

i1Wiφ x minus ci( 1113857 1113944

N

i1Wi exp minus

x minus c2i2σ22

1113888 1113889 (59)




⋮ ⋱ ⋮


⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

W1

⋮

Wm

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

Y1

⋮

Ym

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (60)


W G+D G

TG1113872 1113873

minus 1G

TD (61)









X1

X2

Xn

φ1

φ2

φn

sum

w1

w2

wn





















GPSreceiver

MEMSaccelerometer


removal


compensation


compensation


basedaccelerometer


MEMSgyroscope


Yawestimation


Magnetometer


in navigation frame

State updating

AUKF

Y

N


vector algorithm

Measurement update


Time update


tuning

State update



RBF-ANN

GPS works normally


compensation


basedmagnetometer

N+

YAttitude

data

f bf b~

mb mb~

Vn Vn

ωbib

~

ωbib

~

δXekkndash1

δXkkndash1

Zk Zk|kndash1

Xkkndash1Xkk


Micro-IMU Mti-G



100

50

0

ndash50

Roll

angl

e (de

gree

)

0 10 20 30 40 50 60Time (s)

GyroEKFAccMag

UKFCFReference

50

0

ndash5052 53 54 55

(a)

Roll

angl

e err

or (d

egre

e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

40

30

20

10

0

ndash10

ndash20

ndash30

ndash40

ndash50

(b)

30

0

ndash30

ndash60

ndash90

Pitc

h an

gle (

degr

ee)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

0

ndash40

ndash8025 26 27 28

(c)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

20

15

10

5

0

ndash5

ndash10

ndash15

ndash20

Pitc

h an

gle e

rror

(deg

ree)

(d)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

Yaw

angl

e (de

gree

)

180

150

120

90

60

30

ndash30

ndash60

ndash90

0

8040

0ndash40

11 12 13 14 15

(e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

0

ndash50

ndash100

Yaw

angl

e err

or (d

egre

e)

(f )






GPS antenna

(a)

MTi-G DC power

Slip ring


Micro IMU

GPS signal repeater

GPS receiver

(b)


200

100

0

ndash100

ndash200

Roll

angl

e (de

gree

)

0 2 4 6 8 10 12 14 16 18 20Time (s)

AccMagReference

(a)

AccMagReference

0 2 4 6 8 10 12 14 16 18 20Time (s)

50

0

ndash50

Pitc

h an

gle (

degr

ee)

(b)


End

Start

GPS outage



0 20 40 60 80 100Time (s)


200

150

100

50

0

ndash50

ndash100

ndash150

ndash200

Roll

angl

e (de

gree

)150

100

50

0

ndash50

ndash100

ndash150

0 87654321

60 62 64 66 68

150

100

50

0

ndash50

ndash100

ndash150

(a)

0 20 40 60 80 100Time (s)


Roll

angl

e err

or (d

egre

e)

50

0

ndash50

ndash100ndash5

0

5

ndash150

(b)

UKFAUKF-RBF

ReferenceGPS-outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

4

2

0

ndash2

ndash4

Pitc

h an

gle (

degr

ee)

(c)


0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

Pitc

h an

gle e

rror

(deg

ree)

3

2

1

0

ndash1

ndash2

ndash3

(d)

UKFAUKF-RBF

ReferenceGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

ndash150

ndash155

ndash160

ndash165

ndash170

ndash175

ndash180

Yaw

angl

e (de

gree

)

GPS outage

GPS normal working

(e)


0 10 20 30 40 50 60 70 80 90 100Time (s)

Yaw

angl

e err

or (d

egre

e)

GPS outage

3

4

2

1

0

ndash1

ndash2

ndash3

ndash4

(f )

Figure 10 Continued




5 Conclusions



Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)


00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal








Data Availability




Acknowledgments


References































_Φ 1113957ωbnb +



nb1113872 1113873 (17)






h3



(19)


θi 1113946(i3)h

(iminus13)hω tk + τ( 1113857dτ i 1 2 3 (20)


θ1 13

ah +19

bh2

+127

ch3

θ2 13

ah +39

bh2

+727

ch3

θ3 13

ah +59

bh2

+1927

ch3

(21)


Φ(h) Φ(0) + _Φ(o)h +12Φ_(0)h

2+16Φt(0)h

3+

124Φ(4)

(0)h4

+1120Φ(5)

(0)h5

ah + bh2

+ ch3

+16

(a times b)h3

+14

(a times c)h4

+110

(b times c)h5

θ1 +θ2 +θ3 +3380θ1 timesθ3

+5780θ2 times θ2 minusθ3( 1113857




ϕacc

θacc

ψmag

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

arctan2q2q3 + 2q0q1

2q20 + 2q23 minus 11113888 1113889


arctan2q1q2 + 2q0q3

2q20 + 2q21 minus 11113888 1113889

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

+ nablam (23)





Z Fn

Fnx

Fny

Fnz

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ Cnbf

b+ v (24)



Cnb




⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

(25)


Fn

_Vn

+ 2ωnie + ωn


minus gn (26)









1113960 1113961

⎧⎨

⎩ (27)



(n + λ)Pkminus1

1113969



(n + λ)Pkminus1

1113969


(28)


Wmi

λn + λ

i 0

λ2(n + λ)

ine 0

⎧⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎩

Wci

λn + λ


λ2(n + λ)

ine 0

(i 0 1 2 n 2n)

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎩

(29)




1113954xk|kminus1 11139442n

i0W

mi x

ik|kminus1


i0W

ci x



T

(30)



Zik|kminus1 h x

ik | kminus11113872 1113873 i 1 2 2n (31)

1113954Zk|kminus1 11139442n

i0W

mi Z

ik|kminus1 (32)

Pzzk 1113944

2n

i0W

ci Z



T+ 1113954Rk

Pxzk 1113944

2n

i0W

ci x



T

(33)

where

Kk Pxzk P

zzk( 1113857

minus 1 (34)



Tk (36)

1113954xak|kminus1 1113954xT

k|kminus 1 01113960 1113961T (37)

Pak|kminus1

Pk|kminus1 00 Qk

1113890 1113891 (38)


k|k and 1113954Pa





E εk+jεTk1113872 1113873 0 k 1 2 j 1 2 (40)



zz


Pzzk μk

1113954Pzz

k + 1113954Rk 1 minus μk( 1113857 (41)



zz


zz

k εkεTk (42)



(43)







k1113960 1113961 (45)



1113954Pzz

k1113960 1113961 (46)






1113954Pzz

k 1μk

1113944

2n

i0W

ci Z



T+ 1113954Rk

(48)

1113954Pxz

k 1μk

1113944

2n

i0W

ci x



T

(49)1113954Kk 1113954P

xz

k1113954P

xz

k1113872 1113873minus 1


(51)

1113954Pk|k 1μk


zz

k1113954K

T

k



Pzzk asymp μk

1113954Pzz

k (53)


zz


Mkμk Nk (54)


μk 1 μ0 lt 1

μ0 μ0 ge 11113896 (55)











2σ221113888 1113889 (57)



σ2 dmax2m

radic (58)


yi 1113944N

i1Wiφ x minus ci( 1113857 1113944

N

i1Wi exp minus

x minus c2i2σ22

1113888 1113889 (59)




⋮ ⋱ ⋮


⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

W1

⋮

Wm

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

Y1

⋮

Ym

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (60)


W G+D G

TG1113872 1113873

minus 1G

TD (61)









X1

X2

Xn

φ1

φ2

φn

sum

w1

w2

wn





















GPSreceiver

MEMSaccelerometer


removal


compensation


compensation


basedaccelerometer


MEMSgyroscope


Yawestimation


Magnetometer


in navigation frame

State updating

AUKF

Y

N


vector algorithm

Measurement update


Time update


tuning

State update



RBF-ANN

GPS works normally


compensation


basedmagnetometer

N+

YAttitude

data

f bf b~

mb mb~

Vn Vn

ωbib

~

ωbib

~

δXekkndash1

δXkkndash1

Zk Zk|kndash1

Xkkndash1Xkk


Micro-IMU Mti-G



100

50

0

ndash50

Roll

angl

e (de

gree

)

0 10 20 30 40 50 60Time (s)

GyroEKFAccMag

UKFCFReference

50

0

ndash5052 53 54 55

(a)

Roll

angl

e err

or (d

egre

e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

40

30

20

10

0

ndash10

ndash20

ndash30

ndash40

ndash50

(b)

30

0

ndash30

ndash60

ndash90

Pitc

h an

gle (

degr

ee)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

0

ndash40

ndash8025 26 27 28

(c)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

20

15

10

5

0

ndash5

ndash10

ndash15

ndash20

Pitc

h an

gle e

rror

(deg

ree)

(d)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

Yaw

angl

e (de

gree

)

180

150

120

90

60

30

ndash30

ndash60

ndash90

0

8040

0ndash40

11 12 13 14 15

(e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

0

ndash50

ndash100

Yaw

angl

e err

or (d

egre

e)

(f )






GPS antenna

(a)

MTi-G DC power

Slip ring


Micro IMU

GPS signal repeater

GPS receiver

(b)


200

100

0

ndash100

ndash200

Roll

angl

e (de

gree

)

0 2 4 6 8 10 12 14 16 18 20Time (s)

AccMagReference

(a)

AccMagReference

0 2 4 6 8 10 12 14 16 18 20Time (s)

50

0

ndash50

Pitc

h an

gle (

degr

ee)

(b)


End

Start

GPS outage



0 20 40 60 80 100Time (s)


200

150

100

50

0

ndash50

ndash100

ndash150

ndash200

Roll

angl

e (de

gree

)150

100

50

0

ndash50

ndash100

ndash150

0 87654321

60 62 64 66 68

150

100

50

0

ndash50

ndash100

ndash150

(a)

0 20 40 60 80 100Time (s)


Roll

angl

e err

or (d

egre

e)

50

0

ndash50

ndash100ndash5

0

5

ndash150

(b)

UKFAUKF-RBF

ReferenceGPS-outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

4

2

0

ndash2

ndash4

Pitc

h an

gle (

degr

ee)

(c)


0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

Pitc

h an

gle e

rror

(deg

ree)

3

2

1

0

ndash1

ndash2

ndash3

(d)

UKFAUKF-RBF

ReferenceGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

ndash150

ndash155

ndash160

ndash165

ndash170

ndash175

ndash180

Yaw

angl

e (de

gree

)

GPS outage

GPS normal working

(e)


0 10 20 30 40 50 60 70 80 90 100Time (s)

Yaw

angl

e err

or (d

egre

e)

GPS outage

3

4

2

1

0

ndash1

ndash2

ndash3

ndash4

(f )

Figure 10 Continued




5 Conclusions



Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)


00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal








Data Availability




Acknowledgments


References


































1113960 1113961

⎧⎨

⎩ (27)



(n + λ)Pkminus1

1113969



(n + λ)Pkminus1

1113969


(28)


Wmi

λn + λ

i 0

λ2(n + λ)

ine 0

⎧⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎩

Wci

λn + λ


λ2(n + λ)

ine 0

(i 0 1 2 n 2n)

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎩

(29)




1113954xk|kminus1 11139442n

i0W

mi x

ik|kminus1


i0W

ci x



T

(30)



Zik|kminus1 h x

ik | kminus11113872 1113873 i 1 2 2n (31)

1113954Zk|kminus1 11139442n

i0W

mi Z

ik|kminus1 (32)

Pzzk 1113944

2n

i0W

ci Z



T+ 1113954Rk

Pxzk 1113944

2n

i0W

ci x



T

(33)

where

Kk Pxzk P

zzk( 1113857

minus 1 (34)



Tk (36)

1113954xak|kminus1 1113954xT

k|kminus 1 01113960 1113961T (37)

Pak|kminus1

Pk|kminus1 00 Qk

1113890 1113891 (38)


k|k and 1113954Pa





E εk+jεTk1113872 1113873 0 k 1 2 j 1 2 (40)



zz


Pzzk μk

1113954Pzz

k + 1113954Rk 1 minus μk( 1113857 (41)



zz


zz

k εkεTk (42)



(43)







k1113960 1113961 (45)



1113954Pzz

k1113960 1113961 (46)






1113954Pzz

k 1μk

1113944

2n

i0W

ci Z



T+ 1113954Rk

(48)

1113954Pxz

k 1μk

1113944

2n

i0W

ci x



T

(49)1113954Kk 1113954P

xz

k1113954P

xz

k1113872 1113873minus 1


(51)

1113954Pk|k 1μk


zz

k1113954K

T

k



Pzzk asymp μk

1113954Pzz

k (53)


zz


Mkμk Nk (54)


μk 1 μ0 lt 1

μ0 μ0 ge 11113896 (55)











2σ221113888 1113889 (57)



σ2 dmax2m

radic (58)


yi 1113944N

i1Wiφ x minus ci( 1113857 1113944

N

i1Wi exp minus

x minus c2i2σ22

1113888 1113889 (59)




⋮ ⋱ ⋮


⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

W1

⋮

Wm

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

Y1

⋮

Ym

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (60)


W G+D G

TG1113872 1113873

minus 1G

TD (61)









X1

X2

Xn

φ1

φ2

φn

sum

w1

w2

wn





















GPSreceiver

MEMSaccelerometer


removal


compensation


compensation


basedaccelerometer


MEMSgyroscope


Yawestimation


Magnetometer


in navigation frame

State updating

AUKF

Y

N


vector algorithm

Measurement update


Time update


tuning

State update



RBF-ANN

GPS works normally


compensation


basedmagnetometer

N+

YAttitude

data

f bf b~

mb mb~

Vn Vn

ωbib

~

ωbib

~

δXekkndash1

δXkkndash1

Zk Zk|kndash1

Xkkndash1Xkk


Micro-IMU Mti-G



100

50

0

ndash50

Roll

angl

e (de

gree

)

0 10 20 30 40 50 60Time (s)

GyroEKFAccMag

UKFCFReference

50

0

ndash5052 53 54 55

(a)

Roll

angl

e err

or (d

egre

e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

40

30

20

10

0

ndash10

ndash20

ndash30

ndash40

ndash50

(b)

30

0

ndash30

ndash60

ndash90

Pitc

h an

gle (

degr

ee)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

0

ndash40

ndash8025 26 27 28

(c)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

20

15

10

5

0

ndash5

ndash10

ndash15

ndash20

Pitc

h an

gle e

rror

(deg

ree)

(d)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

Yaw

angl

e (de

gree

)

180

150

120

90

60

30

ndash30

ndash60

ndash90

0

8040

0ndash40

11 12 13 14 15

(e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

0

ndash50

ndash100

Yaw

angl

e err

or (d

egre

e)

(f )






GPS antenna

(a)

MTi-G DC power

Slip ring


Micro IMU

GPS signal repeater

GPS receiver

(b)


200

100

0

ndash100

ndash200

Roll

angl

e (de

gree

)

0 2 4 6 8 10 12 14 16 18 20Time (s)

AccMagReference

(a)

AccMagReference

0 2 4 6 8 10 12 14 16 18 20Time (s)

50

0

ndash50

Pitc

h an

gle (

degr

ee)

(b)


End

Start

GPS outage



0 20 40 60 80 100Time (s)


200

150

100

50

0

ndash50

ndash100

ndash150

ndash200

Roll

angl

e (de

gree

)150

100

50

0

ndash50

ndash100

ndash150

0 87654321

60 62 64 66 68

150

100

50

0

ndash50

ndash100

ndash150

(a)

0 20 40 60 80 100Time (s)


Roll

angl

e err

or (d

egre

e)

50

0

ndash50

ndash100ndash5

0

5

ndash150

(b)

UKFAUKF-RBF

ReferenceGPS-outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

4

2

0

ndash2

ndash4

Pitc

h an

gle (

degr

ee)

(c)


0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

Pitc

h an

gle e

rror

(deg

ree)

3

2

1

0

ndash1

ndash2

ndash3

(d)

UKFAUKF-RBF

ReferenceGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

ndash150

ndash155

ndash160

ndash165

ndash170

ndash175

ndash180

Yaw

angl

e (de

gree

)

GPS outage

GPS normal working

(e)


0 10 20 30 40 50 60 70 80 90 100Time (s)

Yaw

angl

e err

or (d

egre

e)

GPS outage

3

4

2

1

0

ndash1

ndash2

ndash3

ndash4

(f )

Figure 10 Continued




5 Conclusions



Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)


00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal








Data Availability




Acknowledgments


References































zz


zz

k εkεTk (42)



(43)







k1113960 1113961 (45)



1113954Pzz

k1113960 1113961 (46)






1113954Pzz

k 1μk

1113944

2n

i0W

ci Z



T+ 1113954Rk

(48)

1113954Pxz

k 1μk

1113944

2n

i0W

ci x



T

(49)1113954Kk 1113954P

xz

k1113954P

xz

k1113872 1113873minus 1


(51)

1113954Pk|k 1μk


zz

k1113954K

T

k



Pzzk asymp μk

1113954Pzz

k (53)


zz


Mkμk Nk (54)


μk 1 μ0 lt 1

μ0 μ0 ge 11113896 (55)











2σ221113888 1113889 (57)



σ2 dmax2m

radic (58)


yi 1113944N

i1Wiφ x minus ci( 1113857 1113944

N

i1Wi exp minus

x minus c2i2σ22

1113888 1113889 (59)




⋮ ⋱ ⋮


⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

W1

⋮

Wm

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

Y1

⋮

Ym

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (60)


W G+D G

TG1113872 1113873

minus 1G

TD (61)









X1

X2

Xn

φ1

φ2

φn

sum

w1

w2

wn





















GPSreceiver

MEMSaccelerometer


removal


compensation


compensation


basedaccelerometer


MEMSgyroscope


Yawestimation


Magnetometer


in navigation frame

State updating

AUKF

Y

N


vector algorithm

Measurement update


Time update


tuning

State update



RBF-ANN

GPS works normally


compensation


basedmagnetometer

N+

YAttitude

data

f bf b~

mb mb~

Vn Vn

ωbib

~

ωbib

~

δXekkndash1

δXkkndash1

Zk Zk|kndash1

Xkkndash1Xkk


Micro-IMU Mti-G



100

50

0

ndash50

Roll

angl

e (de

gree

)

0 10 20 30 40 50 60Time (s)

GyroEKFAccMag

UKFCFReference

50

0

ndash5052 53 54 55

(a)

Roll

angl

e err

or (d

egre

e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

40

30

20

10

0

ndash10

ndash20

ndash30

ndash40

ndash50

(b)

30

0

ndash30

ndash60

ndash90

Pitc

h an

gle (

degr

ee)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

0

ndash40

ndash8025 26 27 28

(c)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

20

15

10

5

0

ndash5

ndash10

ndash15

ndash20

Pitc

h an

gle e

rror

(deg

ree)

(d)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

Yaw

angl

e (de

gree

)

180

150

120

90

60

30

ndash30

ndash60

ndash90

0

8040

0ndash40

11 12 13 14 15

(e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

0

ndash50

ndash100

Yaw

angl

e err

or (d

egre

e)

(f )






GPS antenna

(a)

MTi-G DC power

Slip ring


Micro IMU

GPS signal repeater

GPS receiver

(b)


200

100

0

ndash100

ndash200

Roll

angl

e (de

gree

)

0 2 4 6 8 10 12 14 16 18 20Time (s)

AccMagReference

(a)

AccMagReference

0 2 4 6 8 10 12 14 16 18 20Time (s)

50

0

ndash50

Pitc

h an

gle (

degr

ee)

(b)


End

Start

GPS outage



0 20 40 60 80 100Time (s)


200

150

100

50

0

ndash50

ndash100

ndash150

ndash200

Roll

angl

e (de

gree

)150

100

50

0

ndash50

ndash100

ndash150

0 87654321

60 62 64 66 68

150

100

50

0

ndash50

ndash100

ndash150

(a)

0 20 40 60 80 100Time (s)


Roll

angl

e err

or (d

egre

e)

50

0

ndash50

ndash100ndash5

0

5

ndash150

(b)

UKFAUKF-RBF

ReferenceGPS-outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

4

2

0

ndash2

ndash4

Pitc

h an

gle (

degr

ee)

(c)


0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

Pitc

h an

gle e

rror

(deg

ree)

3

2

1

0

ndash1

ndash2

ndash3

(d)

UKFAUKF-RBF

ReferenceGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

ndash150

ndash155

ndash160

ndash165

ndash170

ndash175

ndash180

Yaw

angl

e (de

gree

)

GPS outage

GPS normal working

(e)


0 10 20 30 40 50 60 70 80 90 100Time (s)

Yaw

angl

e err

or (d

egre

e)

GPS outage

3

4

2

1

0

ndash1

ndash2

ndash3

ndash4

(f )

Figure 10 Continued




5 Conclusions



Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)


00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal








Data Availability




Acknowledgments


References

































2σ221113888 1113889 (57)



σ2 dmax2m

radic (58)


yi 1113944N

i1Wiφ x minus ci( 1113857 1113944

N

i1Wi exp minus

x minus c2i2σ22

1113888 1113889 (59)




⋮ ⋱ ⋮


⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

W1

⋮

Wm

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

Y1

⋮

Ym

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (60)


W G+D G

TG1113872 1113873

minus 1G

TD (61)









X1

X2

Xn

φ1

φ2

φn

sum

w1

w2

wn





















GPSreceiver

MEMSaccelerometer


removal


compensation


compensation


basedaccelerometer


MEMSgyroscope


Yawestimation


Magnetometer


in navigation frame

State updating

AUKF

Y

N


vector algorithm

Measurement update


Time update


tuning

State update



RBF-ANN

GPS works normally


compensation


basedmagnetometer

N+

YAttitude

data

f bf b~

mb mb~

Vn Vn

ωbib

~

ωbib

~

δXekkndash1

δXkkndash1

Zk Zk|kndash1

Xkkndash1Xkk


Micro-IMU Mti-G



100

50

0

ndash50

Roll

angl

e (de

gree

)

0 10 20 30 40 50 60Time (s)

GyroEKFAccMag

UKFCFReference

50

0

ndash5052 53 54 55

(a)

Roll

angl

e err

or (d

egre

e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

40

30

20

10

0

ndash10

ndash20

ndash30

ndash40

ndash50

(b)

30

0

ndash30

ndash60

ndash90

Pitc

h an

gle (

degr

ee)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

0

ndash40

ndash8025 26 27 28

(c)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

20

15

10

5

0

ndash5

ndash10

ndash15

ndash20

Pitc

h an

gle e

rror

(deg

ree)

(d)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

Yaw

angl

e (de

gree

)

180

150

120

90

60

30

ndash30

ndash60

ndash90

0

8040

0ndash40

11 12 13 14 15

(e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

0

ndash50

ndash100

Yaw

angl

e err

or (d

egre

e)

(f )






GPS antenna

(a)

MTi-G DC power

Slip ring


Micro IMU

GPS signal repeater

GPS receiver

(b)


200

100

0

ndash100

ndash200

Roll

angl

e (de

gree

)

0 2 4 6 8 10 12 14 16 18 20Time (s)

AccMagReference

(a)

AccMagReference

0 2 4 6 8 10 12 14 16 18 20Time (s)

50

0

ndash50

Pitc

h an

gle (

degr

ee)

(b)


End

Start

GPS outage



0 20 40 60 80 100Time (s)


200

150

100

50

0

ndash50

ndash100

ndash150

ndash200

Roll

angl

e (de

gree

)150

100

50

0

ndash50

ndash100

ndash150

0 87654321

60 62 64 66 68

150

100

50

0

ndash50

ndash100

ndash150

(a)

0 20 40 60 80 100Time (s)


Roll

angl

e err

or (d

egre

e)

50

0

ndash50

ndash100ndash5

0

5

ndash150

(b)

UKFAUKF-RBF

ReferenceGPS-outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

4

2

0

ndash2

ndash4

Pitc

h an

gle (

degr

ee)

(c)


0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

Pitc

h an

gle e

rror

(deg

ree)

3

2

1

0

ndash1

ndash2

ndash3

(d)

UKFAUKF-RBF

ReferenceGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

ndash150

ndash155

ndash160

ndash165

ndash170

ndash175

ndash180

Yaw

angl

e (de

gree

)

GPS outage

GPS normal working

(e)


0 10 20 30 40 50 60 70 80 90 100Time (s)

Yaw

angl

e err

or (d

egre

e)

GPS outage

3

4

2

1

0

ndash1

ndash2

ndash3

ndash4

(f )

Figure 10 Continued




5 Conclusions



Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)


00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal








Data Availability




Acknowledgments


References
















































GPSreceiver

MEMSaccelerometer


removal


compensation


compensation


basedaccelerometer


MEMSgyroscope


Yawestimation


Magnetometer


in navigation frame

State updating

AUKF

Y

N


vector algorithm

Measurement update


Time update


tuning

State update



RBF-ANN

GPS works normally


compensation


basedmagnetometer

N+

YAttitude

data

f bf b~

mb mb~

Vn Vn

ωbib

~

ωbib

~

δXekkndash1

δXkkndash1

Zk Zk|kndash1

Xkkndash1Xkk


Micro-IMU Mti-G



100

50

0

ndash50

Roll

angl

e (de

gree

)

0 10 20 30 40 50 60Time (s)

GyroEKFAccMag

UKFCFReference

50

0

ndash5052 53 54 55

(a)

Roll

angl

e err

or (d

egre

e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

40

30

20

10

0

ndash10

ndash20

ndash30

ndash40

ndash50

(b)

30

0

ndash30

ndash60

ndash90

Pitc

h an

gle (

degr

ee)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

0

ndash40

ndash8025 26 27 28

(c)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

20

15

10

5

0

ndash5

ndash10

ndash15

ndash20

Pitc

h an

gle e

rror

(deg

ree)

(d)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

Yaw

angl

e (de

gree

)

180

150

120

90

60

30

ndash30

ndash60

ndash90

0

8040

0ndash40

11 12 13 14 15

(e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

0

ndash50

ndash100

Yaw

angl

e err

or (d

egre

e)

(f )






GPS antenna

(a)

MTi-G DC power

Slip ring


Micro IMU

GPS signal repeater

GPS receiver

(b)


200

100

0

ndash100

ndash200

Roll

angl

e (de

gree

)

0 2 4 6 8 10 12 14 16 18 20Time (s)

AccMagReference

(a)

AccMagReference

0 2 4 6 8 10 12 14 16 18 20Time (s)

50

0

ndash50

Pitc

h an

gle (

degr

ee)

(b)


End

Start

GPS outage



0 20 40 60 80 100Time (s)


200

150

100

50

0

ndash50

ndash100

ndash150

ndash200

Roll

angl

e (de

gree

)150

100

50

0

ndash50

ndash100

ndash150

0 87654321

60 62 64 66 68

150

100

50

0

ndash50

ndash100

ndash150

(a)

0 20 40 60 80 100Time (s)


Roll

angl

e err

or (d

egre

e)

50

0

ndash50

ndash100ndash5

0

5

ndash150

(b)

UKFAUKF-RBF

ReferenceGPS-outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

4

2

0

ndash2

ndash4

Pitc

h an

gle (

degr

ee)

(c)


0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

Pitc

h an

gle e

rror

(deg

ree)

3

2

1

0

ndash1

ndash2

ndash3

(d)

UKFAUKF-RBF

ReferenceGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

ndash150

ndash155

ndash160

ndash165

ndash170

ndash175

ndash180

Yaw

angl

e (de

gree

)

GPS outage

GPS normal working

(e)


0 10 20 30 40 50 60 70 80 90 100Time (s)

Yaw

angl

e err

or (d

egre

e)

GPS outage

3

4

2

1

0

ndash1

ndash2

ndash3

ndash4

(f )

Figure 10 Continued




5 Conclusions



Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)


00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal








Data Availability




Acknowledgments


References





































GPSreceiver

MEMSaccelerometer


removal


compensation


compensation


basedaccelerometer


MEMSgyroscope


Yawestimation


Magnetometer


in navigation frame

State updating

AUKF

Y

N


vector algorithm

Measurement update


Time update


tuning

State update



RBF-ANN

GPS works normally


compensation


basedmagnetometer

N+

YAttitude

data

f bf b~

mb mb~

Vn Vn

ωbib

~

ωbib

~

δXekkndash1

δXkkndash1

Zk Zk|kndash1

Xkkndash1Xkk


Micro-IMU Mti-G



100

50

0

ndash50

Roll

angl

e (de

gree

)

0 10 20 30 40 50 60Time (s)

GyroEKFAccMag

UKFCFReference

50

0

ndash5052 53 54 55

(a)

Roll

angl

e err

or (d

egre

e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

40

30

20

10

0

ndash10

ndash20

ndash30

ndash40

ndash50

(b)

30

0

ndash30

ndash60

ndash90

Pitc

h an

gle (

degr

ee)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

0

ndash40

ndash8025 26 27 28

(c)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

20

15

10

5

0

ndash5

ndash10

ndash15

ndash20

Pitc

h an

gle e

rror

(deg

ree)

(d)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

Yaw

angl

e (de

gree

)

180

150

120

90

60

30

ndash30

ndash60

ndash90

0

8040

0ndash40

11 12 13 14 15

(e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

0

ndash50

ndash100

Yaw

angl

e err

or (d

egre

e)

(f )






GPS antenna

(a)

MTi-G DC power

Slip ring


Micro IMU

GPS signal repeater

GPS receiver

(b)


200

100

0

ndash100

ndash200

Roll

angl

e (de

gree

)

0 2 4 6 8 10 12 14 16 18 20Time (s)

AccMagReference

(a)

AccMagReference

0 2 4 6 8 10 12 14 16 18 20Time (s)

50

0

ndash50

Pitc

h an

gle (

degr

ee)

(b)


End

Start

GPS outage



0 20 40 60 80 100Time (s)


200

150

100

50

0

ndash50

ndash100

ndash150

ndash200

Roll

angl

e (de

gree

)150

100

50

0

ndash50

ndash100

ndash150

0 87654321

60 62 64 66 68

150

100

50

0

ndash50

ndash100

ndash150

(a)

0 20 40 60 80 100Time (s)


Roll

angl

e err

or (d

egre

e)

50

0

ndash50

ndash100ndash5

0

5

ndash150

(b)

UKFAUKF-RBF

ReferenceGPS-outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

4

2

0

ndash2

ndash4

Pitc

h an

gle (

degr

ee)

(c)


0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

Pitc

h an

gle e

rror

(deg

ree)

3

2

1

0

ndash1

ndash2

ndash3

(d)

UKFAUKF-RBF

ReferenceGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

ndash150

ndash155

ndash160

ndash165

ndash170

ndash175

ndash180

Yaw

angl

e (de

gree

)

GPS outage

GPS normal working

(e)


0 10 20 30 40 50 60 70 80 90 100Time (s)

Yaw

angl

e err

or (d

egre

e)

GPS outage

3

4

2

1

0

ndash1

ndash2

ndash3

ndash4

(f )

Figure 10 Continued




5 Conclusions



Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)


00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal








Data Availability




Acknowledgments


References






























100

50

0

ndash50

Roll

angl

e (de

gree

)

0 10 20 30 40 50 60Time (s)

GyroEKFAccMag

UKFCFReference

50

0

ndash5052 53 54 55

(a)

Roll

angl

e err

or (d

egre

e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

40

30

20

10

0

ndash10

ndash20

ndash30

ndash40

ndash50

(b)

30

0

ndash30

ndash60

ndash90

Pitc

h an

gle (

degr

ee)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

0

ndash40

ndash8025 26 27 28

(c)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

20

15

10

5

0

ndash5

ndash10

ndash15

ndash20

Pitc

h an

gle e

rror

(deg

ree)

(d)

GyroEKFAccMag

UKFCFReference

0 10 20 30 40 50 60Time (s)

Yaw

angl

e (de

gree

)

180

150

120

90

60

30

ndash30

ndash60

ndash90

0

8040

0ndash40

11 12 13 14 15

(e)

GyroEKFAccMag

UKFCF

0 10 20 30 40 50 60Time (s)

50

0

ndash50

ndash100

Yaw

angl

e err

or (d

egre

e)

(f )






GPS antenna

(a)

MTi-G DC power

Slip ring


Micro IMU

GPS signal repeater

GPS receiver

(b)


200

100

0

ndash100

ndash200

Roll

angl

e (de

gree

)

0 2 4 6 8 10 12 14 16 18 20Time (s)

AccMagReference

(a)

AccMagReference

0 2 4 6 8 10 12 14 16 18 20Time (s)

50

0

ndash50

Pitc

h an

gle (

degr

ee)

(b)


End

Start

GPS outage



0 20 40 60 80 100Time (s)


200

150

100

50

0

ndash50

ndash100

ndash150

ndash200

Roll

angl

e (de

gree

)150

100

50

0

ndash50

ndash100

ndash150

0 87654321

60 62 64 66 68

150

100

50

0

ndash50

ndash100

ndash150

(a)

0 20 40 60 80 100Time (s)


Roll

angl

e err

or (d

egre

e)

50

0

ndash50

ndash100ndash5

0

5

ndash150

(b)

UKFAUKF-RBF

ReferenceGPS-outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

4

2

0

ndash2

ndash4

Pitc

h an

gle (

degr

ee)

(c)


0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

Pitc

h an

gle e

rror

(deg

ree)

3

2

1

0

ndash1

ndash2

ndash3

(d)

UKFAUKF-RBF

ReferenceGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

ndash150

ndash155

ndash160

ndash165

ndash170

ndash175

ndash180

Yaw

angl

e (de

gree

)

GPS outage

GPS normal working

(e)


0 10 20 30 40 50 60 70 80 90 100Time (s)

Yaw

angl

e err

or (d

egre

e)

GPS outage

3

4

2

1

0

ndash1

ndash2

ndash3

ndash4

(f )

Figure 10 Continued




5 Conclusions



Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)


00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal








Data Availability




Acknowledgments


References

































GPS antenna

(a)

MTi-G DC power

Slip ring


Micro IMU

GPS signal repeater

GPS receiver

(b)


200

100

0

ndash100

ndash200

Roll

angl

e (de

gree

)

0 2 4 6 8 10 12 14 16 18 20Time (s)

AccMagReference

(a)

AccMagReference

0 2 4 6 8 10 12 14 16 18 20Time (s)

50

0

ndash50

Pitc

h an

gle (

degr

ee)

(b)


End

Start

GPS outage



0 20 40 60 80 100Time (s)


200

150

100

50

0

ndash50

ndash100

ndash150

ndash200

Roll

angl

e (de

gree

)150

100

50

0

ndash50

ndash100

ndash150

0 87654321

60 62 64 66 68

150

100

50

0

ndash50

ndash100

ndash150

(a)

0 20 40 60 80 100Time (s)


Roll

angl

e err

or (d

egre

e)

50

0

ndash50

ndash100ndash5

0

5

ndash150

(b)

UKFAUKF-RBF

ReferenceGPS-outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

4

2

0

ndash2

ndash4

Pitc

h an

gle (

degr

ee)

(c)


0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

Pitc

h an

gle e

rror

(deg

ree)

3

2

1

0

ndash1

ndash2

ndash3

(d)

UKFAUKF-RBF

ReferenceGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

ndash150

ndash155

ndash160

ndash165

ndash170

ndash175

ndash180

Yaw

angl

e (de

gree

)

GPS outage

GPS normal working

(e)


0 10 20 30 40 50 60 70 80 90 100Time (s)

Yaw

angl

e err

or (d

egre

e)

GPS outage

3

4

2

1

0

ndash1

ndash2

ndash3

ndash4

(f )

Figure 10 Continued




5 Conclusions



Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)


00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal








Data Availability




Acknowledgments


References






























0 20 40 60 80 100Time (s)


200

150

100

50

0

ndash50

ndash100

ndash150

ndash200

Roll

angl

e (de

gree

)150

100

50

0

ndash50

ndash100

ndash150

0 87654321

60 62 64 66 68

150

100

50

0

ndash50

ndash100

ndash150

(a)

0 20 40 60 80 100Time (s)


Roll

angl

e err

or (d

egre

e)

50

0

ndash50

ndash100ndash5

0

5

ndash150

(b)

UKFAUKF-RBF

ReferenceGPS-outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

4

2

0

ndash2

ndash4

Pitc

h an

gle (

degr

ee)

(c)


0 10 20 30 40 50 60 70 80 90 100Time (s)

GPS outage

Pitc

h an

gle e

rror

(deg

ree)

3

2

1

0

ndash1

ndash2

ndash3

(d)

UKFAUKF-RBF

ReferenceGPS outage

0 10 20 30 40 50 60 70 80 90 100Time (s)

ndash150

ndash155

ndash160

ndash165

ndash170

ndash175

ndash180

Yaw

angl

e (de

gree

)

GPS outage

GPS normal working

(e)


0 10 20 30 40 50 60 70 80 90 100Time (s)

Yaw

angl

e err

or (d

egre

e)

GPS outage

3

4

2

1

0

ndash1

ndash2

ndash3

ndash4

(f )

Figure 10 Continued




5 Conclusions



Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)


00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal








Data Availability




Acknowledgments


References
































5 Conclusions



Time (s)10 12 14 16 18 20

ndash156

ndash158

ndash160

ndash162

Yaw

angl

e (de

gree

)

(g)

UKFAUKF-RBF

ReferenceGPS outage

Time (s)

Yaw

angl

e (de

gree

)

40 42 44 46 48 50

ndash158

ndash160

ndash162

ndash164

(h)


00

100

200

300

400

2 4 6 8 10 12 14Time (s)

RMS

UKFAUKFGoal








Data Availability




Acknowledgments


References































Data Availability




Acknowledgments


References


































Documents

AccurateAttitudeDeterminationBasedonAdaptiveUKFandRBF ...downloads.hindawi.com/journals/mpe/2020/1638678.pdfMEMS Inertial Measurement Unit (MIMU) and magnetic sensor have wide application