# Accurate Attitude Determination Based on Adaptive UKF and RBF Neural Network Using Fusion Methodology for Micro-IMU Applied to Rotating Environment

Authors: Lei Wang; Zhi Min Meng; Ying Guan
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

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


---

## Body

## 1. Introduction

MEMS Inertial Measurement Unit (MIMU) and magnetic sensor have wide application for attitude determination in civilian and military fields, such as Unmanned Aerial Vehicle (UAV) systems [1, 2], land vehicle systems [3, 4], projectile, spinning shell [5, 6], underwater system [7, 8], and others [9, 10]. In these specific applications, the work of different attitude determination algorithms is classified according to the following categories. Among them, representative research studies include attitude determination utilizing the different MEMS sensors [11–14], different Kalman filter methods for attitude estimation [15, 16], sensor fusion algorithms for attitude determination [17, 18], and intelligent control method strategy combined with filters [19, 20].On one side, different MEMS inertial sensors are affordable for attitude calculation. The gyroscope is the principal sensor available for attitude measurement using quaternion algorithm, which quickly degrades over time [11]. Assuming that the object does not move or moves at a constant speed, the roll angle and pitch angle can be estimated by the strapdown triaxial of the accelerometer [14]. Magnetometers are widely used for estimating the yaw angle in AHRS [15] or estimating the ballistic roll in the projectile [16]. The attitude determination approaches utilizing different sensors separately are difficult to provide reliable estimation due to sensor errors and dynamic environmental variations.In addition, considerable work has been carried out on attitude estimation using adaptive filters and fusion algorithms. The Extended Kalman Filter (EKF) [4] and Adaptive EKF (AEKF) [7, 14] are developed for nonlinear system state estimation in navigation system. Unscented Kalman Filter (UKF) [5, 9] and Adaptive UKF (AUKF) [15, 16] are widely used for sensor fusion algorithms. The methods mentioned above can achieve accuracy and adaptability for attitude estimation. For low-cost MEMS IMU sensors, Chiella and Teixeira [15] proposed a quaternion-based robust adaptive unscented Kalman Filter for attitude estimation. In the filter, the adaptive strategy based on covariance matching can adjust the measurement covariance matrix online to deal with the slow time-varying disturbance in the sensors. Xu et al. [16] proposed a multiple-model unscented Kalman filter for attitude estimation, which is a quaternion-based attitude estimator that fuses related strap-down Magnetic, Angular Rate, and Gravity (MARG) sensor arrays.In order to overcome the drawbacks of the abovementioned methods and take full advantage of fusion and adaptive filtering scheme, some fusion methodologies combining complementary filtering or Kalman filtering are proposed. Koksal et al. [17] combines Kalman filter and complementary filter to provide accurate attitude estimation and control performance for quadrotor UAV. Noordin et al. [18] proposed a sensor fusion algorithm for UAV attitude estimation based on nonlinear complementary filter (NCF). To obtain reliable attitude estimation, gyroscope is fused to the accelerometer and magnetometer to correct drift error of the gyroscope. Other representative research studies are the combination of filtering and intelligent control scheme. Ning et al. [19] propose a robust Kalman filter combined with RBF-ANN algorithm to isolate and reduce the influence of the dynamic model error and GNSS observation gross error. Hossein and Jafer [20] adjusts the fuzzy weighting coefficients to correct the orientation estimation by usage of SINS/GPS and AHRS data.The above works on attitude determination algorithm lie in the following problems. The traditional EKF and other improved EKF have the drawbacks such as low estimation accuracy and poor stability, which will cause the biased estimation or even divergence due to which the statistical properties of system and measurement noise cannot be predicted accurately, especially for low-cost MEMS-based sensors [21]. Some complicated nonlinear calculation will cause a heavy burden in real-time micronavigation system, which will weaken the feasibility for application [2, 22]. Therefore, the advanced filtering or adaptive filtering methods will play a key role in these applications [6, 13, 17, 23].This paper concentrates on developing the customized attitude determination algorithm of low-cost MEMS sensor in high dynamic environment. The corresponding technique can be widely applied in measuring the attitude of the projectile and spinning shell of low dynamic rotating. Combined with the proposed adaptive Kalman filtering method and intelligent control scheme, it is suitable for dealing with time-varying noise of low-cost, small-size micro-IMU, and dynamic interference. The novel aspects of this paper can be summarized as follows:(1)
According to dynamic rotating environment, a new attitude estimation scheme of AUKF is designed. Meanwhile, different control strategies are also adopted to fuse MEMS multi-inertial sensors under different dynamic conditions. AUKF attitude determination method based on MEMS sensor and GPS can provide reliable estimation under high dynamic environment change. Using the adaptive scale factor, the influence on the new measurement data is weakened or enhanced during the process of the estimation according to the predicted residual vector.(2)
The proposed method can accommodate sensor failure, particularly GPS failure in adverse environment. When GPS signals are temporarily blocked, the attitude algorithm based on RBF-ANN feedback correction can continue to provide reliable estimated deviation information. The corrected predicted system state will be used in the estimation process in AUKF.(3)
Considering the space limitations in spinning projectile, the small size MEMS IMU is designed. The customized prototype of the DSP controller is developed for it. The corresponding programs of the proposed algorithm can be compiled and run on in real time.The remainder of this paper is organized as follows. Section2 presents MEMS IMU configuration and design, the model of MEMS sensor, and related calibration work. The AUKF algorithm and RBF feedback correction are developed in detail in Section 3. Then, experimental results and discussion are stated in Section 4.


## 2. Modeling and Related Work

### 2.1. Sensor Configuration and Design

According to application background of the spinning projectile, a small size, low power consuming, and low-cost MEMS IMU is developed. Microinertial navigation system consists of a DSP microcontroller, A/D converter, flash, power module, micro-IMU, and magnetometer. The micro-IMU is composed of three single-axis gyroscopes and accelerometers. It is shown in Figure1.Figure 1
The prototype of MEME IMU and micronavigation system.
(a)(b)(c)In Figure1, a six layer MEMS IMU PCB board is designed with size of 45 × 45 mm. The output analog signals of the accelerometer and magnetometer are sampled simultaneously by 8-channel 16 bit analog-to-digital converter. The simple and effective communication between DSP microcontroller and gyroscope is realized through SPI interface. Angular rate data is output in 12-bit 2’s complement format at a maximum rate of 2000 samples per second. Considering the real-time performance, the proposed algorithm is migrated from MATLAB to C code. These codes can be compiled in CCS3.3 environment and loaded into DSP 28335 microcontroller for real-time operation.


### 2.2. Sensor Model

Low-cost, small size MEMS inertial sensors are widely used in these applications due to the dimension constraint, cost consideration in projectile, and spinning shell [5, 6]. Gyroscopes and accelerometers are commonly known as inertial sensors and their orthogonal triads generally form an IMU used as a core means of a navigation system. The precision of gyro is constraint to time varying bias drift and noise. The true output of the gyro can be defined as follows:(1)ωibb=Sg−1ωibb˜−bg,where ωibb is the vector of true angular rate, ωibb˜ is the vector of measured angular rate, and bg corresponds to the vector of output bias. Used indices are e-Earth frame, n-Navigation frame, i-inertial frame, and b-body frame. The upper index defines the referenced coordinate system, whereas the lower index refers to the described value. Sg denotes the matrix of scale factors and misalignments:(2)Sg=kgxσxyσxzσyxkgyσyzσzxσzykgz.The main diagonal elementskg are scale factors that accounts for the sensitivities of the individual sensors by scaling the outputs. The variable σ represents the nonorthogonality and misalignment of the triaxial gyroscope sensors. The biases, scale factors, nonorthogonality, or misalignment errors are main systematic errors or deterministic sources for MEMS inertial sensors.An accelerometer sensor is a dynamic MEMS sensor used to measure acceleration forces up to three orthogonal axes. The accelerometer in the body frame is defined as(3)fb=MaSafb˜−ba,where fb is the vector of true accelerometer, fb˜ is the vector of measured acceleration, ba corresponds to the vector of offsets, Sa represents the matrix of scale factors, and Ma transforms a vector from the nonorthogonal coordinate system to the orthogonal one:(4)Ma=100ζxy10ζzxζzy1,Sa=kax000kay000kaz.The main diagonal elementska are scale factors, the variable ζ represents the nonorthogonality and misalignment. The three angles, namely, pitch, roll, and yaw, denoted by the symbols θ,ϕ, and ψ. Correspondingly, the pitch and roll angle could be computed as(5)θacc=−arctanfxbg,ϕacc=arctanfybg·cosθ,where fxb and fyb are the component variables of fb in body coordinate.The magnetic field components on local geography coordinate (west magnetic field, south magnetic field, and vertical magnetic field) can be measured by the geomagnetic vector measurement system, which mainly contains a three-axis magnetometer. The performance of the magnetometer sensor is distorted by hard iron and soft iron effects. Intrinsic and cross sensor calibration of the three-axis magnetometer is indispensable to eliminate scale factor, cross coupling, and bias errors. The output of the magnetometer is represented as(6)mb=Ce−1mb˜−ε,where mb is the calibrated output of magnetometer, the matrix mb˜ represents measured output in body frame, Ce represents effect of sensor imperfection and magnetic disturbance, and the vector ε represents the noise of magnetic disturbance. Define mn is the local magnetic vector in n-frame. The relationship of magnetic vectors in b-frame and n-frame is represented by(7)mb=Cnbmn.Definemp is the horizontal plane projection of the magnetic field; the relationship between mp and magnetic vectors in b-frame mb is(8)mxpmypmzp=cosθsinϕsinθcosϕcosθ0cosϕ−sinϕ−sinθsinϕcosθcosϕcosθ⋅mxbmybmzb.Correspondingly, yaw angleψm is calculated as(9)ψm=arctanmypmxp.


### 2.3. Sensor Calibration Method

Before MEMS IMU was used for attitude estimation in the actual application, there were measurement errors and installation errors, which will affect the measurement output. Therefore, the calibration method is one effective way to compensate for the deterministic and other errors of the MEMS inertial sensor. Considering the application in high dynamic environment, the coupling effect between sensitive axes will significantly affect the measurement accuracy of micro-IMU. According to the spinning characteristics in the projectile application that the rotation of one sensitive axis is highly dynamic and the rotation of the other sensitive axis is low dynamic, the coupling coefficient of the model is estimated by the compounded calibration method [23]. The high dynamic rolling three-axis turntable is used to simulate different ballistic attitude angle variation of projectile in flight state. Experiments are performed, respectively. Figure 2 shows the compounded calibration scheme for micro-IMU.Figure 2
Compounded calibration experiments of micro-IMU.The calibration process is composed of the initial calibration and compounded calibration. The optimal bias of gyro is estimated and provided to AUKF as the initial calibration process. The biasbg, scale factor kgii=x,y,z and the misalignment σjk were estimated in the initial calibration process using the least square algorithm. The dominant deterministic error result is shown in Table 1.Table 1
Deterministic errors for MEMS gyro.
AxisGyro deterministic error in indoor temperatureBiases (°/s, g, Gauss)Scale factorGyro x−2.05110.9976Gyro y−2.58481.0033Gyro z−0.00431.0021The estimated adjustment valueΔωb in compounded process is used for compensating the coupling effect of different rotating axis in high dynamic environment. The compounded calibration eliminates the coupling effect between the high dynamic rotation axis and the low dynamic rotation axis. The compensation accuracy of high dynamic rotation is improved. The preprocessing data after calibration and compensation can be used for attitude estimation, especially in high dynamic environment. The sensor characteristics of stochastic errors in low MEMS gyro, accelerometer, and magnetometer could be identified using Allan variance [24]. The root mean square random drift error of Allan variance is calculated and provided to AUKF as the initial measurement noise.


## 2.1. Sensor Configuration and Design

According to application background of the spinning projectile, a small size, low power consuming, and low-cost MEMS IMU is developed. Microinertial navigation system consists of a DSP microcontroller, A/D converter, flash, power module, micro-IMU, and magnetometer. The micro-IMU is composed of three single-axis gyroscopes and accelerometers. It is shown in Figure1.Figure 1
The prototype of MEME IMU and micronavigation system.
(a)(b)(c)In Figure1, a six layer MEMS IMU PCB board is designed with size of 45 × 45 mm. The output analog signals of the accelerometer and magnetometer are sampled simultaneously by 8-channel 16 bit analog-to-digital converter. The simple and effective communication between DSP microcontroller and gyroscope is realized through SPI interface. Angular rate data is output in 12-bit 2’s complement format at a maximum rate of 2000 samples per second. Considering the real-time performance, the proposed algorithm is migrated from MATLAB to C code. These codes can be compiled in CCS3.3 environment and loaded into DSP 28335 microcontroller for real-time operation.


## 2.2. Sensor Model

Low-cost, small size MEMS inertial sensors are widely used in these applications due to the dimension constraint, cost consideration in projectile, and spinning shell [5, 6]. Gyroscopes and accelerometers are commonly known as inertial sensors and their orthogonal triads generally form an IMU used as a core means of a navigation system. The precision of gyro is constraint to time varying bias drift and noise. The true output of the gyro can be defined as follows:(1)ωibb=Sg−1ωibb˜−bg,where ωibb is the vector of true angular rate, ωibb˜ is the vector of measured angular rate, and bg corresponds to the vector of output bias. Used indices are e-Earth frame, n-Navigation frame, i-inertial frame, and b-body frame. The upper index defines the referenced coordinate system, whereas the lower index refers to the described value. Sg denotes the matrix of scale factors and misalignments:(2)Sg=kgxσxyσxzσyxkgyσyzσzxσzykgz.The main diagonal elementskg are scale factors that accounts for the sensitivities of the individual sensors by scaling the outputs. The variable σ represents the nonorthogonality and misalignment of the triaxial gyroscope sensors. The biases, scale factors, nonorthogonality, or misalignment errors are main systematic errors or deterministic sources for MEMS inertial sensors.An accelerometer sensor is a dynamic MEMS sensor used to measure acceleration forces up to three orthogonal axes. The accelerometer in the body frame is defined as(3)fb=MaSafb˜−ba,where fb is the vector of true accelerometer, fb˜ is the vector of measured acceleration, ba corresponds to the vector of offsets, Sa represents the matrix of scale factors, and Ma transforms a vector from the nonorthogonal coordinate system to the orthogonal one:(4)Ma=100ζxy10ζzxζzy1,Sa=kax000kay000kaz.The main diagonal elementska are scale factors, the variable ζ represents the nonorthogonality and misalignment. The three angles, namely, pitch, roll, and yaw, denoted by the symbols θ,ϕ, and ψ. Correspondingly, the pitch and roll angle could be computed as(5)θacc=−arctanfxbg,ϕacc=arctanfybg·cosθ,where fxb and fyb are the component variables of fb in body coordinate.The magnetic field components on local geography coordinate (west magnetic field, south magnetic field, and vertical magnetic field) can be measured by the geomagnetic vector measurement system, which mainly contains a three-axis magnetometer. The performance of the magnetometer sensor is distorted by hard iron and soft iron effects. Intrinsic and cross sensor calibration of the three-axis magnetometer is indispensable to eliminate scale factor, cross coupling, and bias errors. The output of the magnetometer is represented as(6)mb=Ce−1mb˜−ε,where mb is the calibrated output of magnetometer, the matrix mb˜ represents measured output in body frame, Ce represents effect of sensor imperfection and magnetic disturbance, and the vector ε represents the noise of magnetic disturbance. Define mn is the local magnetic vector in n-frame. The relationship of magnetic vectors in b-frame and n-frame is represented by(7)mb=Cnbmn.Definemp is the horizontal plane projection of the magnetic field; the relationship between mp and magnetic vectors in b-frame mb is(8)mxpmypmzp=cosθsinϕsinθcosϕcosθ0cosϕ−sinϕ−sinθsinϕcosθcosϕcosθ⋅mxbmybmzb.Correspondingly, yaw angleψm is calculated as(9)ψm=arctanmypmxp.


## 2.3. Sensor Calibration Method

Before MEMS IMU was used for attitude estimation in the actual application, there were measurement errors and installation errors, which will affect the measurement output. Therefore, the calibration method is one effective way to compensate for the deterministic and other errors of the MEMS inertial sensor. Considering the application in high dynamic environment, the coupling effect between sensitive axes will significantly affect the measurement accuracy of micro-IMU. According to the spinning characteristics in the projectile application that the rotation of one sensitive axis is highly dynamic and the rotation of the other sensitive axis is low dynamic, the coupling coefficient of the model is estimated by the compounded calibration method [23]. The high dynamic rolling three-axis turntable is used to simulate different ballistic attitude angle variation of projectile in flight state. Experiments are performed, respectively. Figure 2 shows the compounded calibration scheme for micro-IMU.Figure 2
Compounded calibration experiments of micro-IMU.The calibration process is composed of the initial calibration and compounded calibration. The optimal bias of gyro is estimated and provided to AUKF as the initial calibration process. The biasbg, scale factor kgii=x,y,z and the misalignment σjk were estimated in the initial calibration process using the least square algorithm. The dominant deterministic error result is shown in Table 1.Table 1
Deterministic errors for MEMS gyro.
AxisGyro deterministic error in indoor temperatureBiases (°/s, g, Gauss)Scale factorGyro x−2.05110.9976Gyro y−2.58481.0033Gyro z−0.00431.0021The estimated adjustment valueΔωb in compounded process is used for compensating the coupling effect of different rotating axis in high dynamic environment. The compounded calibration eliminates the coupling effect between the high dynamic rotation axis and the low dynamic rotation axis. The compensation accuracy of high dynamic rotation is improved. The preprocessing data after calibration and compensation can be used for attitude estimation, especially in high dynamic environment. The sensor characteristics of stochastic errors in low MEMS gyro, accelerometer, and magnetometer could be identified using Allan variance [24]. The root mean square random drift error of Allan variance is calculated and provided to AUKF as the initial measurement noise.


## 3. Methodology

### 3.1. System State and Observation Model

According to the output characteristic of MEMS sensor, the filter algorithm can effectively improve the accuracy of sensor measurement output. Considering the nonlinear characteristic of the system, the adaptive UKF fusion algorithm is preferred to solve the attitude estimation in the case of dynamic environment changes.Establishing the perfect state equation and measurement equation is the key to improving the solution. Considering the effect of the model error, the quaternion vectorq and the output error of gyroscope bt are chosen as the state vector. The operator “⊗” denotes the quaternion multiplication:(10)X=qbg7×1T.Here,q is the quaternion vector. q=q0q1q2q3. The bias of gyroscope bg=bgxbgybgz, which is the bias drift in three sensitive axes x, y, and z. The coordinate transformation from the navigation frame to the body fixed frame is based on the three angles θ,ϕ, and ψ. The attitude matrix Cbn from the navigation frame and body frame by three Euler angles is given by(11)Cbn=cosθcosψsinϕsinθcosψ−cosϕsinψcosϕsinθcosψ+sinϕsinψcosθsinψsinϕsinθsinψ+cosϕcosψcosϕsinθsinψ−sinϕcosψ−sinθsinϕcosθcosϕcosθ.The single spline algorithm based on the quaternion vector in equation (12) is suitable for attitude angle estimation in a low dynamic environment:(12)X˙7×1=fq,bt=12A4×404×303×4B3×3⋅X7×1+W7×1,(13)A4×4=0−ω˜nbxb−ω˜nbyb−ω˜nbzbω˜nbxb0ω˜nbzb−ω˜nbybω˜nbyb−ω˜nbzb0ω˜nbxbω˜nbzbω˜nbyb−ω˜nbxb0,B3×3=−1τ∗100010001,W7×1=01×4ωrxωryωrzT,where τ is the time constant of a first order Gauss–Markov process and ωr is Gaussian white process. The body angler rate with respect to the navigation frame ω˜nb is given by the difference between the body angular rate, ω˜ib, and the navigation frame rotation, ωin:(14)ω˜nbb=ω˜ibb−CbnTωinn=ωibb+δωibn−CbnTωinn,where the vector ωinn can be written as ωinn=ωien+ωenn. The vector ωien is the Earth rate in the navigation frame. The vector ωenn is the turn rate of the navigation frame with respect to the velocity of the craft. The output error δωibb of gyro is replaced by the constant deviation b0, and the whole range of the constant deviation b0 is estimated as the definite error through the off-line calibration experiment.In a high dynamic environment, the cubic spline algorithm based on the quaternion vector is used for the estimation of attitude angle:(15)qt+h=qt⊗rh.rh=cosΦ/2+Φ/ΦsinΦ/2, Φ is the equivalent rotating vector in sampling period from t to t + h. h is the time step in sampling time. The corresponding angular rate of gyro in sampling period ωnbb is expressed using the straight line fitting method. The expression of matrix in equation (15) could be rewritten as(16)q0t+hq1t+hq2t+hq3t+h=cosΦ2−ΦxΦsinΦ2−ΦyΦsinΦ2ΦxΦsinΦ2cosΦ2ΦzΦsinΦ2ΦyΦsinΦ2−ΦzΦsinΦ2cosΦ2−ΦzΦsinΦ2−ΦyΦsinΦ2ΦxΦsinΦ2ΦzΦsinΦ2ΦyΦsinΦ2−ΦxΦsinΦ2cosΦ2q0tq1tq2tq3t.The differential equation for the rotating vector can be obtained by neglecting the high order items:(17)Φ˙=ω˜nbb+12Φ×ω˜nbb+112Φ×Φ×ω˜nbb.The polynomial fitting equation of the angular rate is expressed as follows:(18)ωnbb=a+2bτ+3cτ2,0≤τ≤h,where a, b, and c are fitting parameters for quadratic function polynomial. Defining Φtk and Φtk−1 are the attitude quaternions of tk and tk−1 moment, respectively, the Taylor series expansion equation of the attitude quaternion is expressed in(19)Φtk=Φtk−1+hΦ˙tk−1+h22!Φ¨tk−1+h33!Φ⋯tk−1+⋯.The attitude algorithm with incremental angle could be constructed as follows:(20)△θi=∫i−1/3hi/3hωtk+τdτ,i=1,2,3,…,where △θ1, △θ2, and △θ3 are the angular increments of tk,tk+1/3h, tk+1/3h,tk+2/3h, and tk+2/3h,tk+1:(21)△θ1=13ah+19bh2+127ch3,△θ2=13ah+39bh2+727ch3,△θ3=13ah+59bh2+1927ch3.The equivalent rotating vector is expressed according to Taylor series expansion equation:(22)Φh=Φ0+Φ˙oh+12Φ¨0h2+16Φ⃛0h3+124Φ40h4+1120Φ50h5=ah+bh2+ch3+16a×bh3+14a×ch4+110b×ch5=△θ1+△θ2+△θ3+3380△θ1×△θ3+5780△θ2×θ2−△θ3.As can be seen from the Section2.1, the accelerometer can be used for calculating the pitch and roll angles by measuring the gravity vectors if the vehicle is stationary or in low dynamic environment. Triaxis magnetometers are used for calculating the yaw angle. Its accuracy is heavily affected by local magnetic environments. According to the attitude algorithm of the accelerometer and magnetometer, the attitude angle ϕacc, θacc, and ψmag can be treated as the measurement of the attitude system in the low dynamic environment:(23)ϕaccθaccψmag=arctan2q2q3+2q0q12q02+2q32−1−arcsin2q1q3−2q0q2arctan2q1q2+2q0q32q02+2q12−1+∇m.∇m=∇ϕ∇θ∇ψT, where ∇ϕ,∇θ, and ∇ψ are the measurement noise of roll, pitch, and yaw angle. E∇m=0,E∇mk,∇mkT=Rδkj, and R=diagσϕ2σθ2σψ2, where σϕ2, σθ2, and σψ2 are standard deviation of attitude errors. Their values depend on the precision of the single MEMS sensor.In the high dynamic environment, the aforementioned attitude estimation approaches utilizing only the single sensor are difficult to provide reliable estimation due to sensor errors. The dynamic accuracy of the attitude angle that are calculated in virtue of the accelerometer and magnetometer is affected largely due to the motion of body in the high dynamic environment. In such a case, the measurement vectorsϕacc, θacc, and ψmag are unreliable due to the effects of sensor errors. The specific force referenced in n-frame can be treated as the measurement of low-cost micro-IMU attitude system. Therefore, the measurement equation can be replaced as(24)Z=Fn=FxnFynFzn=Cbnfb+v,where the measurement vector Fn is the specific force referenced in n-frame and v is the measurement noise. The attitude error matrix Cbn is expressed in terms of quaternion in equation (11):(25)Cbn=1−2q22+q322q1q2−q0q32q1q3+q0q22q1q2+q0q31−2q12+q322q2q3−q0q12q1q3−q0q22q2q3+q0q11−2q12+q22.The measurement vectorFn is calculated as(26)Fn=V˙n+2ωien+ωenn×Vn−gn.Here, the vectorV˙n is given by calculating the derivative of velocity afforded by GPS.


### 3.2. Adaptive UKF Fusion Algorithm

Due to the fact that the statistical characteristics of the noise are real-time changed, it is essential to adjust noise estimation in the filter algorithm. Based on the nature of UKF algorithm, unscented transformation (UT) is an approximate method to change the mean and covariance of random variables when they undergo nonlinear transformation [21, 25]. The adaptive performance of AUKF is the filter process of the measurement noise variance of the real-time estimation. The basic steps of the AUKF algorithm are as follows:(i)
Step 1: generation of sigma points and weights.The estimated state and corresponding covariance of the system can be expressed as follows:(27)x^k−1=Exk−1,P^k−1=Ex^k−1−xk−1x^k−1−xk−1T.Sigmal pointsxi,k−1i=1,2,…,n,k≥1 are generated according to the following equations:(28)xi,k−1=x^k−1+n+λPk−1i,i=1,2,⋯,n,xi+n,k−1=x^k−1−n+λPk−1i,i=1,2,⋯,n.Correspondingly, the weights of mean and covariance are presented:(29)Wim=λn+λ,i=0,λ2n+λ,i≠0,Wic=λn+λ+1+β−α2,i=0,λ2n+λ,i≠0,i=0,1,2,…,n,…,2n,whereλ=α2n+k−n, and it is a scalar parameter related to the transformation scale factor. Constant k = 3 − n is chosen to ensure the positive semidefinite of the postcovariance. β is a parameter related to state a priori distribution for reducing the higher order effects, and the general value for Gaussian distribution is 2. The scaling factor α is used such that sigma points are sampled within the range of ±ϱ. It should be of little positive value as much as possible, α=0.001, when the system is seriously nonlinear. The use of α guarantees that nonlinearities outside of the uncertainty region cannot affect the solution.(ii)
Step 2: time update:(30)xkk−1i=fxi,k−1,i=1,2,…,2n,x^kk−1=∑i=02nWimxk|k−1i,Pkk−1=∑i=02nWicxkk−1i−x^kk−1xkk−1i−x^kk−1T,wherex^kk−1 and Pkk−1 are predicted nonaugmented system state and predicted error covariance matrix at time tk−1.(iii)
Step 3: measurement update:(31)Zkk−1i=hxkk−1i,i=1,2,…,2n,(32)Z^kk−1=∑i=02nWimZkk−1i,(33)Pkzz=∑i=02nWicZkk−1i−Z^kk−1Zkk−1i−Z^kk−1T+R^k,Pkxz=∑i=02nWicxkk−1i−x^kk−1Zkk−1i−Z^kk−1T,where(34)Kk=PkxzPkzz−1,(35)x^kk=x^kk−1+KkZk−Z^kk−1,(36)Pkk=Pkk−1−KkPkzzKkT,(37)x^kk−1a=x^kk−1T0T,(38)Pkk−1a=Pkk−100Qk,wherex^kk and Pkk are estimated nonaugmented system state and predicted error covariance matrix at time tk, while x^kka and P^kka are estimated augmented system state and estimated error covariance matrix at time tk.(iv)
Step 4: the tuning of the adaptive scale factor.Define the predicted residual vector isεk:(39)εk=Zk−Z^kk−1.By placing the condition that output residual vectors at different sampling time maintain orthogonal to each other, the residual vector is satisfied:(40)Eεk+jεkT=0,k=1,2,…,j=1,2,….The adaptive scale factorμk is adapted to enhance or weaken the effects of previous measurements on current estimation in filter algorithm. In this way, the adaptive scale factor in AUKF is used to adaptively adjust the influence between system prediction Pkzz and observation P^kzz. The adaptive factor μk is introduced to realize a reasonable balance between system prediction and observation:(41)Pkzz=μkP^kzz+R^k1−μk,where the adaptive factor0≤μk≤1. Considering the uncertainty of the model errors, the estimated covariance matrix P^kzz can be estimated by predicted residual vector εk:(42)P^kzz=εkεkT.According to equations (31), (35), and (39), the following expressions are given:(43)xk−x^k|k=xk−x^k|k−1−KkZk−Z^k|k−1=x˜k|k−1−Kkεk,where(44)εk=Zk−Z^k|k−1=hxkk−1i−Z^kk−1,x˜kk−1=xk−x^kk−1.Considering the condition that the measurement modelhxkk−1i is linearized by the first order Taylor series, the following derivations [26] is presented as follows:(45)Exk−x^kkεkT=Ex˜kk−1−KkεkεkT.DefineGPk|k−1,μk is the goal function. It is expressed in equation (46) according to the relationship in equations (34), (41), and (42):(46)GPk|k−1,μk=Ex˜k|k−1εkT−KkP^kzz.The adaptive scale factor model can be calculated as(47)μk=minGPkk−1,μk.Based on (34)–(38), the estimated residual covariance matrices Pkzz and Pkxz can be formulated by adding the adaptive scale factor μk in (48) and (49). Corresponding vectors K^k, x^kk, and P^kk can be described in equation (50)–(52):(48)P^kzz=1μk∑i=02nWicZk|k−1i−Z^k|k−1Zk|k−1i−Z^k|k−1T+R^k,(49)P^kxz=1μk∑i=02nWicxk|k−1i−x^k|k−1Zk|k−1i−Z^k|k−1T,(50)K^k=P^kxzP^kxz−1,(51)x^kk=x^kk−1+K^kZk−Z^kk−1,(52)P^kk=1μkP^kk−1−K^kP^kzzK^kT.Depending on the predicted residual statistics, the adaptive scale factor is used to adaptively inflate the covariance matrix of thex^kk and P^kk. Meanwhile, the gain matrix K^k is also changed in real time to achieve strong tracking of AUKF. It is reasonable to assume(53)Pkzz≈μkP^kzz.DefinePkzz=Nk and P^kzz=Mk. It can be described as(54)Mkμk=Nk.Here, the adaptive scale factor is introduced:(55)μk=1,μ0<1,μ0,μ0≥1,whereμ0=trNk/trMk; in case of the practical applications, the adaptive scale factor can be set to satisfy the condition μk≤1. Its specific implementation is as follows.It is noted that model disturbance usually exists in the first stage of AUKF. In order to reduce model disturbances on current estimates in filer algorithm, the weight of model prediction in the estimation process should be low. The scaleμk is approximately set to 1. On the contrary, as the measurement model error is gradually reduced over time, the scale μk is close to 0 for weakening the effects of previous measurements. In this way, the filter is caused to enhance the weight of the new measurement data according to the predicted residual vector εk.


### 3.3. RBF Feedback Correction in AUKF

Considering the case of GPS failure, the reliable new measurement data will not be updated. In order to continue to provide reliable information in GPS-denied environment, an attitude algorithm based on RBF-ANN feedback correction is adopted for AUKF. RBF-ANN shows good performance in classification modeling. It consists of three layers: one input layer, one hidden layer, and one output layer. The output of the network is a linear combination of input radial basis function and neuron parameters. Its diagram is shown in Figure3.Figure 3
Radial feedforward neuron structure diagram.When GPS works normally, the estimated deviation of predicted nonaugmented system stateδx^kk is used for correcting the predicted nonaugmented system state x^kk in AUKF according to equation (51):(56)δx^kk=x^kk−x^kk−1=K^kZk−Z^kk−1.When GPS signals are temporarily blocked, the estimated deviation of system stateδx^kk will be provided based on RBF-ANN. The corrected predicted nonenhanced system state δx^kk is used in the estimation process in AUKF.Considering the error effect resulted from low-cost micro-IMU, the processed data of accelerometer, gyro, and magnetometer after calibration process are selected as the input of RBF-ANN. The estimated deviation of the system stateδx^kk is taken as the output of RBF-ANN. In case GPS works normally, the processed outputs data of micro-IMU and the expecting estimated deviation of predicted system state δx^kke in equation (56) are used as the training data of RBF-ANN. The activation function of RBF is chosen as(57)ϕix=exp−x−ci22σ22,where the input data is modeled as a real vector, x∈R. φ· is the activation function of hidden layer, which can be defined as the Gaussian function, and ci is the clustering center vector of input data, which is determined by K-means algorithm. The value σ2 is the standard deviation of activation function:(58)σ2=dmax2m,where dmax is the maximum distance between different cluster centers and m is the quantity of cluster, which is also the neural quantity of the hidden layer. According to minimum error sum of squares, the optimal value m is selected. In the optimization process, K-mean cluster analysis method is adopted. The output is expressed as follows:(59)yi=∑i=1NWiφx−ci=∑i=1NWiexp−x−ci22σ22,where N is the number of hidden units; Wi is the connecting weight coefficient between the kth hidden unit; and the output yi is differentiable with respect to the weight Wi.Input sample dataset is introduced asXi,YiXi∈RN,Yi∈RN,i=1,2,…,N. Here, Yi is the estimated deviation of predicted nonaugmented system state δx^kk. The matrix G is Green function, which is calculated in equation (60), and the function is as follows:(60)G11⋯G1m⋮⋱⋮Gm1⋯GmmW1⋮Wm=Y1⋮Ym.Considering that the matrixG is usually not a nonsingular matrix, the weight matrix W is updated using a pseudoinverse method:(61)W=G+D=GTG−1GTD,where G+ is pseudoinverse matrix and D is an expecting matrix composed of detecting motion errors Yi. The RBF network increases the speed of training by using a local transfer function so that a few neurons have a nonzero response and become active to each input value. RBF network can avoid falling into local minimum when the training is in process. The variable RBF networks are used to represent time-varying dynamics with both the structure and parameters, which are tuned in real time:(62)ΔWkj=ηdk−WkTΦφj.According to the low dynamic and high dynamic environment, the different control strategies of low and high dynamic environments are adopted to solve dynamic accuracy of attitude calculation. The proposed method can estimate the attitude angles utilizing affordable sensors under different dynamic situations. The switching criterion is determined according to roll angle rates of body. The attitude algorithm of single sample quaternion vector and attitude algorithm based on accelerometer, gyro, and magnetometer are adopted in low dynamic environment. Alternatively, cubic spline algorithm based on quaternion vector based on Micro-IMU and GPS is designed in the high dynamic environment. Considering the situation of GPS failure, an attitude algorithm based on RBF-ANN feedback correction is adopted to guarantee the reliable measurement. The estimated deviation of system stateδx^kk will be provided based on RBF-ANN. Furthermore, the adaptive scale factor μk is set to reduce or enhance the influence of previous measurements on the current estimation in AUKF. In this way, a reasonable balance between system prediction and observation is achieved. The scheme of the proposed method is shown in Figure 4.Figure 4
The scheme of combining AUKF fusion algorithm with RBF-ANN for low-cost micro-IMU and GPS.This scheme is actually a combined adaptive filtering approach and RBF algorithm architecture. It comprises two parts: AUKF based on multisensor fusion module and RBF estimation method. The multisensor fusion module consists of micro-IMU, magnetometer, and single antenna GPS. The dominant errors are compensated by the calibration process before fusion algorithm. Different fusion strategies provide efficient solutions to enhance the accuracy of the attitude estimation.


## 3.1. System State and Observation Model

According to the output characteristic of MEMS sensor, the filter algorithm can effectively improve the accuracy of sensor measurement output. Considering the nonlinear characteristic of the system, the adaptive UKF fusion algorithm is preferred to solve the attitude estimation in the case of dynamic environment changes.Establishing the perfect state equation and measurement equation is the key to improving the solution. Considering the effect of the model error, the quaternion vectorq and the output error of gyroscope bt are chosen as the state vector. The operator “⊗” denotes the quaternion multiplication:(10)X=qbg7×1T.Here,q is the quaternion vector. q=q0q1q2q3. The bias of gyroscope bg=bgxbgybgz, which is the bias drift in three sensitive axes x, y, and z. The coordinate transformation from the navigation frame to the body fixed frame is based on the three angles θ,ϕ, and ψ. The attitude matrix Cbn from the navigation frame and body frame by three Euler angles is given by(11)Cbn=cosθcosψsinϕsinθcosψ−cosϕsinψcosϕsinθcosψ+sinϕsinψcosθsinψsinϕsinθsinψ+cosϕcosψcosϕsinθsinψ−sinϕcosψ−sinθsinϕcosθcosϕcosθ.The single spline algorithm based on the quaternion vector in equation (12) is suitable for attitude angle estimation in a low dynamic environment:(12)X˙7×1=fq,bt=12A4×404×303×4B3×3⋅X7×1+W7×1,(13)A4×4=0−ω˜nbxb−ω˜nbyb−ω˜nbzbω˜nbxb0ω˜nbzb−ω˜nbybω˜nbyb−ω˜nbzb0ω˜nbxbω˜nbzbω˜nbyb−ω˜nbxb0,B3×3=−1τ∗100010001,W7×1=01×4ωrxωryωrzT,where τ is the time constant of a first order Gauss–Markov process and ωr is Gaussian white process. The body angler rate with respect to the navigation frame ω˜nb is given by the difference between the body angular rate, ω˜ib, and the navigation frame rotation, ωin:(14)ω˜nbb=ω˜ibb−CbnTωinn=ωibb+δωibn−CbnTωinn,where the vector ωinn can be written as ωinn=ωien+ωenn. The vector ωien is the Earth rate in the navigation frame. The vector ωenn is the turn rate of the navigation frame with respect to the velocity of the craft. The output error δωibb of gyro is replaced by the constant deviation b0, and the whole range of the constant deviation b0 is estimated as the definite error through the off-line calibration experiment.In a high dynamic environment, the cubic spline algorithm based on the quaternion vector is used for the estimation of attitude angle:(15)qt+h=qt⊗rh.rh=cosΦ/2+Φ/ΦsinΦ/2, Φ is the equivalent rotating vector in sampling period from t to t + h. h is the time step in sampling time. The corresponding angular rate of gyro in sampling period ωnbb is expressed using the straight line fitting method. The expression of matrix in equation (15) could be rewritten as(16)q0t+hq1t+hq2t+hq3t+h=cosΦ2−ΦxΦsinΦ2−ΦyΦsinΦ2ΦxΦsinΦ2cosΦ2ΦzΦsinΦ2ΦyΦsinΦ2−ΦzΦsinΦ2cosΦ2−ΦzΦsinΦ2−ΦyΦsinΦ2ΦxΦsinΦ2ΦzΦsinΦ2ΦyΦsinΦ2−ΦxΦsinΦ2cosΦ2q0tq1tq2tq3t.The differential equation for the rotating vector can be obtained by neglecting the high order items:(17)Φ˙=ω˜nbb+12Φ×ω˜nbb+112Φ×Φ×ω˜nbb.The polynomial fitting equation of the angular rate is expressed as follows:(18)ωnbb=a+2bτ+3cτ2,0≤τ≤h,where a, b, and c are fitting parameters for quadratic function polynomial. Defining Φtk and Φtk−1 are the attitude quaternions of tk and tk−1 moment, respectively, the Taylor series expansion equation of the attitude quaternion is expressed in(19)Φtk=Φtk−1+hΦ˙tk−1+h22!Φ¨tk−1+h33!Φ⋯tk−1+⋯.The attitude algorithm with incremental angle could be constructed as follows:(20)△θi=∫i−1/3hi/3hωtk+τdτ,i=1,2,3,…,where △θ1, △θ2, and △θ3 are the angular increments of tk,tk+1/3h, tk+1/3h,tk+2/3h, and tk+2/3h,tk+1:(21)△θ1=13ah+19bh2+127ch3,△θ2=13ah+39bh2+727ch3,△θ3=13ah+59bh2+1927ch3.The equivalent rotating vector is expressed according to Taylor series expansion equation:(22)Φh=Φ0+Φ˙oh+12Φ¨0h2+16Φ⃛0h3+124Φ40h4+1120Φ50h5=ah+bh2+ch3+16a×bh3+14a×ch4+110b×ch5=△θ1+△θ2+△θ3+3380△θ1×△θ3+5780△θ2×θ2−△θ3.As can be seen from the Section2.1, the accelerometer can be used for calculating the pitch and roll angles by measuring the gravity vectors if the vehicle is stationary or in low dynamic environment. Triaxis magnetometers are used for calculating the yaw angle. Its accuracy is heavily affected by local magnetic environments. According to the attitude algorithm of the accelerometer and magnetometer, the attitude angle ϕacc, θacc, and ψmag can be treated as the measurement of the attitude system in the low dynamic environment:(23)ϕaccθaccψmag=arctan2q2q3+2q0q12q02+2q32−1−arcsin2q1q3−2q0q2arctan2q1q2+2q0q32q02+2q12−1+∇m.∇m=∇ϕ∇θ∇ψT, where ∇ϕ,∇θ, and ∇ψ are the measurement noise of roll, pitch, and yaw angle. E∇m=0,E∇mk,∇mkT=Rδkj, and R=diagσϕ2σθ2σψ2, where σϕ2, σθ2, and σψ2 are standard deviation of attitude errors. Their values depend on the precision of the single MEMS sensor.In the high dynamic environment, the aforementioned attitude estimation approaches utilizing only the single sensor are difficult to provide reliable estimation due to sensor errors. The dynamic accuracy of the attitude angle that are calculated in virtue of the accelerometer and magnetometer is affected largely due to the motion of body in the high dynamic environment. In such a case, the measurement vectorsϕacc, θacc, and ψmag are unreliable due to the effects of sensor errors. The specific force referenced in n-frame can be treated as the measurement of low-cost micro-IMU attitude system. Therefore, the measurement equation can be replaced as(24)Z=Fn=FxnFynFzn=Cbnfb+v,where the measurement vector Fn is the specific force referenced in n-frame and v is the measurement noise. The attitude error matrix Cbn is expressed in terms of quaternion in equation (11):(25)Cbn=1−2q22+q322q1q2−q0q32q1q3+q0q22q1q2+q0q31−2q12+q322q2q3−q0q12q1q3−q0q22q2q3+q0q11−2q12+q22.The measurement vectorFn is calculated as(26)Fn=V˙n+2ωien+ωenn×Vn−gn.Here, the vectorV˙n is given by calculating the derivative of velocity afforded by GPS.


## 3.2. Adaptive UKF Fusion Algorithm

Due to the fact that the statistical characteristics of the noise are real-time changed, it is essential to adjust noise estimation in the filter algorithm. Based on the nature of UKF algorithm, unscented transformation (UT) is an approximate method to change the mean and covariance of random variables when they undergo nonlinear transformation [21, 25]. The adaptive performance of AUKF is the filter process of the measurement noise variance of the real-time estimation. The basic steps of the AUKF algorithm are as follows:(i)
Step 1: generation of sigma points and weights.The estimated state and corresponding covariance of the system can be expressed as follows:(27)x^k−1=Exk−1,P^k−1=Ex^k−1−xk−1x^k−1−xk−1T.Sigmal pointsxi,k−1i=1,2,…,n,k≥1 are generated according to the following equations:(28)xi,k−1=x^k−1+n+λPk−1i,i=1,2,⋯,n,xi+n,k−1=x^k−1−n+λPk−1i,i=1,2,⋯,n.Correspondingly, the weights of mean and covariance are presented:(29)Wim=λn+λ,i=0,λ2n+λ,i≠0,Wic=λn+λ+1+β−α2,i=0,λ2n+λ,i≠0,i=0,1,2,…,n,…,2n,whereλ=α2n+k−n, and it is a scalar parameter related to the transformation scale factor. Constant k = 3 − n is chosen to ensure the positive semidefinite of the postcovariance. β is a parameter related to state a priori distribution for reducing the higher order effects, and the general value for Gaussian distribution is 2. The scaling factor α is used such that sigma points are sampled within the range of ±ϱ. It should be of little positive value as much as possible, α=0.001, when the system is seriously nonlinear. The use of α guarantees that nonlinearities outside of the uncertainty region cannot affect the solution.(ii)
Step 2: time update:(30)xkk−1i=fxi,k−1,i=1,2,…,2n,x^kk−1=∑i=02nWimxk|k−1i,Pkk−1=∑i=02nWicxkk−1i−x^kk−1xkk−1i−x^kk−1T,wherex^kk−1 and Pkk−1 are predicted nonaugmented system state and predicted error covariance matrix at time tk−1.(iii)
Step 3: measurement update:(31)Zkk−1i=hxkk−1i,i=1,2,…,2n,(32)Z^kk−1=∑i=02nWimZkk−1i,(33)Pkzz=∑i=02nWicZkk−1i−Z^kk−1Zkk−1i−Z^kk−1T+R^k,Pkxz=∑i=02nWicxkk−1i−x^kk−1Zkk−1i−Z^kk−1T,where(34)Kk=PkxzPkzz−1,(35)x^kk=x^kk−1+KkZk−Z^kk−1,(36)Pkk=Pkk−1−KkPkzzKkT,(37)x^kk−1a=x^kk−1T0T,(38)Pkk−1a=Pkk−100Qk,wherex^kk and Pkk are estimated nonaugmented system state and predicted error covariance matrix at time tk, while x^kka and P^kka are estimated augmented system state and estimated error covariance matrix at time tk.(iv)
Step 4: the tuning of the adaptive scale factor.Define the predicted residual vector isεk:(39)εk=Zk−Z^kk−1.By placing the condition that output residual vectors at different sampling time maintain orthogonal to each other, the residual vector is satisfied:(40)Eεk+jεkT=0,k=1,2,…,j=1,2,….The adaptive scale factorμk is adapted to enhance or weaken the effects of previous measurements on current estimation in filter algorithm. In this way, the adaptive scale factor in AUKF is used to adaptively adjust the influence between system prediction Pkzz and observation P^kzz. The adaptive factor μk is introduced to realize a reasonable balance between system prediction and observation:(41)Pkzz=μkP^kzz+R^k1−μk,where the adaptive factor0≤μk≤1. Considering the uncertainty of the model errors, the estimated covariance matrix P^kzz can be estimated by predicted residual vector εk:(42)P^kzz=εkεkT.According to equations (31), (35), and (39), the following expressions are given:(43)xk−x^k|k=xk−x^k|k−1−KkZk−Z^k|k−1=x˜k|k−1−Kkεk,where(44)εk=Zk−Z^k|k−1=hxkk−1i−Z^kk−1,x˜kk−1=xk−x^kk−1.Considering the condition that the measurement modelhxkk−1i is linearized by the first order Taylor series, the following derivations [26] is presented as follows:(45)Exk−x^kkεkT=Ex˜kk−1−KkεkεkT.DefineGPk|k−1,μk is the goal function. It is expressed in equation (46) according to the relationship in equations (34), (41), and (42):(46)GPk|k−1,μk=Ex˜k|k−1εkT−KkP^kzz.The adaptive scale factor model can be calculated as(47)μk=minGPkk−1,μk.Based on (34)–(38), the estimated residual covariance matrices Pkzz and Pkxz can be formulated by adding the adaptive scale factor μk in (48) and (49). Corresponding vectors K^k, x^kk, and P^kk can be described in equation (50)–(52):(48)P^kzz=1μk∑i=02nWicZk|k−1i−Z^k|k−1Zk|k−1i−Z^k|k−1T+R^k,(49)P^kxz=1μk∑i=02nWicxk|k−1i−x^k|k−1Zk|k−1i−Z^k|k−1T,(50)K^k=P^kxzP^kxz−1,(51)x^kk=x^kk−1+K^kZk−Z^kk−1,(52)P^kk=1μkP^kk−1−K^kP^kzzK^kT.Depending on the predicted residual statistics, the adaptive scale factor is used to adaptively inflate the covariance matrix of thex^kk and P^kk. Meanwhile, the gain matrix K^k is also changed in real time to achieve strong tracking of AUKF. It is reasonable to assume(53)Pkzz≈μkP^kzz.DefinePkzz=Nk and P^kzz=Mk. It can be described as(54)Mkμk=Nk.Here, the adaptive scale factor is introduced:(55)μk=1,μ0<1,μ0,μ0≥1,whereμ0=trNk/trMk; in case of the practical applications, the adaptive scale factor can be set to satisfy the condition μk≤1. Its specific implementation is as follows.It is noted that model disturbance usually exists in the first stage of AUKF. In order to reduce model disturbances on current estimates in filer algorithm, the weight of model prediction in the estimation process should be low. The scaleμk is approximately set to 1. On the contrary, as the measurement model error is gradually reduced over time, the scale μk is close to 0 for weakening the effects of previous measurements. In this way, the filter is caused to enhance the weight of the new measurement data according to the predicted residual vector εk.


## 3.3. RBF Feedback Correction in AUKF

Considering the case of GPS failure, the reliable new measurement data will not be updated. In order to continue to provide reliable information in GPS-denied environment, an attitude algorithm based on RBF-ANN feedback correction is adopted for AUKF. RBF-ANN shows good performance in classification modeling. It consists of three layers: one input layer, one hidden layer, and one output layer. The output of the network is a linear combination of input radial basis function and neuron parameters. Its diagram is shown in Figure3.Figure 3
Radial feedforward neuron structure diagram.When GPS works normally, the estimated deviation of predicted nonaugmented system stateδx^kk is used for correcting the predicted nonaugmented system state x^kk in AUKF according to equation (51):(56)δx^kk=x^kk−x^kk−1=K^kZk−Z^kk−1.When GPS signals are temporarily blocked, the estimated deviation of system stateδx^kk will be provided based on RBF-ANN. The corrected predicted nonenhanced system state δx^kk is used in the estimation process in AUKF.Considering the error effect resulted from low-cost micro-IMU, the processed data of accelerometer, gyro, and magnetometer after calibration process are selected as the input of RBF-ANN. The estimated deviation of the system stateδx^kk is taken as the output of RBF-ANN. In case GPS works normally, the processed outputs data of micro-IMU and the expecting estimated deviation of predicted system state δx^kke in equation (56) are used as the training data of RBF-ANN. The activation function of RBF is chosen as(57)ϕix=exp−x−ci22σ22,where the input data is modeled as a real vector, x∈R. φ· is the activation function of hidden layer, which can be defined as the Gaussian function, and ci is the clustering center vector of input data, which is determined by K-means algorithm. The value σ2 is the standard deviation of activation function:(58)σ2=dmax2m,where dmax is the maximum distance between different cluster centers and m is the quantity of cluster, which is also the neural quantity of the hidden layer. According to minimum error sum of squares, the optimal value m is selected. In the optimization process, K-mean cluster analysis method is adopted. The output is expressed as follows:(59)yi=∑i=1NWiφx−ci=∑i=1NWiexp−x−ci22σ22,where N is the number of hidden units; Wi is the connecting weight coefficient between the kth hidden unit; and the output yi is differentiable with respect to the weight Wi.Input sample dataset is introduced asXi,YiXi∈RN,Yi∈RN,i=1,2,…,N. Here, Yi is the estimated deviation of predicted nonaugmented system state δx^kk. The matrix G is Green function, which is calculated in equation (60), and the function is as follows:(60)G11⋯G1m⋮⋱⋮Gm1⋯GmmW1⋮Wm=Y1⋮Ym.Considering that the matrixG is usually not a nonsingular matrix, the weight matrix W is updated using a pseudoinverse method:(61)W=G+D=GTG−1GTD,where G+ is pseudoinverse matrix and D is an expecting matrix composed of detecting motion errors Yi. The RBF network increases the speed of training by using a local transfer function so that a few neurons have a nonzero response and become active to each input value. RBF network can avoid falling into local minimum when the training is in process. The variable RBF networks are used to represent time-varying dynamics with both the structure and parameters, which are tuned in real time:(62)ΔWkj=ηdk−WkTΦφj.According to the low dynamic and high dynamic environment, the different control strategies of low and high dynamic environments are adopted to solve dynamic accuracy of attitude calculation. The proposed method can estimate the attitude angles utilizing affordable sensors under different dynamic situations. The switching criterion is determined according to roll angle rates of body. The attitude algorithm of single sample quaternion vector and attitude algorithm based on accelerometer, gyro, and magnetometer are adopted in low dynamic environment. Alternatively, cubic spline algorithm based on quaternion vector based on Micro-IMU and GPS is designed in the high dynamic environment. Considering the situation of GPS failure, an attitude algorithm based on RBF-ANN feedback correction is adopted to guarantee the reliable measurement. The estimated deviation of system stateδx^kk will be provided based on RBF-ANN. Furthermore, the adaptive scale factor μk is set to reduce or enhance the influence of previous measurements on the current estimation in AUKF. In this way, a reasonable balance between system prediction and observation is achieved. The scheme of the proposed method is shown in Figure 4.Figure 4
The scheme of combining AUKF fusion algorithm with RBF-ANN for low-cost micro-IMU and GPS.This scheme is actually a combined adaptive filtering approach and RBF algorithm architecture. It comprises two parts: AUKF based on multisensor fusion module and RBF estimation method. The multisensor fusion module consists of micro-IMU, magnetometer, and single antenna GPS. The dominant errors are compensated by the calibration process before fusion algorithm. Different fusion strategies provide efficient solutions to enhance the accuracy of the attitude estimation.


## 4. Experimental Results

### 4.1. Attitude Determination Experiment in Low Dynamic Environment

To verify the effect of the proposed algorithm in the low dynamic environment, the attitude testing experiments are implemented in tracking human body motions, as shown in Figure5.Figure 5
Attitude testing experiment in low dynamic environment.The different algorithms are implemented in DSP development prototype. The attitudes estimated separately by different sensors and filter algorithms are demonstrated in Figure6. Data is denoted as “Gyro,” “Acc/Mag,” “CF,” “EKF,” “UKF,” and “Reference,” respectively, derived from gyroscope integration based on quaternion algorithm and attitude algorithm based on the accelerometer and magnetometer, Complementary Filter, Extended Kalman Filter algorithm, Unscented Kalman filter, and referenced Mti-G.Figure 6
Attitude results in low dynamic environment.
(a)(b)(c)(d)(e)(f)The result shows that the single spline quaternion algorithm based on gyro has short-term accuracy, but the error will be accumulated over time due to gyro bias drift. The attitude algorithm of the accelerometer is used to provide the long-term accuracy estimation of the roll and pitch. The yaw angle is later calculated by the magnetometer. It is also affected by sensor error and vibration noise.The complementary filter (CF) method can provide accurate Euler estimate in low dynamic condition. It utilizes characters of accelerometers that has long-term accuracy, the gyroscope has only short-term accuracy and magnetometer that has low noise and high bandwidth. According to the characteristics of the MEMS sensor, the cut-off bandwidths of low-pass filter and high-pass filter are chosen as the weight coefficient.The attitude algorithms based on EKF and UKF have similar estimation performances. The UKF has better accuracy than EKF due to undergoing the nonlinear measurement equation in filter. The STD of the pitch, roll, and yaw angle is 1.72°, 3.11°, and 5.75°, respectively, at all times in EKF. The STD of attitude angles is 0.55°, 0.87°, and 0.93°, respectively, in UKF. The estimation of the yaw angle is affected by the error of pitch and roll angle calculated by the accelerometer and gyro.


### 4.2. Attitude Determination Experiment in High Dynamic Environment

Considering the characteristics of spinning projectile, the testing experiments are performed to simulate the situation that the rotation of the symmetry axis is in high dynamic and rest the rotations of other orthogonal axes are in low dynamic. A portable, single rotating table is designed, which can be mounted in a mobile vehicle. The single axis rotating table driving by the servo motor is used to simulate the motion of the roll attitude for the spinning projectile. Meanwhile, the attitude changes for the pitch and yaw angle are achieved when the testing vehicle equipped with portable table moves quickly in different routes. The experimental platform consists the prototype of low-cost IMU, a single-antenna GPS receiver, prototype of DSP microcontroller, the MTi-G, servo motor, slip ring, DC to AC inverter, DC regulated power, laptop, and testing vehicle. The experimental system is shown in Figure7.Figure 7
Experimental platform of microembedded navigation system.
(a)(b)In experiment system, the Mti-G device from the Xsens Motion Technologies is used to provide reference attitude, which consists MEMS gyro, accelerometer, and magnetometer. The sampling frequency is 100 Hz to collect raw data. The measurements of the GPS receiver are provided at 10 Hz sampling frequency. The single antenna is mounted in the long pole, which is held up outside the testing vehicle. The micro-IMU, GPS receiver, and MTi-G are mounted in the single axis rotating table, which is connected to the servo motor directly. The reference frames of micro-IMU and MTi-G are consistent during the rotating motion. At the beginning of the experiment, the single axis rotating table is fitted into the floor of the testing vehicle. The rated speed of the servo motor is 3000 rpm. The experimental collecting real-time data are transmitted to the laptop PC with the USB port from converting the RS232 serial port by slip ring between rotating and nonrotating mechanism. Experimental tests were performed in the urban section.In order to verify the effect of the different filter algorithms and the proposed method, several testing experiments are implemented in different rotating motions and traveling paths. A set of comparative experiments are carried out to verify the accuracy of different algorithms. The results are shown in Figure8.Figure 8
(a) Roll angle and (b) pitch angle in high dynamic condition.
(a)(b)Figure8(a) illustrates the estimation result of attitude algorithm by the accelerometer and magnetometer. In this scenario, the rotating angular rate of servo motor varies from 360°/s to 575°/s. For the roll angle, it can track with a reference angle in the range of less than 360°/s. With the increase of servo-motor speed, the dynamic condition has a great influence on the estimation accuracy. The propagation of error occurred during the estimation of roll angle. The maximum error of roll angle is 5°. In addition, the pitch angle calculated by the attitude algorithm of the accelerometer and magnetometer deviates greatly from the reference pitch angle in Figure 8(b). The performance degradation can be attributed to the reason that the error of pitch angle is caused due to the sensor errors and vibration noises. The estimated pitch angle is affected by large errors (up to 70% during the peak angle). The results show that the inaccuracy calculation of the algorithm has a great influence on the measurement precision. As it is visible, the calculated roll and pitch can be acceptable in low dynamic condition. In high dynamic condition, the measurement precision was affected greatly due to the inevitable disturbances and dynamic motions. So, the calculated results of attitude algorithm by the accelerometer and magnetometer cannot be affordable as the measurement vector in AUKF.To produce as many as possible driving maneuvers to validate the performance in complex driving conditions, the experiment was conducted on an overpass over the road. The testing trajectory is shown in Figure9. It can produce large changes in both the pitch and yaw angle during the experiment. The corresponding experiments were carried out on the cases that GPS worked normally, the vehicle was in turning with large sideslip angle, and GPS signals were affected and lost by obstruction.Figure 9
Testing trajectory of the attitude determination system.The results of the on-vehicle test are presented from 0 s to 100 s. During the testing period, all attitude data were collected and recorded in the embedded navigation system. The GPS satellite signal is obstructed artificially to simulate external environmental disturbance in part path. To evaluate the effect of the proposed algorithm, the real experimental tests in the high dynamic environment are carried out. During the testing experiments, the servo motor of single rotating table is performed actions automatically to predesigned motion program. The maximum roll angle rate can reach 1080°/s to simulate the rotating of the spinning shell. In the meantime, the changes of the pitch and yaw angle is simulated by the testing vehicle riding in different routes. The results are presented in Figure10. Data denoted as “UKF,” “AUKF-RBF,” “Reference,” and “GPS-Outage,” respectively, derived from the conventional UKF, adaptive UKF based on RBF network, and GPS outage.Figure 10
Attitude determination results in high dynamic environment.
(a)(b)(c)(d)(e)(f)(g)(h)As shown in Figure10(a), UKF and AUKF-RBF algorithms can both track the reference roll angle within angle rate range of 1080°/s. The quaternion vector by cubic spline algorithm is estimated in the filter. Using quaternion updating algorithm, the roll angle is obtained. Although its accuracy is affected by the model disturbances on current estimates and the random error of IMU, the performance is still acceptable in UKF and AUKF-RBF algorithms. During the whole stage, the maximum of the errors of roll angle in UKF and AUKF-RBF are 5° and 2°, respectively. AUKF-RBF algorithm is effective and accurate in full tracking route through the tuning of the adaptive scale factor. Mean Square Error (MSE) of AUKF-RBF algorithm is 0.42° in dynamic condition.It can also be found that the performance degrades during the GPS outage(i.e., from 40 to 50 s). As the GPS-failure interval, the performance of the proposed method becomes worse in comparison with that at the same interval without GPS failure. Owing to lack of enough measurement data, the estimation of the roll angle has no convergence and a trend of divergence in high rotating environment in Figure10(b). To solve the loss of measurement data in GPS outage section, the proposed RBF-ANN algorithm can continue to offer data availability. The estimated deviation of system state δx^kk in equation (56) is replaced, which is provided by RBF-ANN. The corrected system state x^kk can be used for estimation process in AUKF-RBF and UKF.In Figures10(c) and 10(e), the estimated pitch and yaw angle are obtained. Considering the fact that the micro-IMU is mounted by a cantilever beam in single axis rotating table, mechanical vibration of rotating motion may affect the accuracy of the estimated pitch and yaw angle. As it is visible, the estimates of pitch and yaw angle based on UKF and AUKF can follow the reference attitude angle with the acceptable error. The errors for pitch and yaw angle are within 0.79° and 0.8° in UKF algorithm. The errors for pitch and yaw angle with adaptive estimation are within 0.39° and 0.43° in AUKF-RBF algorithm. During the intervals of 10–20 s and 20–30 s, the estimated yaw angle greatly changed in the opposite direction at sample time t = 8.3 s, t = 25 s, and t = 30 s. The estimated yaw angle is affected by large sideslip angle in Figures 10(e) and 10(g). Correspondingly, the estimated errors come out at these sampling times in Figure 10(f). These errors increase by an average of 0.5° as compared with the errors in the steady section. In Figure 10(h), the yaw angle is estimated depending on RBF feedback and tuning of adaptive scale factor in AUKF, and the fluctuation of error decreased faster in the GPS outage.The convergence of RBF in UKF and AUKF is shown in Figure11.Figure 11
The convergence process of RBF-ANN.As can be seen from above, the training time in AUKF algorithm takes about 8.25 s. Compared with UKF algorithm, the costing time of training process takes about 13.25 s. Obviously, in the process of training RBF network, it spends less time than UKF algorithm.The attitude angles are estimated in both UKF and AUKF-RBF algorithms. The AUKF-RBF algorithm has better estimation precision than UKF during the GPS outage section. RBF correction algorithm is regarded to be reliable and can be utilized by AUKF. The estimated precision of RBF depends on the accuracy of training data. The estimated attitude angles in AUKF-RBF algorithm provided more reliable results as training samples than UKF algorithm during the GPS nonoutage stage. Compared with the performance of GPS normal working sections (i.e., 10–20 s) and GPS outage section (i.e., 40–50 s), the RMS of the proposed AUKF-RBF is shown in Table2.Table 2
Contrast of mean square error of GPS failure and GPS normal.
RollPitchYawUKF-GPS outage2.00480.98820.8042UKF-GPS normal1.97400.66330.5834AUKF-RBF- GPS outage0.41850.38160.3658It can also be found that the performance degrades during the GPS outage. The estimation error in GPS outage section is larger than the error in GPS normal working section. The reason is that the reliable measurements are provided by GPS and accelerometer in the GPS normal working section. On the contrary, the measurements were estimated by RBF in GPS outage. The corresponding programs of the proposed algorithm is compiled and run on CCS 3.3 environment for the DSP controller in real time. The calculating time takes 8.96 ms in a sampling period. For brevity, we will concentrate on comparing the performances of proposed in the whole stage. The AUKF-RBF algorithm can not only provide accurate estimation GPS normal working section but also guarantee reliable estimation in the GPS-denied section. The attitude angle estimation error statistics of various methods in testing experiment are summarized in Table3.Table 3
Mean square error of the attitude angle in the whole stage.
RollPitchYawUKF1.98570.79130.8049AUKF-RBF0.42120.38790.4254Compared with performance of the abovementioned methods, the proposed AUKF-RBF method provides more accurate and reliable estimation whether in normal working condition or GPS failure. It has good estimation performance in the whole section, which suffers from vibration noises due to inevitable disturbances and high rotating motion. The MSE of roll, pitch, and yaw based on the proposed method is 0.4212°, 0.3879°, and 0.4254°, respectively.


## 4.1. Attitude Determination Experiment in Low Dynamic Environment

To verify the effect of the proposed algorithm in the low dynamic environment, the attitude testing experiments are implemented in tracking human body motions, as shown in Figure5.Figure 5
Attitude testing experiment in low dynamic environment.The different algorithms are implemented in DSP development prototype. The attitudes estimated separately by different sensors and filter algorithms are demonstrated in Figure6. Data is denoted as “Gyro,” “Acc/Mag,” “CF,” “EKF,” “UKF,” and “Reference,” respectively, derived from gyroscope integration based on quaternion algorithm and attitude algorithm based on the accelerometer and magnetometer, Complementary Filter, Extended Kalman Filter algorithm, Unscented Kalman filter, and referenced Mti-G.Figure 6
Attitude results in low dynamic environment.
(a)(b)(c)(d)(e)(f)The result shows that the single spline quaternion algorithm based on gyro has short-term accuracy, but the error will be accumulated over time due to gyro bias drift. The attitude algorithm of the accelerometer is used to provide the long-term accuracy estimation of the roll and pitch. The yaw angle is later calculated by the magnetometer. It is also affected by sensor error and vibration noise.The complementary filter (CF) method can provide accurate Euler estimate in low dynamic condition. It utilizes characters of accelerometers that has long-term accuracy, the gyroscope has only short-term accuracy and magnetometer that has low noise and high bandwidth. According to the characteristics of the MEMS sensor, the cut-off bandwidths of low-pass filter and high-pass filter are chosen as the weight coefficient.The attitude algorithms based on EKF and UKF have similar estimation performances. The UKF has better accuracy than EKF due to undergoing the nonlinear measurement equation in filter. The STD of the pitch, roll, and yaw angle is 1.72°, 3.11°, and 5.75°, respectively, at all times in EKF. The STD of attitude angles is 0.55°, 0.87°, and 0.93°, respectively, in UKF. The estimation of the yaw angle is affected by the error of pitch and roll angle calculated by the accelerometer and gyro.


## 4.2. Attitude Determination Experiment in High Dynamic Environment

Considering the characteristics of spinning projectile, the testing experiments are performed to simulate the situation that the rotation of the symmetry axis is in high dynamic and rest the rotations of other orthogonal axes are in low dynamic. A portable, single rotating table is designed, which can be mounted in a mobile vehicle. The single axis rotating table driving by the servo motor is used to simulate the motion of the roll attitude for the spinning projectile. Meanwhile, the attitude changes for the pitch and yaw angle are achieved when the testing vehicle equipped with portable table moves quickly in different routes. The experimental platform consists the prototype of low-cost IMU, a single-antenna GPS receiver, prototype of DSP microcontroller, the MTi-G, servo motor, slip ring, DC to AC inverter, DC regulated power, laptop, and testing vehicle. The experimental system is shown in Figure7.Figure 7
Experimental platform of microembedded navigation system.
(a)(b)In experiment system, the Mti-G device from the Xsens Motion Technologies is used to provide reference attitude, which consists MEMS gyro, accelerometer, and magnetometer. The sampling frequency is 100 Hz to collect raw data. The measurements of the GPS receiver are provided at 10 Hz sampling frequency. The single antenna is mounted in the long pole, which is held up outside the testing vehicle. The micro-IMU, GPS receiver, and MTi-G are mounted in the single axis rotating table, which is connected to the servo motor directly. The reference frames of micro-IMU and MTi-G are consistent during the rotating motion. At the beginning of the experiment, the single axis rotating table is fitted into the floor of the testing vehicle. The rated speed of the servo motor is 3000 rpm. The experimental collecting real-time data are transmitted to the laptop PC with the USB port from converting the RS232 serial port by slip ring between rotating and nonrotating mechanism. Experimental tests were performed in the urban section.In order to verify the effect of the different filter algorithms and the proposed method, several testing experiments are implemented in different rotating motions and traveling paths. A set of comparative experiments are carried out to verify the accuracy of different algorithms. The results are shown in Figure8.Figure 8
(a) Roll angle and (b) pitch angle in high dynamic condition.
(a)(b)Figure8(a) illustrates the estimation result of attitude algorithm by the accelerometer and magnetometer. In this scenario, the rotating angular rate of servo motor varies from 360°/s to 575°/s. For the roll angle, it can track with a reference angle in the range of less than 360°/s. With the increase of servo-motor speed, the dynamic condition has a great influence on the estimation accuracy. The propagation of error occurred during the estimation of roll angle. The maximum error of roll angle is 5°. In addition, the pitch angle calculated by the attitude algorithm of the accelerometer and magnetometer deviates greatly from the reference pitch angle in Figure 8(b). The performance degradation can be attributed to the reason that the error of pitch angle is caused due to the sensor errors and vibration noises. The estimated pitch angle is affected by large errors (up to 70% during the peak angle). The results show that the inaccuracy calculation of the algorithm has a great influence on the measurement precision. As it is visible, the calculated roll and pitch can be acceptable in low dynamic condition. In high dynamic condition, the measurement precision was affected greatly due to the inevitable disturbances and dynamic motions. So, the calculated results of attitude algorithm by the accelerometer and magnetometer cannot be affordable as the measurement vector in AUKF.To produce as many as possible driving maneuvers to validate the performance in complex driving conditions, the experiment was conducted on an overpass over the road. The testing trajectory is shown in Figure9. It can produce large changes in both the pitch and yaw angle during the experiment. The corresponding experiments were carried out on the cases that GPS worked normally, the vehicle was in turning with large sideslip angle, and GPS signals were affected and lost by obstruction.Figure 9
Testing trajectory of the attitude determination system.The results of the on-vehicle test are presented from 0 s to 100 s. During the testing period, all attitude data were collected and recorded in the embedded navigation system. The GPS satellite signal is obstructed artificially to simulate external environmental disturbance in part path. To evaluate the effect of the proposed algorithm, the real experimental tests in the high dynamic environment are carried out. During the testing experiments, the servo motor of single rotating table is performed actions automatically to predesigned motion program. The maximum roll angle rate can reach 1080°/s to simulate the rotating of the spinning shell. In the meantime, the changes of the pitch and yaw angle is simulated by the testing vehicle riding in different routes. The results are presented in Figure10. Data denoted as “UKF,” “AUKF-RBF,” “Reference,” and “GPS-Outage,” respectively, derived from the conventional UKF, adaptive UKF based on RBF network, and GPS outage.Figure 10
Attitude determination results in high dynamic environment.
(a)(b)(c)(d)(e)(f)(g)(h)As shown in Figure10(a), UKF and AUKF-RBF algorithms can both track the reference roll angle within angle rate range of 1080°/s. The quaternion vector by cubic spline algorithm is estimated in the filter. Using quaternion updating algorithm, the roll angle is obtained. Although its accuracy is affected by the model disturbances on current estimates and the random error of IMU, the performance is still acceptable in UKF and AUKF-RBF algorithms. During the whole stage, the maximum of the errors of roll angle in UKF and AUKF-RBF are 5° and 2°, respectively. AUKF-RBF algorithm is effective and accurate in full tracking route through the tuning of the adaptive scale factor. Mean Square Error (MSE) of AUKF-RBF algorithm is 0.42° in dynamic condition.It can also be found that the performance degrades during the GPS outage(i.e., from 40 to 50 s). As the GPS-failure interval, the performance of the proposed method becomes worse in comparison with that at the same interval without GPS failure. Owing to lack of enough measurement data, the estimation of the roll angle has no convergence and a trend of divergence in high rotating environment in Figure10(b). To solve the loss of measurement data in GPS outage section, the proposed RBF-ANN algorithm can continue to offer data availability. The estimated deviation of system state δx^kk in equation (56) is replaced, which is provided by RBF-ANN. The corrected system state x^kk can be used for estimation process in AUKF-RBF and UKF.In Figures10(c) and 10(e), the estimated pitch and yaw angle are obtained. Considering the fact that the micro-IMU is mounted by a cantilever beam in single axis rotating table, mechanical vibration of rotating motion may affect the accuracy of the estimated pitch and yaw angle. As it is visible, the estimates of pitch and yaw angle based on UKF and AUKF can follow the reference attitude angle with the acceptable error. The errors for pitch and yaw angle are within 0.79° and 0.8° in UKF algorithm. The errors for pitch and yaw angle with adaptive estimation are within 0.39° and 0.43° in AUKF-RBF algorithm. During the intervals of 10–20 s and 20–30 s, the estimated yaw angle greatly changed in the opposite direction at sample time t = 8.3 s, t = 25 s, and t = 30 s. The estimated yaw angle is affected by large sideslip angle in Figures 10(e) and 10(g). Correspondingly, the estimated errors come out at these sampling times in Figure 10(f). These errors increase by an average of 0.5° as compared with the errors in the steady section. In Figure 10(h), the yaw angle is estimated depending on RBF feedback and tuning of adaptive scale factor in AUKF, and the fluctuation of error decreased faster in the GPS outage.The convergence of RBF in UKF and AUKF is shown in Figure11.Figure 11
The convergence process of RBF-ANN.As can be seen from above, the training time in AUKF algorithm takes about 8.25 s. Compared with UKF algorithm, the costing time of training process takes about 13.25 s. Obviously, in the process of training RBF network, it spends less time than UKF algorithm.The attitude angles are estimated in both UKF and AUKF-RBF algorithms. The AUKF-RBF algorithm has better estimation precision than UKF during the GPS outage section. RBF correction algorithm is regarded to be reliable and can be utilized by AUKF. The estimated precision of RBF depends on the accuracy of training data. The estimated attitude angles in AUKF-RBF algorithm provided more reliable results as training samples than UKF algorithm during the GPS nonoutage stage. Compared with the performance of GPS normal working sections (i.e., 10–20 s) and GPS outage section (i.e., 40–50 s), the RMS of the proposed AUKF-RBF is shown in Table2.Table 2
Contrast of mean square error of GPS failure and GPS normal.
RollPitchYawUKF-GPS outage2.00480.98820.8042UKF-GPS normal1.97400.66330.5834AUKF-RBF- GPS outage0.41850.38160.3658It can also be found that the performance degrades during the GPS outage. The estimation error in GPS outage section is larger than the error in GPS normal working section. The reason is that the reliable measurements are provided by GPS and accelerometer in the GPS normal working section. On the contrary, the measurements were estimated by RBF in GPS outage. The corresponding programs of the proposed algorithm is compiled and run on CCS 3.3 environment for the DSP controller in real time. The calculating time takes 8.96 ms in a sampling period. For brevity, we will concentrate on comparing the performances of proposed in the whole stage. The AUKF-RBF algorithm can not only provide accurate estimation GPS normal working section but also guarantee reliable estimation in the GPS-denied section. The attitude angle estimation error statistics of various methods in testing experiment are summarized in Table3.Table 3
Mean square error of the attitude angle in the whole stage.
RollPitchYawUKF1.98570.79130.8049AUKF-RBF0.42120.38790.4254Compared with performance of the abovementioned methods, the proposed AUKF-RBF method provides more accurate and reliable estimation whether in normal working condition or GPS failure. It has good estimation performance in the whole section, which suffers from vibration noises due to inevitable disturbances and high rotating motion. The MSE of roll, pitch, and yaw based on the proposed method is 0.4212°, 0.3879°, and 0.4254°, respectively.


## 5. Conclusions

The attitude determination method combining AUKF with RBF-ANN is proposed. Considering the situations in low dynamic and high dynamic environment, the different control strategies fusing the MEMS multisensors are adopted. Using the adaptive scale factor, the influence on the new measurement data is weakened or enhanced during the process of the estimation in AUKF. The attitude algorithm based on RBF-ANN feedback correction can continue to provide reliable estimated deviation information in GPS outage. A portable experiment system to simulate high rotating motion of spinning projectile simulate is setup. The performance of the proposed method has been evaluated and verified through testing experiments for comparing with other representative methods for attitude estimation. Experimental results indicate that the proposed method exhibits reliable and satisfactory performance under various dynamic conditions, as well as GPS-friendly or temporarily GPS-denied conditions. The calculating time of the proposed method in DSP controller takes 8.96 ms in a sampling period. The proposed method has good estimation performance in the whole section The MSE of roll, pitch, and yaw based on the proposed method is 0.4212°, 0.3879°, and 0.4254°, respectively.



---

*Source: 1638678-2020-07-31.xml*