# Compounded Calibration Based on FNN and Attitude Estimation Method Using Intelligent Filtering for Low Cost MEMS Sensor Application

Authors: Lei Wang; Ying Guan; Xuedong Hu
Journal: Mathematical Problems in Engineering
 (2019)

---

## Abstract

Micro electro mechanical system (MEMS) inertial sensors have advantages, including small size and low power consumption. The performances of Micro Inertial measurement unit (IMU), which is composed of MEMS inertial sensors, degrade, and error, will become larger in high dynamic environment. In order to solve the problem, a novel combined calibration method for compensating the deterministic error of MEMS sensors is proposed. Considering the rotation of different sensitive axes in high dynamic and low dynamic environment, the compounded calibration based on fuzzy neural network (FNN) is adopted to identify the coupling coefficients to eliminate the adverse coupling effects between different rotation axes. Furthermore, the self-developed Micro IMU and magnetometer are applied in attitude estimation system. Considering the large attitude error occurred in most cases, the approach utilizing the estimation of error quaternion vector could avoid the calculation error due to inaccurate modeling in the skew symmetric matrix that comprises attitude error vector components. The intelligent Kalman filter (IKF) based on complexity state equation of error quaternion is designed to improve the performance by adjusting the parameters of filter on line. The experimental results show that the proposed approach could have a higher level of stability and accuracy in comparison to other attitude estimation algorithms.


---

## Body

## 1. Introduction

According to the embedded navigation application of MEMS inertial sensor and Micro IMU in high dynamic environment, the performance and precision of MEMS inertial sensor and Micro IMU are improved based on error compensation methods [1]. Among them, most calibration methods are developed to eliminate the adverse effects of the deterministic and systematic errors [2, 3]. For example, a coarse estimation and postparameter estimation calibration methods are proposed. The effectiveness of the calibration method was evaluated by estimating heading and inclination using the calibrated IMU [1]. For low cost micro IMU, an easy self-calibration method is proposed utilizing the accelerometers as level datum for calibrating gyros [3]. Moreover, attitude estimation approaches with sensor fusion algorithms on low cost MEMS gyroscope, accelerometer, and magnetometer and satellite receive system (GPS) are discussed.For triaxial magnetometers, a two-step calibration algorithm based on functional link artificial neural network and least squares is reported. The coefficients could be identified according to optimal weights [4]. The attitude estimation based on gyros is constrained to time varying bias drift and noise [5]. According to the principle of accelerometer measuring gravity, the accelerometer could be used for calculating the pitch and roll angles precisely in stationary or low dynamic condition [6]. Magnetometer sensor, which is manufactured by making use of alloy resistance having sensitive response to direction magnetic field, could be used for calculating the yaw angle in AHRS or estimating the ballistic roll in the projectile [7, 8].The multisensor fusion methodology for attitude estimation has been developed over past decade, integrating low cost of micro IMU, global navigation satellite system (GNSS, which consists of the GPS, GLONASS, Galileo or BeiDou system, is used in many navigation applications.) and the other sensors. Corresponding methods have been widely used in autonomous unmanned aerial vehicle (UAV) system [9], autonomous underwater vehicles (AUV) [10], gun launched munitions [11], etc.The researches based on Kalman filter and data fusion method have been carried out for estimating the attitude with the use of GNSS, Micro IMU, magnetometers, and other sensors. Kannan proposed a quaternion-based sensor fusion approach for estimating orientation based on accelerometer, magnetometer, and gyroscope sensors [12]. The practical application of the adaptive high-gain extended Kalman filter onboard a quadcopter UAV was demonstrated in [13]. The high-gain parameter of filter was tuned according to a metric called innovation. An attitude algorithm that combines Kalman filter and Artificial neural network (ANN) for magnetic and IMU is proposed. ANN is used to establish the functional relationship [14].In calibration process, the difficult issue is that the coupling parameters in IMU are rarely estimated precisely in high dynamic measurement environment. Besides, the challenge of estimating the attitude lies in solving the strong model nonlinearity, state disturbances, and the uncertainty of Micro IMU noise [15, 16]. The traditional extended Kalman filter for nonlinear system largely relies on prior information for the uncertainty of measurement and process noise [17, 18]. Since the statistical properties of process and measurement noise cannot be predicted accurately, it would cause the inaccurate estimation or even divergence, especially for low cost MEMS-based sensors. Some complicated nonlinear filter and adaptive control methods could be used for optimal estimation for such nonlinear system [19]. On the other hand, the main drawback is that introducing a heavy computational burden leads to difficulty of application in real-time navigation system. The novel aspects of this paper can be summarized as follows.( 1 ) For the application of projectile/spinning spacecraft, a compounded calibration for Micro IMU in high dynamic environment is proposed. Considering the case that the rotation from one of the sensitive axis is in high dynamic, the rotations of rest sensitive axes were in low dynamic environment and the bias and scale factor are estimated by the least square algorithm in initial calibration. Furthermore, the coupling parameters are estimated through compounded calibration based on FNN. The coupling effect could be eliminated in performing the experimental calibration for simulating the different dynamic environments.( 2 ) Considering the large attitude error between the true navigation frame and the estimated navigation frame, an attitude estimation method utilizing the error quaternion vector for low cost Micro IMU and magnetometer is developed. The approach utilizing the estimation of error quaternion vector could overcome the drawback of the calculation error due to inaccurate modeling in the skew symmetric matrix that is composed of attitude error vector components.( 3 ) To utilize the strengths of the error quaternion vector algorithm and adaptive filter approaches, the fuzzy controller in intelligent Kalman filter is designed to tune the adaptive gain scale for compensating the uncertainty of measurement and process noise. The effect of faster convergence could be realized when the adaptive Kalman filter is in steady state from transition process. The proposed methodology could significantly improve the attitude estimation performance.The paper is organized as follows. Section2 gives the model of MEMS sensors and compounded method of low cost Micro IMU. Moreover, it demonstrates the error Quaternion vector method based on IKF for attitude estimation. Experimental results and discussion are stated in Section 3.


## 2. Methodology

### 2.1. Design of Micro Navigation System

Micro navigation system is limited by size and cost. Correspondingly the micro IMU is designed as inertial measurement device-based attitude control for the applications in some military field, such as projectile and spinning shell. The micro navigation system comprises the DSP (Digital Signal Processing)/FPGA (Field Programmable Gate Array) prototype, low cost micro IMU, and magnetometer. The DSP/FPGA prototype is designed to six-layer PCB, which comprises FPGA controller, DSP controller, flash, SDRAM, power module, I/O interface circuit, etc. The micro IMU is designed to four-layer PCB for integration of three single axial gyros, three single axial accelerometers, and the triaxial magnetometer. The size of micro navigation system isφ80×30mm. It is shown in Figure 1.Figure 1
The prototype of micro navigation system.
(a)
FPGA (top side) (b)
DSP(bottom side) (c)
DSP/FPGA controller and Micro IMUThe communication infrastructure of FPGA and DSP microcontroller is based on FIFO structure. The FIFO module in FPGA was applied to realize data saving and taking. The processed data after the filters were written in the buffer cache, which are used for latter calculation in DSP microcontroller. DSP microcontroller accesses the data from EDMA (Enhanced Direct Memory Access) channel after acquiring the send requirement from FPGA. The output analog signals of gyro, accelerometer, and magnetometer are simultaneously sampled by an 8 channel 16-bit analog-to-digital converter. Maximum Operating Frequency of FPGA is 260MHz. The clock frequency of DSP is 300MHz. Considering the real time implementation, the proposed algorithm would be migrated from Matlab to C code. The code could be compiled in QuartusII 11 for FPGA, in CCS3.3 environment and loaded into a DSP micro-controller for real time operation. The system framework of DSP/FPGA controller is shown in Figure2.Figure 2
The system framework of DSP/FPGA controller.


### 2.2. Models of Inertial and Magnetic Sensors

The application of MEMS inertial sensors is restricted of low precision. The bias instability, scale factor and misalignment error are dominant errors in MEMS sensors. It is essential to test, calibrate and compensate for these sensors. For gyro, the model is presented in(1)ωb~=S∙ωb+b0whereωb~ is the raw measurement angular rate of the gyroscope; ωb is the true angular rate; b0 is the output constant bias. Here, the matrix S is usually modeled by a 3×3 matrix.(2)S=sxmxymxzmyxsymyzmzxmzyszThe main diagonal vectorsi(i=x,y,z) is scale factors of sensitive axis. The parameter mjk(jk=xy,xz,yx,yz,zx,zy) represents the misalignment and coupling coefficients.The accelerometer’s measurement expressed in the body coordinate frame of IMU could be written as(3)fb~=Mbfb+baand the vector ba is the bias of accelerometer. The matrix Mb represents scale factor and misalignment error of accelerometer. It is given by(4)Mb=SaxδKz-δKy-δKzSayδKxδKy-δKxSazConsidering the hard iron and soft iron effects of magnetometer sensor, the model of magnetometer is represented by(5)mb~=Cemb+εThe vectorCe represents the effects of the scale factor error, nonorthogonality, and the soft-iron. The matrix mb~ represents the raw measurement in body frame. The matrix mb represents the calibrated magnetometer output.


### 2.3. Compounded Calibration of Low Cost Micro IMU

MEMS inertial sensors are used widely because of low cost, small size, and low power consumption. It is also restricted of low precision. The effective solution to improve the accuracy performance is not only depending on the traditional design principle and structure, signal modulate circuit, but also calibration and error compensation of inertial models. The systematic error sources for MEMS inertial sensors mainly embody in biases, scale factors, nonorthogonality, or misalignment errors.The application of these sensors is restricted to low precision and bias instability, which represents dominant navigation systematic error [3]. It is essential to test, calibrate, and compensate for these sensors. A two-step calibration method is implemented based on initial calibration and compounded calibration. In initial calibration, the dynamic bias and scale factor would be estimated using static and angular rate method by single rotation table or tri-axis rotation table [20].The precision of micro IMU could be influenced due to the coupling effects among the sensitive axis, especially in high dynamic environment. Considering the case that the rotation from one of the sensitive axis is in high dynamic, the rotations of rest sensitive axes were in low dynamic and the coupling coefficients of model using compounded calibration experiments are estimated by FNN. The following procedures are demonstrated.Step 1.
The biasba, scale factor si(i=x,y,z), and the misalignment mjk were estimated in initial calibration process using the least square algorithm.Step 2.
The experiments of different ballistic attitudes for simulating projectile/spinning spacecraft are implemented in triaxial test table.
The open system based on “IPC(Industrial PC)+Multi-axes motion controller” is used for motion control of the triaxial table. The triaxial table setting data used as simulating ballistic attitude variations are stored in IPC. In order to simulate the attitude of projectile/spinning spacecraft, The inner-axis as simulating the change of roll angle of projectile/spinning spacecraft is within -900°/s to +900°/s. The middle-axis as simulating the pitch angle of projectile/spinning spacecraft is set in the range of -60 degree to +60 degree. The outer-axis as simulating the yaw angle is in the range of +0.1 degree to +1degree.Step 3.
Collecting the data of customized Micro IMU, the reference (1) to calculate the preprocessing data ω^b for initial calibration process is given.(6)ω^b=S^∙ω1xbω1ybω1zb
whereS^=s^xm^xym^xzm^yxs^ym^yzm^zxm^zys^z, ω^b=ω~xb-b0xω~yb-b0yω~zb-b0z
The main diagonal vectorss^i(i=x,y,z) and m^jk(jk=xy,xz,yx,yz,zx,zy) are estimated in initial calibration process. The compensated data ω1b for initial calibration process are calculated in (7).(7)ω1b=S^-1∙ω^b
whereω1b=ω1xbω1ybω1zb.Step 4.
Xsens Mti-G is ones of representative products in navigation field. The gyro bias stability is only10°/h(0.0027°/s). One side, the calibrated inertial sensor data from Mti-G, which is composed of three-axis MEMS gyro, accelerometer, and magnetometer, is provided for reference sensor data. The raw gyro data from customized Micro IMU would be calibrated in comparison with Mti-G’ reference sensor data. It is defined as ωrb in (8).(8)ωrb=S^-1+∆S∙ω^b
whereωrb=ωrxbωrybωrzb.
The adjustment value matrix∆S is imported into the corrected (8) directly in the compounded calibration process. It is defined as in (9)∆S=∆m11∆m12∆m13∆m21∆m22∆m23∆m31∆m32∆m33
According to (7) and (8), the relationship is derived in (10)∆ωb=∆S∙ω^b
where∆ωb=ωrb-ω1b=ωrxb-ω1xbω1yb-ω1ybωrzb-ω1zb=∆ωxb∆ωyb∆ωzb.
The estimated adjustment value∆ωb in compounded process is used for compensating the coupling effect of different rotating axis in high dynamic environment. Correspondingly, the adjustment value matrix ∆S is estimated in (11)∆S=L∆ωb,ω^b
L ( ∙ ) represents the operation function. For the experiments of different ballistic attitudes for simulating projectile/spinning spacecraft, the measurement output data ω~zb for simulating rotation of high dynamic axisZ is far greater than the low dynamic values ω~xb, ω~yb forX-axis andY axis. The small coupling terms due to the weakly effects of the low dynamic values ω~xb and ω~yb are neglected in actual applications. The large coupling terms in relation to the high dynamic data ω~zb are preserved. Therefore, the adjustment value matrix ∆S is simplified as (12)∆S≈∆m1303×2∆m23∆m33
The dominant coupling parameters∆m13,∆m23,∆m33 in (12) are estimated based on FNN in the compounded process.Step 5.
The FNN is three layers feed-forward network. The input of ANN is the triaxial raw gyro dataωb~=[ω~xb,ω~ybω~zb]T, and the output of FNN is the coupling coefficients ∆S=L(∆ωb,ω^b).Step 6.
The different adjustment values∆S^ would be adopted for wide range of dynamic environment. The three phases, which comprise the launch stage, the steady guidance stage, and falling stage of missile, are classified based on different simulating attitudes data.
C a s e   1. For the launch stage, the roll angle changed from low dynamic to high dynamic. The pitch angle would be stable in constant launch angle.
C a s e   2. For the steady guidance stage, the roll angle is stable in high dynamic. The pitch angle changed in the range of +60 degree to -60 degree in low dynamic.
C a s e   3. For the falling stage of missile, the roll angle is decreased in high dynamic. The pitch angle changed in low dynamic.The compensated dataω1b for initial calibration process, which could be reflected from the change of roll and pitch angle, are used for above mentioned different cases. The different adjustment values ∆S^ are set owing to the data classified algorithm. Correspondingly, the fuzzy rules are designed according to the compensated data ω1b and adjustment values ∆S^ in FNN. The fuzzy associative memory laws could be transformed to neural net learning specimen copy. Through artificial neural network learning calculation procedure of FNN, these regulation specimen copies are brought in memory.Step 7.
The trained data are originated from the simulating data of experiments of different ballistic attitudes for simulating projectile/spinning spacecraft in triaxial table. The connection weights through the back propagation update rule are gradually adjusted in the process of offline training.Step 8.
The output data of FNN∆S^ through an online calculation of FNN is afforded. The estimated adjustment value ∆ωb is calculated and compensated for initial calibration results.
The flow diagram of compounded calibration method based on FNN is presented in Figure3.
The experiment of different ballistic attitudes for simulating projectile/spinning spacecraft is shown in Figure4.Figure 3
The flow diagram of compounded calibration method based on FNN.Figure 4
The compounded calibration experiment.One of simulating data for testing experiments is chosen as training samples. The pitch angle changed in the range of -40° to -25° in low dynamic. The rate of roll angle changed from 0 degree/s to 900°/s in high dynamic. According to the simulating data, the dominant coupling parameters ∆m13,∆m23,∆m33 are estimated in (12). They are shown, respectively, in Figure 5.Figure 5
The dominant coupling parameters.It is seen that the results of estimated coupling parameters are chosen according to the change of the pitch and roll angle. In order to verify the effect of the compounded algorithm, the three setting values of roll rate are360°/s, 500°/s, and 900°/s for simulating the high dynamic revolving. Meanwhile, the subsequent variations in pitch angle are set from -40° to -25°. In testing experiments, the rate of pitch angle is 13°/s for simulating the low dynamic action. In comparison with the three coupling parameters in Figure 5, the dominant coupling parameters are insensitive to the change of pitch angle. The magnitude of coupling parameters is inversely proportional to the setting value of roll angles. For example, the STD of ∆m13 is 0.017, 0.012, and 0.007, respectively, and for setting values of roll rate is 360°/s, 500°/s, and 900°/s.In order to verify the effect of compounded calibration, one set of data of simulating ballistic attitude is set for testing experiment. In experiments, inner-axis of three axis motorized turntable is motived in high dynamic, and the middle-axis is motived in low dynamic. The estimated coupling parameters by FNN are used for compensating for the effect of rotating axis and nonrotating axis. The coupling effect would be eliminated depending on the adjustment value matrix in (11). The compensated results are shown in Figure 6. Compared with results between initial calibration and compounded calibration, the compensated accuracy is improved significantly. For the comparison of error of revolving and nonrevolving axis, the compensated result for revolving axis is improved significantly. The RMS errors of resolving and non-resolving axis are within 0.44°, 0.63°, and 0.55° in high rotating testing experiment of simulating ballistic attitude. It is shown in Table 1.Table 1
The RMS and STD in initial, compounded calibration high rotating testing experiment.
error initial calibration compounded calibration Resolving Axis Z STD 6.0514 0.4377 RMS 6.2931 0.4373  non-resolving axis Y STD 1.3522 0.6330 RMS 1.3509 0.6324  non-resolving axis X STD 0.7137 0.5515 RMS 0.7390 0.5509Figure 6
The compensated results in testing experiment.It is shown that the better compensated result is obtained after compensating for coupling coefficients in high dynamic rotation, compared with the initial calibration method. The adverse coupling effects between high dynamic rotation and low dynamic rotation axis of gyro are removed through compounded calibration. The compensated accuracy in high dynamic rotation is improved.The preprocessed data of micro IMU after the compounded calibration and compensation procedure could be used for attitude determination, especially in high dynamic environment. The optimal bias of gyro is estimated in the initial calibration process, which would be provided to IKF as initial process. The sensor characteristics of stochastic errors in low MEMS gyro, accelerometer, and magnetometer could be identified using Allan variance. The root mean square random drift errors of Allan variance are calculated, which would be provided to IKF as initial measurement noises.


### 2.4. The Attitude Angle Estimation Method

#### 2.4.1. State Model Based on Error Quaternion

The transformation matrix from navigation framen to body fixed frameb is based on the three Euler angles, namely, pitch, roll, and yaw, denoted by the symbols θ, ϕ, and ψ. The estimated direction cosine matrix attitude matrix Cbn′ from actual navigation frame n′ and body frameb by three Euler angles. The transformation matrix Cn′n denotes the transformation relationship between true navigation framen and estimated navigation frame n′. The attitude errors ∅e,∅n,and ∅u are given as small error components considering that high order terms are neglected. (13)Cn′n=I+ϕ×whereϕ× is the skew symmetric matrix, ϕ×=0-ϕuϕnϕu0-ϕe-ϕnϕe0 andI is the identity matrix. The relationship is described for the true strapdown matrix Cbn relative to the estimated attitude matrix Cbn′.(14)Cbn=Cn′n∙Cbn′=I+ϕ×·Cbn′If the attitude errors components∅e, ∅n, and ∅u are chosen as state variables in filter, the skew symmetric matrix ϕ× is updated by the calculation due to the estimated state variables. The more accurate strapdown matrix Cbn would be corrected to improve the precision of attitude in (14). The actual transformation matrix. Cn′n, which denotes the attitude error between the true strapdown matrix Cbn and the estimated attitude matrix Cbn′, could not be model as skew symmetric matrix, especially the attitude error is larger in actual navigation system. In order to solve it, the error quaternion vector of attitude δq is given to description of the transformation relationship of nominal navigation framen relative to the actual navigation frame n′ to avoid the calculation error due to inaccurate modeling in the skew symmetric matrix Cn′n that consists attitude error vector ∅e,∅n,and ∅u.The true quaternion vector of attitude is defined as vectorq, corresponding the estimated quaternion vector is q^. According to the equivalence relation of attitude matrix and attitude quaternion, the expression Cbn′=Cnn′Cbn in reference of (14) is transformed as(15)q^=δq∗⊗qHere,δq is the error quaternion vector δq=δq0δq1δq2δq3. The conjugate of the error quaternion is defined as vector δq∗. The operator “⊗” denotes the quaternion multiplication. The error quaternion vector δq and the output error of gyroscope bt are chosen as state vector considering the effect of the model error of attitude errors components.(16)X=δqbt7×1TThe bias of gyroscopebt=btxbtybtz, which is the bias drift in three sensitive axisx,y, andz. The nonlinear differential equations of the true and actual quaternion vector are expressed as(17)q˙=12q⨂ωibb-ωinb(18)q^˙=12q^⨂ω^ibb-ω^inbwhereω^ibb is the actual measurement angular rate of the gyroscope. ωibb is the true angular rate. The vector ωinb=Cnbωinn, ωinn=ωien+ωenn. The vector ωien is the earth rate in navigation frame. The vector ωenn is the turn rate of the navigation frame with respect to the velocity of the craft. According to the equivalence transformation in (15), the expression is derived as(19)δq=q⊗q^∗Accordingly,q=δq⊗q^∗ is also deduced. The derivative of the equation on both sides is described in (17), (18), and (19)(20)δq˙=q˙⊗q^∗+q⊗q^˙∗According to (20), the derivative of error quaternion vector could be expressed(21)δq˙=-12δωibn⊗δq-12ωinn⊗δq+12δq⊗ω^innIn actual strapdown inertial navigation system, the small earth rate componentωien could be ignored. Furthermore, the turn rate of the navigation frame ωenn would be substituted for estimated value ω^enn. The output error of gyro δωibb would be would be substituted for the constant bias b0, which is estimated in full range as the deterministic error via calibration experiment offline.Corresponding, the state equation is given by(22)X˙7×1=fδq,bt=-12A4×404×303×4C3×3·X7×1+W7×1Here(23)A=0-b0x-b0y-b0zb0x0-2ω^enzn-b0z2ω^enyn+b0yb0y2ω^enzn+b0z0-2ω^enxn-b0xb0z-2ω^enyn-b0y2ω^enxn+b0x0C=-1β∗100010001W7×1=01×4ωrxωryωrzTwhereβ is the time constant of a first-order Gauss-Markov process and ωr is Gaussian white process. In discrete time, the state equation could be rearranged.


#### 2.4.2. System Measurement Model

The measurement model is constructed by choosing the projection vectors of gravitational acceleration and magnetometer’s vectors in actual navigation system.(24)Zk=fgn′mn′The measurement vector(25)Zk=Cnn′00Cnn′gnmn+∇n′The attitude error matrixCnn′ is expressed in terms of error quaternion. ∇n′=∇gn′∇mn′T is the output error of accelerometer and magnetometer in the actual navigation system.(26)Cnn′=δq12+δq02-δq32-δq222δq1δq2+δq0δq32δq1δq3-δq0δq22δq1δq2-δq0δq3δq22-δq32+δq02-δq122δq2δq3+δq0δq12δq1δq3+δq0δq22δq2δq3-δq0δq1δq32-δq22-δq12+δq02


#### 2.4.3. The Adaptive Gain Scale in Intelligent Kalman Filter

Owing to the high and time varying noise characters of low cost Micro IMU, the traditional extended Kalman filter (EKF) could not predict the statistical properties of system and measurement noise accuracy [17]. On the other hand, some complicated nonlinear filters impose a heavy burden on the small scaled microcontroller. To resolve the problem of robust control and characters of the uncertain noise statistics influence, an intelligent Kalman filter (IKF) of attitude estimation method is designed. The IKF could adjust the filter parameters online to the changes of both dynamic and observation noise via adaptive gain scale. The priori error covariance matrix of IKF is (27)Pk,k-1=Φk,k-1Pk-1Φk,k-1T+λk∙LkQk-1LkThe gain of IKF is presented(28)Kk=Pk,k-1HkTHkPk,k-1HkT+λk-1∙okRkok-1The matricesLk and ok are diagonal. The variable λk is the adaptive gain scale (1≤λk≤λmax). The innovation function is defined for evaluating the measurement error.(29)Bτ,k=∑i=nkti+1-tizti-z~ti2(30)Dτ,k=Bτ,k-BthresholdDefineBτ,k is the cost function, and τ is the calculation length of moving window. The parameter n is the first sampling time. Dτ,k is the difference value between the cost function and threshold value at sampling timek, which demonstrates the error of observation. The variable λk is the adaptive gain scale; its scope is within 1 and λmax.(31)λk=RfuzzyDτ,kThe following analytical solution algorithm for adjusting the adaptive gain scale is proposed. Kalman filter error does not depend on the selection of process noise variance or measurement noise statistics absolute levels but on their relationship [21]. The estimation process of the filter comprises two stages. One is a transition process of the estimation, the other one is a steady state. To solve the drawbacks of traditional extended Kalman filter applied in nonlinear systems highly depending on measurement and process noise variance values, the fuzzy rules to tune the adaptive gain scale are constructed in transition process and steady state. The initial process and measurement noises covariance matrix could be identified using Allan variance [22] or PSD method [23].In the transition process of filter, the true behavior of some state vector components is usually unknown. Therefore, the larger variationDτ,k would occur in the first stage of filter. In this case, larger values of process noise variance should be chosen in the transition process. Meanwhile, the weight of measurements λk-1 in estimation process has to be decreased for the sake of reducing the influence of measurement noise on current estimates. Therefore, the adaptive gain scale λk should tune its initial value to maximum value with the increasing of variation Dτ,k in the first stage of filter. As a result, all estimates of state vector components could converge to true values faster.With the selection of reasonable adaptive gain scale, the filter would provide the estimates with constant accuracy from steady state to transition process. The valueDτ,k would be decreased to a small value gradually with the estimation process. On that time, the smaller adaptive gain scale λk would be chosen. The weight of measurements would be increased with the effective estimation.According to the above analysis, a fuzzy controller with one-dimension is implemented in filter. The increasing of variationDτ,k and adaptive gain scale λk are input and output of the fuzzy controller, respectively. The corresponding fuzzy rules to tune the adaptive gain scale would be produced. The fuzzy controller could obtain the outputs in real time from query rule table online, which is generated according to rules offline. The structure of the system is shown in Figure 7. The dominant errors of micro IMU were compensated by initial and compounded calibration method. In attitude estimation method, the processed data of three axis gyro is corrected by eliminating the bias error from IKF. The estimated error quaternion is used for calculation of the transformation matrix. Then, the estimated attitude matrix is also corrected eventually.Figure 7
The algorithm structure for compounded calibration and attitude estimation.


## 2.1. Design of Micro Navigation System

Micro navigation system is limited by size and cost. Correspondingly the micro IMU is designed as inertial measurement device-based attitude control for the applications in some military field, such as projectile and spinning shell. The micro navigation system comprises the DSP (Digital Signal Processing)/FPGA (Field Programmable Gate Array) prototype, low cost micro IMU, and magnetometer. The DSP/FPGA prototype is designed to six-layer PCB, which comprises FPGA controller, DSP controller, flash, SDRAM, power module, I/O interface circuit, etc. The micro IMU is designed to four-layer PCB for integration of three single axial gyros, three single axial accelerometers, and the triaxial magnetometer. The size of micro navigation system isφ80×30mm. It is shown in Figure 1.Figure 1
The prototype of micro navigation system.
(a)
FPGA (top side) (b)
DSP(bottom side) (c)
DSP/FPGA controller and Micro IMUThe communication infrastructure of FPGA and DSP microcontroller is based on FIFO structure. The FIFO module in FPGA was applied to realize data saving and taking. The processed data after the filters were written in the buffer cache, which are used for latter calculation in DSP microcontroller. DSP microcontroller accesses the data from EDMA (Enhanced Direct Memory Access) channel after acquiring the send requirement from FPGA. The output analog signals of gyro, accelerometer, and magnetometer are simultaneously sampled by an 8 channel 16-bit analog-to-digital converter. Maximum Operating Frequency of FPGA is 260MHz. The clock frequency of DSP is 300MHz. Considering the real time implementation, the proposed algorithm would be migrated from Matlab to C code. The code could be compiled in QuartusII 11 for FPGA, in CCS3.3 environment and loaded into a DSP micro-controller for real time operation. The system framework of DSP/FPGA controller is shown in Figure2.Figure 2
The system framework of DSP/FPGA controller.


## 2.2. Models of Inertial and Magnetic Sensors

The application of MEMS inertial sensors is restricted of low precision. The bias instability, scale factor and misalignment error are dominant errors in MEMS sensors. It is essential to test, calibrate and compensate for these sensors. For gyro, the model is presented in(1)ωb~=S∙ωb+b0whereωb~ is the raw measurement angular rate of the gyroscope; ωb is the true angular rate; b0 is the output constant bias. Here, the matrix S is usually modeled by a 3×3 matrix.(2)S=sxmxymxzmyxsymyzmzxmzyszThe main diagonal vectorsi(i=x,y,z) is scale factors of sensitive axis. The parameter mjk(jk=xy,xz,yx,yz,zx,zy) represents the misalignment and coupling coefficients.The accelerometer’s measurement expressed in the body coordinate frame of IMU could be written as(3)fb~=Mbfb+baand the vector ba is the bias of accelerometer. The matrix Mb represents scale factor and misalignment error of accelerometer. It is given by(4)Mb=SaxδKz-δKy-δKzSayδKxδKy-δKxSazConsidering the hard iron and soft iron effects of magnetometer sensor, the model of magnetometer is represented by(5)mb~=Cemb+εThe vectorCe represents the effects of the scale factor error, nonorthogonality, and the soft-iron. The matrix mb~ represents the raw measurement in body frame. The matrix mb represents the calibrated magnetometer output.


## 2.3. Compounded Calibration of Low Cost Micro IMU

MEMS inertial sensors are used widely because of low cost, small size, and low power consumption. It is also restricted of low precision. The effective solution to improve the accuracy performance is not only depending on the traditional design principle and structure, signal modulate circuit, but also calibration and error compensation of inertial models. The systematic error sources for MEMS inertial sensors mainly embody in biases, scale factors, nonorthogonality, or misalignment errors.The application of these sensors is restricted to low precision and bias instability, which represents dominant navigation systematic error [3]. It is essential to test, calibrate, and compensate for these sensors. A two-step calibration method is implemented based on initial calibration and compounded calibration. In initial calibration, the dynamic bias and scale factor would be estimated using static and angular rate method by single rotation table or tri-axis rotation table [20].The precision of micro IMU could be influenced due to the coupling effects among the sensitive axis, especially in high dynamic environment. Considering the case that the rotation from one of the sensitive axis is in high dynamic, the rotations of rest sensitive axes were in low dynamic and the coupling coefficients of model using compounded calibration experiments are estimated by FNN. The following procedures are demonstrated.Step 1.
The biasba, scale factor si(i=x,y,z), and the misalignment mjk were estimated in initial calibration process using the least square algorithm.Step 2.
The experiments of different ballistic attitudes for simulating projectile/spinning spacecraft are implemented in triaxial test table.
The open system based on “IPC(Industrial PC)+Multi-axes motion controller” is used for motion control of the triaxial table. The triaxial table setting data used as simulating ballistic attitude variations are stored in IPC. In order to simulate the attitude of projectile/spinning spacecraft, The inner-axis as simulating the change of roll angle of projectile/spinning spacecraft is within -900°/s to +900°/s. The middle-axis as simulating the pitch angle of projectile/spinning spacecraft is set in the range of -60 degree to +60 degree. The outer-axis as simulating the yaw angle is in the range of +0.1 degree to +1degree.Step 3.
Collecting the data of customized Micro IMU, the reference (1) to calculate the preprocessing data ω^b for initial calibration process is given.(6)ω^b=S^∙ω1xbω1ybω1zb
whereS^=s^xm^xym^xzm^yxs^ym^yzm^zxm^zys^z, ω^b=ω~xb-b0xω~yb-b0yω~zb-b0z
The main diagonal vectorss^i(i=x,y,z) and m^jk(jk=xy,xz,yx,yz,zx,zy) are estimated in initial calibration process. The compensated data ω1b for initial calibration process are calculated in (7).(7)ω1b=S^-1∙ω^b
whereω1b=ω1xbω1ybω1zb.Step 4.
Xsens Mti-G is ones of representative products in navigation field. The gyro bias stability is only10°/h(0.0027°/s). One side, the calibrated inertial sensor data from Mti-G, which is composed of three-axis MEMS gyro, accelerometer, and magnetometer, is provided for reference sensor data. The raw gyro data from customized Micro IMU would be calibrated in comparison with Mti-G’ reference sensor data. It is defined as ωrb in (8).(8)ωrb=S^-1+∆S∙ω^b
whereωrb=ωrxbωrybωrzb.
The adjustment value matrix∆S is imported into the corrected (8) directly in the compounded calibration process. It is defined as in (9)∆S=∆m11∆m12∆m13∆m21∆m22∆m23∆m31∆m32∆m33
According to (7) and (8), the relationship is derived in (10)∆ωb=∆S∙ω^b
where∆ωb=ωrb-ω1b=ωrxb-ω1xbω1yb-ω1ybωrzb-ω1zb=∆ωxb∆ωyb∆ωzb.
The estimated adjustment value∆ωb in compounded process is used for compensating the coupling effect of different rotating axis in high dynamic environment. Correspondingly, the adjustment value matrix ∆S is estimated in (11)∆S=L∆ωb,ω^b
L ( ∙ ) represents the operation function. For the experiments of different ballistic attitudes for simulating projectile/spinning spacecraft, the measurement output data ω~zb for simulating rotation of high dynamic axisZ is far greater than the low dynamic values ω~xb, ω~yb forX-axis andY axis. The small coupling terms due to the weakly effects of the low dynamic values ω~xb and ω~yb are neglected in actual applications. The large coupling terms in relation to the high dynamic data ω~zb are preserved. Therefore, the adjustment value matrix ∆S is simplified as (12)∆S≈∆m1303×2∆m23∆m33
The dominant coupling parameters∆m13,∆m23,∆m33 in (12) are estimated based on FNN in the compounded process.Step 5.
The FNN is three layers feed-forward network. The input of ANN is the triaxial raw gyro dataωb~=[ω~xb,ω~ybω~zb]T, and the output of FNN is the coupling coefficients ∆S=L(∆ωb,ω^b).Step 6.
The different adjustment values∆S^ would be adopted for wide range of dynamic environment. The three phases, which comprise the launch stage, the steady guidance stage, and falling stage of missile, are classified based on different simulating attitudes data.
C a s e   1. For the launch stage, the roll angle changed from low dynamic to high dynamic. The pitch angle would be stable in constant launch angle.
C a s e   2. For the steady guidance stage, the roll angle is stable in high dynamic. The pitch angle changed in the range of +60 degree to -60 degree in low dynamic.
C a s e   3. For the falling stage of missile, the roll angle is decreased in high dynamic. The pitch angle changed in low dynamic.The compensated dataω1b for initial calibration process, which could be reflected from the change of roll and pitch angle, are used for above mentioned different cases. The different adjustment values ∆S^ are set owing to the data classified algorithm. Correspondingly, the fuzzy rules are designed according to the compensated data ω1b and adjustment values ∆S^ in FNN. The fuzzy associative memory laws could be transformed to neural net learning specimen copy. Through artificial neural network learning calculation procedure of FNN, these regulation specimen copies are brought in memory.Step 7.
The trained data are originated from the simulating data of experiments of different ballistic attitudes for simulating projectile/spinning spacecraft in triaxial table. The connection weights through the back propagation update rule are gradually adjusted in the process of offline training.Step 8.
The output data of FNN∆S^ through an online calculation of FNN is afforded. The estimated adjustment value ∆ωb is calculated and compensated for initial calibration results.
The flow diagram of compounded calibration method based on FNN is presented in Figure3.
The experiment of different ballistic attitudes for simulating projectile/spinning spacecraft is shown in Figure4.Figure 3
The flow diagram of compounded calibration method based on FNN.Figure 4
The compounded calibration experiment.One of simulating data for testing experiments is chosen as training samples. The pitch angle changed in the range of -40° to -25° in low dynamic. The rate of roll angle changed from 0 degree/s to 900°/s in high dynamic. According to the simulating data, the dominant coupling parameters ∆m13,∆m23,∆m33 are estimated in (12). They are shown, respectively, in Figure 5.Figure 5
The dominant coupling parameters.It is seen that the results of estimated coupling parameters are chosen according to the change of the pitch and roll angle. In order to verify the effect of the compounded algorithm, the three setting values of roll rate are360°/s, 500°/s, and 900°/s for simulating the high dynamic revolving. Meanwhile, the subsequent variations in pitch angle are set from -40° to -25°. In testing experiments, the rate of pitch angle is 13°/s for simulating the low dynamic action. In comparison with the three coupling parameters in Figure 5, the dominant coupling parameters are insensitive to the change of pitch angle. The magnitude of coupling parameters is inversely proportional to the setting value of roll angles. For example, the STD of ∆m13 is 0.017, 0.012, and 0.007, respectively, and for setting values of roll rate is 360°/s, 500°/s, and 900°/s.In order to verify the effect of compounded calibration, one set of data of simulating ballistic attitude is set for testing experiment. In experiments, inner-axis of three axis motorized turntable is motived in high dynamic, and the middle-axis is motived in low dynamic. The estimated coupling parameters by FNN are used for compensating for the effect of rotating axis and nonrotating axis. The coupling effect would be eliminated depending on the adjustment value matrix in (11). The compensated results are shown in Figure 6. Compared with results between initial calibration and compounded calibration, the compensated accuracy is improved significantly. For the comparison of error of revolving and nonrevolving axis, the compensated result for revolving axis is improved significantly. The RMS errors of resolving and non-resolving axis are within 0.44°, 0.63°, and 0.55° in high rotating testing experiment of simulating ballistic attitude. It is shown in Table 1.Table 1
The RMS and STD in initial, compounded calibration high rotating testing experiment.
error initial calibration compounded calibration Resolving Axis Z STD 6.0514 0.4377 RMS 6.2931 0.4373  non-resolving axis Y STD 1.3522 0.6330 RMS 1.3509 0.6324  non-resolving axis X STD 0.7137 0.5515 RMS 0.7390 0.5509Figure 6
The compensated results in testing experiment.It is shown that the better compensated result is obtained after compensating for coupling coefficients in high dynamic rotation, compared with the initial calibration method. The adverse coupling effects between high dynamic rotation and low dynamic rotation axis of gyro are removed through compounded calibration. The compensated accuracy in high dynamic rotation is improved.The preprocessed data of micro IMU after the compounded calibration and compensation procedure could be used for attitude determination, especially in high dynamic environment. The optimal bias of gyro is estimated in the initial calibration process, which would be provided to IKF as initial process. The sensor characteristics of stochastic errors in low MEMS gyro, accelerometer, and magnetometer could be identified using Allan variance. The root mean square random drift errors of Allan variance are calculated, which would be provided to IKF as initial measurement noises.


## 2.4. The Attitude Angle Estimation Method

### 2.4.1. State Model Based on Error Quaternion

The transformation matrix from navigation framen to body fixed frameb is based on the three Euler angles, namely, pitch, roll, and yaw, denoted by the symbols θ, ϕ, and ψ. The estimated direction cosine matrix attitude matrix Cbn′ from actual navigation frame n′ and body frameb by three Euler angles. The transformation matrix Cn′n denotes the transformation relationship between true navigation framen and estimated navigation frame n′. The attitude errors ∅e,∅n,and ∅u are given as small error components considering that high order terms are neglected. (13)Cn′n=I+ϕ×whereϕ× is the skew symmetric matrix, ϕ×=0-ϕuϕnϕu0-ϕe-ϕnϕe0 andI is the identity matrix. The relationship is described for the true strapdown matrix Cbn relative to the estimated attitude matrix Cbn′.(14)Cbn=Cn′n∙Cbn′=I+ϕ×·Cbn′If the attitude errors components∅e, ∅n, and ∅u are chosen as state variables in filter, the skew symmetric matrix ϕ× is updated by the calculation due to the estimated state variables. The more accurate strapdown matrix Cbn would be corrected to improve the precision of attitude in (14). The actual transformation matrix. Cn′n, which denotes the attitude error between the true strapdown matrix Cbn and the estimated attitude matrix Cbn′, could not be model as skew symmetric matrix, especially the attitude error is larger in actual navigation system. In order to solve it, the error quaternion vector of attitude δq is given to description of the transformation relationship of nominal navigation framen relative to the actual navigation frame n′ to avoid the calculation error due to inaccurate modeling in the skew symmetric matrix Cn′n that consists attitude error vector ∅e,∅n,and ∅u.The true quaternion vector of attitude is defined as vectorq, corresponding the estimated quaternion vector is q^. According to the equivalence relation of attitude matrix and attitude quaternion, the expression Cbn′=Cnn′Cbn in reference of (14) is transformed as(15)q^=δq∗⊗qHere,δq is the error quaternion vector δq=δq0δq1δq2δq3. The conjugate of the error quaternion is defined as vector δq∗. The operator “⊗” denotes the quaternion multiplication. The error quaternion vector δq and the output error of gyroscope bt are chosen as state vector considering the effect of the model error of attitude errors components.(16)X=δqbt7×1TThe bias of gyroscopebt=btxbtybtz, which is the bias drift in three sensitive axisx,y, andz. The nonlinear differential equations of the true and actual quaternion vector are expressed as(17)q˙=12q⨂ωibb-ωinb(18)q^˙=12q^⨂ω^ibb-ω^inbwhereω^ibb is the actual measurement angular rate of the gyroscope. ωibb is the true angular rate. The vector ωinb=Cnbωinn, ωinn=ωien+ωenn. The vector ωien is the earth rate in navigation frame. The vector ωenn is the turn rate of the navigation frame with respect to the velocity of the craft. According to the equivalence transformation in (15), the expression is derived as(19)δq=q⊗q^∗Accordingly,q=δq⊗q^∗ is also deduced. The derivative of the equation on both sides is described in (17), (18), and (19)(20)δq˙=q˙⊗q^∗+q⊗q^˙∗According to (20), the derivative of error quaternion vector could be expressed(21)δq˙=-12δωibn⊗δq-12ωinn⊗δq+12δq⊗ω^innIn actual strapdown inertial navigation system, the small earth rate componentωien could be ignored. Furthermore, the turn rate of the navigation frame ωenn would be substituted for estimated value ω^enn. The output error of gyro δωibb would be would be substituted for the constant bias b0, which is estimated in full range as the deterministic error via calibration experiment offline.Corresponding, the state equation is given by(22)X˙7×1=fδq,bt=-12A4×404×303×4C3×3·X7×1+W7×1Here(23)A=0-b0x-b0y-b0zb0x0-2ω^enzn-b0z2ω^enyn+b0yb0y2ω^enzn+b0z0-2ω^enxn-b0xb0z-2ω^enyn-b0y2ω^enxn+b0x0C=-1β∗100010001W7×1=01×4ωrxωryωrzTwhereβ is the time constant of a first-order Gauss-Markov process and ωr is Gaussian white process. In discrete time, the state equation could be rearranged.


### 2.4.2. System Measurement Model

The measurement model is constructed by choosing the projection vectors of gravitational acceleration and magnetometer’s vectors in actual navigation system.(24)Zk=fgn′mn′The measurement vector(25)Zk=Cnn′00Cnn′gnmn+∇n′The attitude error matrixCnn′ is expressed in terms of error quaternion. ∇n′=∇gn′∇mn′T is the output error of accelerometer and magnetometer in the actual navigation system.(26)Cnn′=δq12+δq02-δq32-δq222δq1δq2+δq0δq32δq1δq3-δq0δq22δq1δq2-δq0δq3δq22-δq32+δq02-δq122δq2δq3+δq0δq12δq1δq3+δq0δq22δq2δq3-δq0δq1δq32-δq22-δq12+δq02


### 2.4.3. The Adaptive Gain Scale in Intelligent Kalman Filter

Owing to the high and time varying noise characters of low cost Micro IMU, the traditional extended Kalman filter (EKF) could not predict the statistical properties of system and measurement noise accuracy [17]. On the other hand, some complicated nonlinear filters impose a heavy burden on the small scaled microcontroller. To resolve the problem of robust control and characters of the uncertain noise statistics influence, an intelligent Kalman filter (IKF) of attitude estimation method is designed. The IKF could adjust the filter parameters online to the changes of both dynamic and observation noise via adaptive gain scale. The priori error covariance matrix of IKF is (27)Pk,k-1=Φk,k-1Pk-1Φk,k-1T+λk∙LkQk-1LkThe gain of IKF is presented(28)Kk=Pk,k-1HkTHkPk,k-1HkT+λk-1∙okRkok-1The matricesLk and ok are diagonal. The variable λk is the adaptive gain scale (1≤λk≤λmax). The innovation function is defined for evaluating the measurement error.(29)Bτ,k=∑i=nkti+1-tizti-z~ti2(30)Dτ,k=Bτ,k-BthresholdDefineBτ,k is the cost function, and τ is the calculation length of moving window. The parameter n is the first sampling time. Dτ,k is the difference value between the cost function and threshold value at sampling timek, which demonstrates the error of observation. The variable λk is the adaptive gain scale; its scope is within 1 and λmax.(31)λk=RfuzzyDτ,kThe following analytical solution algorithm for adjusting the adaptive gain scale is proposed. Kalman filter error does not depend on the selection of process noise variance or measurement noise statistics absolute levels but on their relationship [21]. The estimation process of the filter comprises two stages. One is a transition process of the estimation, the other one is a steady state. To solve the drawbacks of traditional extended Kalman filter applied in nonlinear systems highly depending on measurement and process noise variance values, the fuzzy rules to tune the adaptive gain scale are constructed in transition process and steady state. The initial process and measurement noises covariance matrix could be identified using Allan variance [22] or PSD method [23].In the transition process of filter, the true behavior of some state vector components is usually unknown. Therefore, the larger variationDτ,k would occur in the first stage of filter. In this case, larger values of process noise variance should be chosen in the transition process. Meanwhile, the weight of measurements λk-1 in estimation process has to be decreased for the sake of reducing the influence of measurement noise on current estimates. Therefore, the adaptive gain scale λk should tune its initial value to maximum value with the increasing of variation Dτ,k in the first stage of filter. As a result, all estimates of state vector components could converge to true values faster.With the selection of reasonable adaptive gain scale, the filter would provide the estimates with constant accuracy from steady state to transition process. The valueDτ,k would be decreased to a small value gradually with the estimation process. On that time, the smaller adaptive gain scale λk would be chosen. The weight of measurements would be increased with the effective estimation.According to the above analysis, a fuzzy controller with one-dimension is implemented in filter. The increasing of variationDτ,k and adaptive gain scale λk are input and output of the fuzzy controller, respectively. The corresponding fuzzy rules to tune the adaptive gain scale would be produced. The fuzzy controller could obtain the outputs in real time from query rule table online, which is generated according to rules offline. The structure of the system is shown in Figure 7. The dominant errors of micro IMU were compensated by initial and compounded calibration method. In attitude estimation method, the processed data of three axis gyro is corrected by eliminating the bias error from IKF. The estimated error quaternion is used for calculation of the transformation matrix. Then, the estimated attitude matrix is also corrected eventually.Figure 7
The algorithm structure for compounded calibration and attitude estimation.


## 2.4.1. State Model Based on Error Quaternion

The transformation matrix from navigation framen to body fixed frameb is based on the three Euler angles, namely, pitch, roll, and yaw, denoted by the symbols θ, ϕ, and ψ. The estimated direction cosine matrix attitude matrix Cbn′ from actual navigation frame n′ and body frameb by three Euler angles. The transformation matrix Cn′n denotes the transformation relationship between true navigation framen and estimated navigation frame n′. The attitude errors ∅e,∅n,and ∅u are given as small error components considering that high order terms are neglected. (13)Cn′n=I+ϕ×whereϕ× is the skew symmetric matrix, ϕ×=0-ϕuϕnϕu0-ϕe-ϕnϕe0 andI is the identity matrix. The relationship is described for the true strapdown matrix Cbn relative to the estimated attitude matrix Cbn′.(14)Cbn=Cn′n∙Cbn′=I+ϕ×·Cbn′If the attitude errors components∅e, ∅n, and ∅u are chosen as state variables in filter, the skew symmetric matrix ϕ× is updated by the calculation due to the estimated state variables. The more accurate strapdown matrix Cbn would be corrected to improve the precision of attitude in (14). The actual transformation matrix. Cn′n, which denotes the attitude error between the true strapdown matrix Cbn and the estimated attitude matrix Cbn′, could not be model as skew symmetric matrix, especially the attitude error is larger in actual navigation system. In order to solve it, the error quaternion vector of attitude δq is given to description of the transformation relationship of nominal navigation framen relative to the actual navigation frame n′ to avoid the calculation error due to inaccurate modeling in the skew symmetric matrix Cn′n that consists attitude error vector ∅e,∅n,and ∅u.The true quaternion vector of attitude is defined as vectorq, corresponding the estimated quaternion vector is q^. According to the equivalence relation of attitude matrix and attitude quaternion, the expression Cbn′=Cnn′Cbn in reference of (14) is transformed as(15)q^=δq∗⊗qHere,δq is the error quaternion vector δq=δq0δq1δq2δq3. The conjugate of the error quaternion is defined as vector δq∗. The operator “⊗” denotes the quaternion multiplication. The error quaternion vector δq and the output error of gyroscope bt are chosen as state vector considering the effect of the model error of attitude errors components.(16)X=δqbt7×1TThe bias of gyroscopebt=btxbtybtz, which is the bias drift in three sensitive axisx,y, andz. The nonlinear differential equations of the true and actual quaternion vector are expressed as(17)q˙=12q⨂ωibb-ωinb(18)q^˙=12q^⨂ω^ibb-ω^inbwhereω^ibb is the actual measurement angular rate of the gyroscope. ωibb is the true angular rate. The vector ωinb=Cnbωinn, ωinn=ωien+ωenn. The vector ωien is the earth rate in navigation frame. The vector ωenn is the turn rate of the navigation frame with respect to the velocity of the craft. According to the equivalence transformation in (15), the expression is derived as(19)δq=q⊗q^∗Accordingly,q=δq⊗q^∗ is also deduced. The derivative of the equation on both sides is described in (17), (18), and (19)(20)δq˙=q˙⊗q^∗+q⊗q^˙∗According to (20), the derivative of error quaternion vector could be expressed(21)δq˙=-12δωibn⊗δq-12ωinn⊗δq+12δq⊗ω^innIn actual strapdown inertial navigation system, the small earth rate componentωien could be ignored. Furthermore, the turn rate of the navigation frame ωenn would be substituted for estimated value ω^enn. The output error of gyro δωibb would be would be substituted for the constant bias b0, which is estimated in full range as the deterministic error via calibration experiment offline.Corresponding, the state equation is given by(22)X˙7×1=fδq,bt=-12A4×404×303×4C3×3·X7×1+W7×1Here(23)A=0-b0x-b0y-b0zb0x0-2ω^enzn-b0z2ω^enyn+b0yb0y2ω^enzn+b0z0-2ω^enxn-b0xb0z-2ω^enyn-b0y2ω^enxn+b0x0C=-1β∗100010001W7×1=01×4ωrxωryωrzTwhereβ is the time constant of a first-order Gauss-Markov process and ωr is Gaussian white process. In discrete time, the state equation could be rearranged.


## 2.4.2. System Measurement Model

The measurement model is constructed by choosing the projection vectors of gravitational acceleration and magnetometer’s vectors in actual navigation system.(24)Zk=fgn′mn′The measurement vector(25)Zk=Cnn′00Cnn′gnmn+∇n′The attitude error matrixCnn′ is expressed in terms of error quaternion. ∇n′=∇gn′∇mn′T is the output error of accelerometer and magnetometer in the actual navigation system.(26)Cnn′=δq12+δq02-δq32-δq222δq1δq2+δq0δq32δq1δq3-δq0δq22δq1δq2-δq0δq3δq22-δq32+δq02-δq122δq2δq3+δq0δq12δq1δq3+δq0δq22δq2δq3-δq0δq1δq32-δq22-δq12+δq02


## 2.4.3. The Adaptive Gain Scale in Intelligent Kalman Filter

Owing to the high and time varying noise characters of low cost Micro IMU, the traditional extended Kalman filter (EKF) could not predict the statistical properties of system and measurement noise accuracy [17]. On the other hand, some complicated nonlinear filters impose a heavy burden on the small scaled microcontroller. To resolve the problem of robust control and characters of the uncertain noise statistics influence, an intelligent Kalman filter (IKF) of attitude estimation method is designed. The IKF could adjust the filter parameters online to the changes of both dynamic and observation noise via adaptive gain scale. The priori error covariance matrix of IKF is (27)Pk,k-1=Φk,k-1Pk-1Φk,k-1T+λk∙LkQk-1LkThe gain of IKF is presented(28)Kk=Pk,k-1HkTHkPk,k-1HkT+λk-1∙okRkok-1The matricesLk and ok are diagonal. The variable λk is the adaptive gain scale (1≤λk≤λmax). The innovation function is defined for evaluating the measurement error.(29)Bτ,k=∑i=nkti+1-tizti-z~ti2(30)Dτ,k=Bτ,k-BthresholdDefineBτ,k is the cost function, and τ is the calculation length of moving window. The parameter n is the first sampling time. Dτ,k is the difference value between the cost function and threshold value at sampling timek, which demonstrates the error of observation. The variable λk is the adaptive gain scale; its scope is within 1 and λmax.(31)λk=RfuzzyDτ,kThe following analytical solution algorithm for adjusting the adaptive gain scale is proposed. Kalman filter error does not depend on the selection of process noise variance or measurement noise statistics absolute levels but on their relationship [21]. The estimation process of the filter comprises two stages. One is a transition process of the estimation, the other one is a steady state. To solve the drawbacks of traditional extended Kalman filter applied in nonlinear systems highly depending on measurement and process noise variance values, the fuzzy rules to tune the adaptive gain scale are constructed in transition process and steady state. The initial process and measurement noises covariance matrix could be identified using Allan variance [22] or PSD method [23].In the transition process of filter, the true behavior of some state vector components is usually unknown. Therefore, the larger variationDτ,k would occur in the first stage of filter. In this case, larger values of process noise variance should be chosen in the transition process. Meanwhile, the weight of measurements λk-1 in estimation process has to be decreased for the sake of reducing the influence of measurement noise on current estimates. Therefore, the adaptive gain scale λk should tune its initial value to maximum value with the increasing of variation Dτ,k in the first stage of filter. As a result, all estimates of state vector components could converge to true values faster.With the selection of reasonable adaptive gain scale, the filter would provide the estimates with constant accuracy from steady state to transition process. The valueDτ,k would be decreased to a small value gradually with the estimation process. On that time, the smaller adaptive gain scale λk would be chosen. The weight of measurements would be increased with the effective estimation.According to the above analysis, a fuzzy controller with one-dimension is implemented in filter. The increasing of variationDτ,k and adaptive gain scale λk are input and output of the fuzzy controller, respectively. The corresponding fuzzy rules to tune the adaptive gain scale would be produced. The fuzzy controller could obtain the outputs in real time from query rule table online, which is generated according to rules offline. The structure of the system is shown in Figure 7. The dominant errors of micro IMU were compensated by initial and compounded calibration method. In attitude estimation method, the processed data of three axis gyro is corrected by eliminating the bias error from IKF. The estimated error quaternion is used for calculation of the transformation matrix. Then, the estimated attitude matrix is also corrected eventually.Figure 7
The algorithm structure for compounded calibration and attitude estimation.


## 3. Experimental Results

The testing experimental platform was set up. It comprises the prototype of Micro IMU, GPS receiver, and Xsens MTi-G. The proposed algorithm was performed using the testing experimental platform compared with a commercial Xsens MTi-G from the Xsens Motion Technologies, which is an industrial grade miniature GNSS-aided, IMU-enhanced GNSS/INS and AHRS that provides high-quality position, velocity, acceleration, and orientation, even in challenging environments. The dynamic precision of pitch, roll, and yaw angle is within0.3° and 0.8°. So it could be used to provide reference attitude calculation results for comparison.Through the comparison with setting data of simulating ballistic attitude variations and reference attitude from MTi-G, the attitude data from Mti device could accurately reflect the real information on the simulating ballistic attitude data prepared in advance [24]. Therefore, MTi-G could be a reliable device for providing data of the MEMS sensors and reference attitude in system. It comprises MEMS gyroscope, accelerometer, and magnetometer. The sampling frequency 100Hz is set to collect raw data from customized the prototype of micro IMU and Xsens Mti-G. The experimental devices are mounted in car roof, which is shown in Figure 8. The experimental data are collected by means of a laptop PC with the USB port from converting the RS232 serial port. The open section of countryside was chosen as the vehicle testing route.Figure 8
Experimental system of attitude estimation.Distinctive testing routes of different districts are implemented to verify the effect of the proposed method. Among them, the trajectory of larger dynamic variation in yaw angle is chosen as the illustration of experimental result. The start point located at 123.4479° E and 41.6744° N. The end point located at 123.4482° E and 41.6743° N. The result of testing trajectory is illustrated from 0s to 200s. The custom-built algorithms are programmed. The series experiments are carried out with EKF method based on attitude error, the proposed IKF method based on error quaternion. The Euler data from MTi-G are used as reference attitude for comparison. The corresponding programs of the proposed algorithm could be compiled and run on Quartus 11 and CCS 3.3 environment for FPGA/DSP controller in real time. The calculating time of DSP/FPGA prototype is 6.64ms in a sampling period. The results of roll, pitch, and yaw angle are shown in Figure9.Figure 9
Attitude angle estimation comparison between the proposed algorithm and other algorithm.As can be presented in Figure9, it can be concluded that the estimation accuracy in terms of error quaternion for IKF is superior to that for attitude error EKF. The system model in terms of error quaternion is established to estimate the attitude error matrix Cnn′accurately, which is used for substituting the skew symmetric matrix via attitude error vector ∅e,∅n,and ∅u. The effect due to the inaccurate modeling is eliminated. Moreover, the effective fuzzy rules of adaptive gain scale are constructed in transition process of IKF. The larger system noise by the adjustment of adaptive gain scale is chosen from transition stage to steady state. The compensation for the effects of inaccurate system model of subjective choice and the responsive ability are accomplished in the proposed algorithm. The attitude estimation accuracy has greatly been improved by the proposed method. The estimated results of roll and pitch angle based on the Error quaternion IKF have an accurate tracking ability. Compared with using the measurement of MTi-G, they are closer to the reference attitude than attitude estimated result of attitude error EKF. The maximum estimated roll and pitch angle errors of the proposed method are 0.23° and 0.24°, respectively, while those for attitude error EKF method reach 4.82° and 3.14°, respectively. The dominant errors are generated from inaccurate model of the skew symmetric matrix in actual navigation system. Besides, the precision of estimated attitude error components ∅e,∅n,and ∅u is affected with the disturbing effect of system and measurement modeling in EKF method.It is worth noting that the accuracy of yaw estimation is lagged especially when the vehicle is turning. It is suffered from large sideslip angle of vehicle’s turning, which leads to inaccurate calculation of vehicle accelerationab in single antenna GPS system. Furthermore, the little time delay occurred when the estimated yaw angle changed with different quadrant transformation (from-180° to +180°) at the 143s’ tracking point. Due to the effect of noise in GPS measured vehicle velocity and measurement of accelerometer in dynamic environment, maximum yaw error of attitude error EKF is more than 6°. The maximum yaw error of the proposed algorithm is 0.59°. The result of attitude error is shown in Table 2.Table 2
The RMSE of attitude error in IKF algorithm.
Attitude Attitude Error EKF Error Quaternion IKF RMS STD RMS STD Roll 1.8761 1.0185 0.0790 0.0783 Pitch 1.4575 0.6900 0.0861 0.0805 Yaw 3.4165 3.2852 0.3511 0.3210The RMSE (dynamic accruracy) and STD of the roll, pitch and yaw estimation error are shown at the full testing trajectory. The improvement in RMSE is about 90.35%.


## 4. Conclusions

For low cost Micro IMU in lab, a combined calibration method with initial and compounded calibration based on FNN is proposed. The estimated adjustment value in compounded process is used for compensating the coupling effect of different rotating axis in high dynamic environment. The adverse coupling effects between high dynamic rotation and low dynamic rotation axis of gyro are removed through compounded calibration. Furthermore, the complexity state equation of error quaternion vectors is deduced, which is applied in the IKF. The fuzzy controller in IKF is designed to tune the adaptive gain scale for compensating the uncertainty of measurement and process noise. The proposed approach based on error quaternion and traditional EKF based on attitude error vector are valuated via the testing experiment. Experimental results show that the proposed algorithm has closer results in comparison with the commercial Xsens MTi-G device. The RMSE (dynamic accuracy) of the roll, pitch, and yaw estimation error is 0.08°, 0.09°, and 0.35° at the full testing trajectory.



---

*Source: 4514873-2019-05-14.xml*