Hindawi Mathematical Problems in Engineering Volume 2017, Article ID 4517673, 14 pages https://doi.org/10.1155/2017/4517673 Research Article Attitude Determination Method by Fusing Single Antenna GPS and Low Cost MEMS Sensors Using Intelligent Kalman Filter Algorithm Lei Wang, Bo Song, Xueshuai Han, and Yongping Hao Research Centre of Weaponry Science and Technology, Shenyang Ligong University, Shenyang, Liaoning, China Correspondence should be addressed to Lei Wang; [email protected] Received 2 March 2017; Revised 10 May 2017; Accepted 12 June 2017; Published 17 July 2017 Academic Editor: Kalyana C. Veluvolu Copyright © 2017 Lei Wang et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited. For meeting the demands of cost and size for micronavigation system, a combined attitude determination approach with sensor fusion algorithm and intelligent Kalman filter (IKF) on low cost Micro-Electro-Mechanical System (MEMS) gyroscope, accelerometer, and magnetometer and single antenna Global Positioning System (GPS) is proposed. The effective calibration method is performed to compensate the effect of errors in low cost MEMS Inertial Measurement Unit (IMU). The different control strategies fusing the MEMS multisensors are designed. The yaw angle fusing gyroscope, accelerometer, and magnetometer algorithm is estimated accurately under GPS failure and unavailable sideslip situations. For resolving robust control and characters of the uncertain noise statistics influence, the high gain scale of IKF is adjusted by fuzzy controller in the transition process and steady state to achieve faster convergence and accurate estimation. The experiments comparing different MEMS sensors and fusion algorithms are implemented to verify the validity of the proposed approach. 1. Introduction Attitude estimation is essential information for control of micronavigation system, such as Micro Autonomous Vehicle, Micro Aerial Vehicle (MAV), and Guided Munition Shell [1–4]. In these specific applications, the size and cost are limited. Low cost and small size MEMS IMU, which consists of MEMS gyroscope and accelerometer, could be used in micronavigation system. Lower price, smaller sized MEMS IMU is less accurate, has large biases, and is more noisy. The errors of MEMS sensors will lead to a great impact on the calculation performance and reduce the estimation accuracy significantly [5, 6]. Thus, the effective calibration and error compensation method is essential to evaluate and improve the performance of MEMS IMU before attitude estimation [7–9]. Nowadays, the different attitude estimation algorithms have been developed, which are employed to Micro IMUs and integrated with magnetic sensors, Global navigation satellite system (GNSS) receiver, and other sensors [1–4, 10–17]. Among them, representative researches like multiinformation fusion technique utilize the different MEMS sensor [10–12], different Kalman filtering methods for attitude estimation [3, 13–15], and methodology combining data fusion with filtering strategies [16, 17]. Utilizing barometer and low cost IMU, the data fusion method via the complementary filter is proposed to fuse altitude data for unmanned aerial vehicle (UAV) system [10]. A quaternion-based complementary filter for estimating the attitude of a smartphone using IMU and magnetic sensor is developed. The optimal tuning correction factors of the algorithm across all the body movements were also identified [11]. An attitude estimation method is proposed using low cost gyroscope, magnetometer, and GPS receiver. The pseudoattitude, magnetic attitude, and gyroscope measurement are fused based on Euler angle. The simulation and flight test verified the proposed method [12]. For small UAVs, an attitude estimation method fusion of gyroscopes and single antenna GPS with quaternion-based 2 EKF is proposed. The results of the simulations and the flight experiment demonstrate the ability of the proposed algorithm [3]. The sideslip angle is estimated in Kalman filter using the velocity signals from GPS and angular signals from magnetometer in case that no longitudinal tire force and no pitch angle exist. Road tests are implemented [13]. According to practical application of quadcopter UAV, the adaptive gain parameter in EKF filter was tuned [14]. The attitude estimation algorithm is developed by incorporating a nonlinear observer in order to fuse measurements from both the gyroscope and accelerometer. The attitude rotation matrix is evaluated with estimated the gyro bias and tilt error [15]. A wireless IMU with the smallest volume and weight is designed. The data fusion is implemented before a Kalman filter combined with the zero velocity update. The methodology for wireless IMU is adopted to track a person in an indoor area [16]. Integrating double-antenna Global Positioning System (DA-GPS) with other low cost in-vehicle sensors, two parallel extended Kalman filters (EKFs) are adopted to estimate the roll angle firstly; then vehicle sideslip and yaw angles are estimated through fusion methodology [17]. The above works on attitude estimation algorithm lie in following problems. Some attitude estimation methods could provide reliable estimation, but it has to rely on the use of multiple antennas, which incurs a complex structure, large volume, and high cost of the system [1, 17]. It is restricted in such micro navigation system. For traditional extended Kalman filter and other improved Kalman filter methods for attitude estimation, it would cause the inaccurate estimation or even divergence because the statistical properties of process and measurement noise cannot be predicted accurately, especially for low cost MEMS-based sensors [3, 13, 14]. Some complicated nonlinear filter and optimal control methods could obtain optimal estimate. On the other hand, the main drawback lies in introducing a heavy computational burden that leads to difficulty of application in real time navigation system [15, 18]. For meeting the demands of cost and size for micro navigation system, such as spinning shell, Missiles, and aircrafts, the scheme of attitude estimation combining single antenna GPS with low cost MEMS IMU and magnetometer is designed. This paper concentrates on developing a combined fusion methodology with intelligent Kalman filter, which realizes the fusion of Micro IMU, GPS, and magnetometer. It could continue to provide reliable information, particularly in larger sideslip angle, GPS failure situation. Testing experiments using the proposed approach are presented by comparison with different fusion algorithms and filter methods. The novel aspects of this paper can be summarized as follows. To utilize the strengths of the fusion estimation algorithm and adaptive filter approaches, the combined approach has been proposed. The different control strategies fusing the MEMS multi-inertial sensors are adopted under different driving situations. The high gain scale in IKF is tuned by fuzzy controller to resolve the problem of robust control and characters of the uncertain noise statistics influence for low micro IMU. Mathematical Problems in Engineering GPS-measured yaw angle for Micro IMU/GPS system is afforded as measurement to enhance the observability of IKF. Besides, the fusion information of yaw angle using gyroscope, accelerometer, and magnetometer fusion algorithm is added into IKF observer, which could provide reliable information, particularly under unavailable larger sideslip angle, GPS failure situation. Integrating data fusion algorithm with IFK method, the proposed methodology could significantly improve the estimation performance. 2. Methodology 2.1. Gyroscope Model and Attitude Algorithm. MEMS inertial sensors of low cost, small size, and low power consumption are now widely used in micronavigation system and guidance system of tactical weapons. The application of these sensors is restricted for low precision and bias instability. It is essential to test, calibrate, and compensate for these sensors. The systematic error sources for MEMS inertial sensors mainly embody in biases, scale factors, nonorthogonality, or misalignment errors. The output model of a triad of MEMS gyro sensor in matrix form can be represented as ̃𝑏 = 𝑠 ⋅ 𝜔𝑏 + 𝑏. 𝜔 𝑖𝑏 𝑖𝑏 (1) In (1), the definition of angular rates is based on the following conventions. The upper index defines the referenced coordinate system, whereas the lower index refers to the described value. The rotation refers to the relative movement of coordinate system. Used indices are 𝑒-Earth frame, 𝑛Navigation frame, 𝑖-inertial frame, and 𝑏-body frame, where ̃𝑏 is the raw measurement angular rate of the gyroscope; 𝜔𝑏 𝜔 𝑖𝑏 𝑖𝑏 is the true angular rate; 𝑏 is the output bias. The scale factor and misalignment matrix 𝑠 is given by 𝑘𝑥 𝜎𝑥𝑦 𝜎𝑥𝑧 [ ] ] 𝑠=[ [𝜎𝑦𝑥 𝑘𝑦 𝜎𝑦𝑧 ] . [𝜎𝑧𝑥 𝜎𝑧𝑦 𝑘𝑧 ] (2) The main diagonal elements 𝑘 are scale factors that account 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 error or deterministic sources for MEMS inertial sensors. The body angler rates with respect to the navigation frame 𝜔𝑛𝑏 are given by the difference between the body angler rates, 𝜔𝑖𝑏 , and the navigation frame rotation, 𝜔𝑖𝑛 . 𝑇 𝑏 𝑏 𝑛 = 𝜔𝑖𝑏 − (𝐶𝑏𝑛 ) 𝜔𝑖𝑛 . 𝜔𝑛𝑏 (3) The coordinate transformation from navigation frame to body fixed frame is based on the three angles, namely, pitch, roll, and yaw, denoted by the symbols 𝜃, 𝜙, and 𝜓. The attitude matrix 𝐶𝑏𝑛 from navigation frame and body frame by three Euler angles is given by Mathematical Problems in Engineering 𝐶𝑏𝑛 3 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 𝜃 The propagation of the Euler angles with time yields is derived in 𝑏 𝜔𝑛𝑏𝑥 𝜙̇ 1 sin 𝜙 tan 𝜃 cos 𝜙 tan 𝜃 [ ] [ ] [ 𝑏 ] [ 𝜃̇ ] = [0 cos 𝜙 − sin 𝜙 ] (5) ][ [𝜔𝑛𝑏𝑦 ] . [ ] 𝑏 [𝜓̇ ] [0 sin 𝜙 sec 𝜃 cos 𝜙 sec 𝜃 ] [𝜔𝑛𝑏𝑧 ] By (3), neglecting of the earth rate and the angular velocity of the local-level frame for the spherical earth model, 𝑏 𝑏 𝑏 are approximately equal as 𝜔𝑖𝑏 ≈ 𝜔𝑛𝑏 . the gyroscope rates 𝜔𝑖𝑏 2.2. Accelerometer Model and Attitude Algorithm. Define the ̃𝑏 as the raw measurements of accelerometer in body frame. 𝑓 The vector 𝑓𝑏 is the true output of accelerometer. Considering the scale factor error and fixed error, the relation is given by ̃𝑏 = (𝐼 + [𝛿𝐾 ]) (𝐼 + [𝛿𝐴]) 𝑓𝑏 . 𝑓 𝐴 (6) The matrix [𝛿𝐾𝐴] representing scale factor error for accelerometer and the skew symmetric matrix [𝛿𝐴] representing the fixing error of accelerometer are given by 𝛿𝐾𝐴𝑥 0 [ 0 𝛿𝐾 [𝛿𝐾𝐴 ] = [ 𝐴𝑦 [ 0 0 [ [𝛿𝐴] = [ [−𝛿𝐴𝑧 0 0 0 ] ], 𝛿𝐾𝐴𝑧 ] 𝛿𝐴𝑧 −𝛿𝐴𝑦 (7) ] 𝛿𝐴𝑥 ] ]. 0 ] 0 [ 𝛿𝐴𝑦 −𝛿𝐴𝑥 The parameters of matrix [𝛿𝐾𝐴 ] and [𝛿𝐴] are computed by calibration method. ∇𝑏 is the error vectors of accelerometer. Define 𝑓𝑛 as the value in the navigation frame. The relation between the output of accelerometer 𝑓𝑏 and 𝑓𝑛 is in 𝑓𝑏 = 𝐶𝑛𝑏 𝑓𝑛 = 𝐶𝑛𝑏 (𝑎𝑛 + 𝑔𝑛 + ∇𝑛 ) = 𝑎𝑏 + 𝐶𝑛𝑏 𝑔𝑛 + ∇𝑏 , (8) 𝑏 where 𝑔 is the gravity. The vector 𝑎 is the vehicle acceleration. The following two cases are considered in (8). One is that the vehicle is in stationary or low dynamic condition, accelerometer measures gravity mainly. The accelerometer output in navigation frame is given by 𝑇 𝑛 𝑓 = [0 0 𝑔] . 𝜙 = arctan ( 𝑓𝑥𝑏 ), 𝑔 𝑓𝑦𝑏 𝑔 ⋅ cos 𝜃 (10) ). ] On the other side, when the vehicle is accelerated, the accelerometer measurement reflects not only gravity but also the acceleration of the vehicle. The vehicle acceleration, 𝑎𝑏 ≠ 0, is determined by Costa theorem. The vector 𝜐𝑏 is the vehicle velocity in body frame 𝑎𝑏 = 𝜐𝑏̇ + 𝜔𝑏 × 𝜐𝑏 . (11) 2.3. Magnetometer Model and Attitude Algorithm. Magnetometer sensor is designed and manufactured by making use of alloy resistance having sensitive response to direction magnetic field. The performance of magnetometer sensor is distorted by hard iron and soft iron effects. Intrinsic and cross sensor calibration of three-axis magnetometer is indispensable to eliminate scale factor, cross coupling, and bias errors. Thus, the magnetometer’s output can be modeled by ̃𝑏 = 𝐶 𝑚𝑏 + 𝜀. 𝑚 𝑒 (12) ̃𝑏 represents the raw measurement in body The matrix 𝑚 𝑏 frame. 𝑚 represents the calibrated magnetometer output. Here, 𝐶𝑒 can be decomposed as 𝐶𝑒 = 𝐶sf 𝐶no 𝐶sm , where 𝐶sf , 𝐶no , and 𝐶sm matrix represent effect of sensor imperfection and magnetic disturbance. The subscript “sf” means the scale factor error, “no” means nonorthogonality and misalignment error, and “sm” means the soft iron effect. The vector 𝜀 represents the noise of magnetic disturbance. Define 𝑚𝑛 as the local magnetic vector in 𝑛-frame. The relationship of magnetic vectors in 𝑏-frame and 𝑛-frame is given by 𝑚𝑏 = 𝐶𝑛𝑏 𝑚𝑛 . (13) Considering the horizontal plane projection of magnetic field 𝑚𝑝 , corresponding representation is given by 𝑚𝑥𝑏 [ ] [ 𝑝] [ 0 𝑏] cos 𝜙 − sin 𝜙 ] ]⋅[ [𝑚𝑦 ] = [ [𝑚𝑦 ] . (14) 𝑝 [𝑚𝑧 ] [− sin 𝜃 sin 𝜙 cos 𝜃 cos 𝜙 cos 𝜃] [𝑚𝑧𝑏 ] 𝑚𝑥𝑝 cos 𝜃 sin 𝜙 sin 𝜃 cos 𝜙 cos 𝜃 Eventually, yaw angle 𝜓𝑚 is calculated in (9) Thus, the roll and pitch angles are determined by the following equations, respectively, 𝜃 = − arctan ( cos 𝜙 cos 𝜃 (4) 𝜓𝑚 = arctan ( 𝑚𝑦𝑝 𝑝). 𝑚𝑥 (15) 2.4. Different Sensor Attitude Fusion Estimation Algorithm. For the scheme of attitude estimation combining low cost MEMS IMU, magnetometer with single antenna GPS, Strapedown Inertial Navigation system (SINS) of MEMS gyroscope, and accelerometer could provide angles utilizing the differential equation for quaternion vector but suffer from 4 Mathematical Problems in Engineering accumulating errors induced by gyroscopes bias drift. The differential equations of the quaternion vector are expressed as 1 𝑏 𝑏 (16) − 𝜔𝑖𝑛 ). 𝑞 ̇ = 𝑞 ⊗ (𝜔𝑖𝑏 2 Here, 𝑞 is the error quaternion vector, 𝑞 = 𝑇 [𝑞0 𝑞1 𝑞2 𝑞3 ] . The operator “⊗” denotes the quaternion 𝑏 𝑛 = 𝐶𝑏𝑛 𝜔𝑖𝑛 . multiplication. The vector 𝜔𝑖𝑛 Accelerometers can provide the pitch and roll angles by measuring the gravity vectors if the vehicle is stationary or in low dynamic environment in (10). Triaxis magnetometers are used for calculating the yaw angle in (14) and (15). Its accuracy is heavily affected by local magnetic environments. Derived from the single antenna GPS, the pseudoattitude method [19, 20] is adopted. The velocity vector 𝑉𝑛 = 𝑇 [𝑉𝐸𝑛 𝑉𝑁𝑛 𝑉𝐷𝑛 ] is the single antenna GPS-measured velocity vector in the east, north, and down directions of the navigation frame, respectively. The lift acceleration vector 𝐿 is equivalent to the difference between the normal acceleration component 𝑎𝑛 and gravitational acceleration component 𝑔𝑛 . Then 𝐿 is given by 𝐿 = 𝑎𝑛 − 𝑔𝑛 . A horizontal reference vector 𝑇 𝑃 is derived from 𝑉𝐷 = [0 0 𝑉𝐷𝑛 ] and 𝑉𝑛 , in the form 𝐷 𝑛 𝑃 = 𝑉 × 𝑉 . The Euler angle denoting pseudoattitude algorithm is determined in 𝜃𝑠 = arctan ( 𝜙𝑠 = arcsin [ 𝑉𝐷𝑛 (√𝑉𝑁𝑛 2 + 𝑉𝐸𝑛 2 ) ), (𝐿 ⋅ 𝑃) ], (|𝐿| |𝑃|) 𝜓𝑠 = arctan ( (17) 𝑉𝐸𝑛 ). 𝑉𝑁𝑛 It has been verified that GPS could reliably provide the estimation. However, some common limitations are that GPS failure is not considered or larger sideslip angle could not be estimated for single antenna GPS. For attitude estimation algorithm by fusing data from MEMS inertial sensors, complementary filter (CF) is usually utilized to estimate attitudes due to the complementary characteristic of gyroscopes and accelerometer [10, 11]. The classical form of the complementary filters can be reformulated. ̂1 (𝑠) + 𝐺2 (𝑠) 𝑋 ̂2 (𝑠) . ̂ (𝑠) = 𝐺1 (𝑠) 𝑋 𝑋 (18) ̂ ̂1 (𝑠) 𝑋(𝑠) is the estimation of the complementary filter. 𝑋 ̂2 (𝑠) are the estimation by gyro and accelerometer, and 𝑋 respectively. The transfer function 𝐺1 (𝑠) is first-order high pass filter and 𝐺2 (𝑠) = 1 − 𝐺1 (𝑠) is first-order low pass filter. The transfer functions of the filters are as follows 𝑠 𝐺1 (𝑠) = , (𝑠 + 𝐾) (19) 𝐾 𝐺2 (𝑠) = . (𝑠 + 𝐾) For attitude algorithm, gyroscopes can provide angles but suffer from accumulating errors induced by gyroscopes bias drift. High pass filter could eliminate the low-frequency random noise of the Euler angle integrated by gyroscope, while the low pass filter could eliminate the high frequency random noise of calculation by accelerometer. 2.5. IKF Fusion Algorithm for Attitude Estimation. According to the micro navigation system of low cost Micro IMU, magnetometer, and a single antenna GPS receiver, the aforementioned attitude estimation approaches utilizing different sensors separately are difficult to provide reliable estimation due to sensor errors and dynamic environmental variations. The single fusion algorithm employing complementary filter is also difficult to achieve good robustness for different driving situations and complementary filter parameters. In practice cases, small adjustment in fusion and control strategy may finally affect the estimation performance. In addition, for trying to solve the attitude estimation utilizing conventional EKF or modified EKF methods, the uncertain noise for low micro IMU would not be estimated accurately. To overcome the drawbacks of above-mentioned methods, it is preferable to develop a reliable approach to estimate attitude angles using affordable sensors in micro navigation system. Utilizing the advantage of the fusion estimation algorithm and adaptive filter approach, the combined approach has been also developed. The proposed method could estimate the yaw angle utilizing affordable sensors under different driving situations. It could accommodate GPS failure or the situation of unavailable sideslip, which could continue to provide reliable information. In IKF module, the adaptive gain scale is adjusted by the fuzzy controller to compensate the uncertainty of measurement and process noise. Furthermore, the control strategies of different measurements considering sensor failure are adopted to solve the lack of observability of yaw angle. The switching criterion is determined according to GPS satellite number, yaw error estimate result, and the vehicle acceleration measurements to make a distinction between normal driving condition, vehicle turning, and GPS outage. One side, besides measurements from accelerometer, GPS-measured heading angle for micronavigation system is afforded as the measurement to enhance the observability in normal driving condition. Additionally, in case of GPS failure or the situation of unavailable sideslip, the fusion information of yaw angle using gyroscope, accelerometer, and magnetometer fusion algorithm is added into IKF observer. The scheme of the proposed methodology is shown in Figure 1. This scheme is actually a combined fusion algorithm and filtering approach architecture. It comprises two parts: the multisensor fusion module and IKF method. The multisensor fusion module consists of Micro IMU, magnetometer, and single antenna GPS. Micro IMU includes three single-axis MEMS gyroscopes and accelerometers. The deterministic errors are compensated by calibration process before fusion algorithm. Different fusion strategies provide efficient solutions to enhance the accuracy of the attitude estimation. To realize further accurate estimation, the IKF is designed. Mathematical Problems in Engineering 5 GPS outage or unavailable sideslip Magnetometer b m Calibration & mb error compensation Attitude determination based magnetometer Micro IMU MEMS gyroscope MEMS accelerometer Fusion algorithm m Roll & pitch estimation b ib Calibration & b ib error compensation Attitude determination based gyroscope fb Calibration & fb error compensation Attitude determination + based accelerometer _ g Intelligent Kalman filter algorithm (IKF) Control strategy N E D Measurement updates E, k G , , Pseudoattitude algorithm GPS works normally Fuzzy controller ab Vehicle acceleration calculation Single antenna GPS receiver Yaw estimation bx , by , bz Figure 1: The scheme of combing fusion algorithm with IKF for low cost Miro IMU and single antenna GPS. According to difference equation of the Euler angles in (5), the state vector is chosen of three attitude angles and triaxis gyroscope random bias vector. 𝑇 𝑥 = [𝜑 𝜃 𝜓 𝑏𝑥 𝑏𝑦 𝑏𝑧 ] . (20) The state model is thus ̃𝑏 ) = ( 𝑥̇ = 𝑓 (𝑥, 𝜔 𝑖𝑏 ̃𝑏 − 𝑏) Γ ⋅ 𝑠−1 (𝜔 𝑖𝑏 ). 1 − 𝜀𝑟 + 𝑤𝑟 𝛽 (21) The time varying bias component 𝜀𝑟 will be modeled as a first-order Gauss-Markov process. 𝑤𝑟 is a Gaussian white process. 𝛽 is the time constant. 𝑏 is the constant bias. With 1 sin 𝜙 tan 𝜃 cos 𝜙 tan 𝜃 [0 cos 𝜙 − sin 𝜙 ] Γ=[ ], (22) [0 sin 𝜙 sec 𝜃 cos 𝜙 sec 𝜃 ] ̃𝑏 is the measurement angular rate signal of the where 𝜔 𝑖𝑏 gyroscope and 𝑠 is the misalignment and scale factor matrix in (1), the GPS and accelerometer with high accuracy provide measurements. The measurement model is chosen as follows: ∇𝐺 𝜓𝐺 𝜓 ] [ [ 𝑏] [ 𝑏 𝑏 ] ] [ ] 𝑧=[ [𝑓𝑥 − 𝑎𝑥 ] = [ − sin 𝜃 ⋅ 𝑔 ] + [ ∇𝑥 ] , 𝑏 𝑏 𝑏 [𝑓𝑦 − 𝑎𝑦 ] [sin 𝜙 cos 𝜃 ⋅ 𝑔] [ ∇𝑦 ] (23) where 𝜓𝐺 is the GPS-measured yaw angle, ∇𝜓 is the zero Gaussian white noise with variance 𝜎𝐺2 , 𝑓𝑏 is the acceleration signal of the accelerometer, and ∇𝑏 is the measurement noise of accelerometer. When single antenna GPS system could provide normal satellite signals from GPS receiver, the yaw angle is calculated by velocity of GPS. If the single antenna GPS signal is lock, the GPS-measured yaw angle is unavailable. A compensation method integrating the MEMS inertial sensors is presented. The Euler angles can be estimated by means of measurement of gyroscopes according to 𝑏 𝑏 cos 𝜙 − 𝜔𝑖𝑏𝑧 sin 𝜙, 𝜃𝑔̇ = 𝜔𝑖𝑏𝑦 𝑏 𝑏 𝑏 𝜙𝑔̇ = 𝜔𝑖𝑏𝑥 + 𝜔𝑖𝑏𝑦 sin 𝜙 tan 𝜃 + 𝜔𝑖𝑏𝑧 cos 𝜙 tan 𝜃, (24) 𝑏 𝑏 𝜓̇𝑔 = 𝜔𝑖𝑏𝑦 sin 𝜙 sec 𝜃 + 𝜔𝑖𝑏𝑧 cos 𝜙 sec 𝜃. The yaw angle 𝜓(𝑡) is calculated in (25); correspondingly, the corrected yaw angle 𝜓(𝑡) is used as measurement vector in intelligent Kalman filter. 𝜓 (𝑡) = (1 − 𝛼1 ) ⋅ 𝜓 (𝑡 − 𝑇) + 𝛼1 ⋅ 𝜓𝑚 (𝑡) + 𝛼2 ⋅ 𝜓̇𝑔 (𝑡) , (25) where 𝛼1 = (𝑘1 ⋅ 𝑇)/(𝑘1 ⋅ 𝑇 + 1), 𝛼2 = 𝑇/(𝑘2 ⋅ 𝑇 + 1). Here T is the sampling time length and 𝜓(𝑡 − 𝑇) is the estimated value of last sampling time. 𝜓𝑚 (𝑡) is the estimated value by magnetometer sensor in (15). 𝜓𝑔 (𝑡) is the estimated value by gyro sensor in (24). It is worthwhile to note that the roll and pitch angles estimated by accelerometer are prior to calculating horizontal plane projection of magnetic field 𝑚𝑝 with reference to (14). Then the magnetometer-based yaw 6 Mathematical Problems in Engineering angle could be determined with reference to (15). 𝛼1 and 𝛼2 are the weight coefficients of the first-order low pass and high pass filter; 𝑘1 is cutoff bandwidth of low pass filter; 𝑘2 is cutoff bandwidth of high pass filter. Considering the higher frequency for noises of magnetometers, the high frequency noises of magnetometer are eliminated in low pass filter. Furthermore, the high pass filter is preferred to eliminate the lower frequency noises of gyroscope. In this way, the yaw angle could be estimated accurately depending on fusion of gyroscope and magnetometer in case that GPS signals is unavailable. The switching criterion is determined through detecting the vehicle acceleration and GPS-measured velocity. Different measurements are provided in IKF. The initial system noise statistics 𝑄 matrix can be determined based on Allan standard variance of angle random walk about system model. The statistic characteristic of measurement noise could be determined by using Allan variance method; angular velocity random walk and bias instabilities were crucial noise in MEMS sensors. The initial measurement noise statistics R matrix can be determined based on the observation noise of measurement acceleration bias instabilities. 𝐵𝑥 and 𝐵𝑦 are bias instabilities of accelerometer. The initial values of 𝑅 and 𝑄 are given by 03×3 03×3 ], 𝑄=[ 03×3 𝐶3×3 −2 𝐶3×3 = 10 5.693 8.485 3.059 [8.485 1.265 4.559] ∗[ ], [3.059 4.559 1.644] 0 0 0 [ ] ] 𝑅=[ [0 𝐵𝑥 ⋅ 𝐵 𝐵𝑥 ⋅ 𝐵𝑦 ] [0 𝐵𝑦 ⋅ 𝐵𝑥 𝐵𝑦 ⋅ 𝐵𝑦 ] −4 = 10 (26) 0 0 0 [0 4.942 4.175] ∗[ ]. [0 4.175 3.528] The IKF algorithm from transition process to steady state would adjust the magnitude of 𝑄 matrix by high gain scale, and not only depending on initial value. 2.6. The Tuning of High Gain Scale. In order to resolve the problem of robust control and characters of the uncertain noise statistics influence, an intelligent Kalman filter (IKF) is proposed and it, just like the traditional EKF, does not require a constant time step, nor synchronous measurements. Define 𝑥𝑘 as the state vector, 𝑧𝑘 as the vector of measurement, and 𝜔𝑘 and ]𝑘 as the process and measurement noises whose covariance is given by 𝑄𝑘 and 𝑅𝑘 , respectively. The transition and measurement models are expressed as follows: 𝑥̂𝑘,𝑘−1 = 𝑓 (𝑥̂𝑘−1 , 𝜔𝑘 ) , 𝑃𝑘,𝑘−1 = Φ𝑘,𝑘−1 𝑃𝑘−1 Φ𝑇𝑘,𝑘−1 + 𝛾𝑘 L𝑘 𝑄𝑘−1 L𝑘 , −1 𝐾𝑘 = 𝑃𝑘,𝑘−1 𝐻𝑘𝑇 (𝐻𝑘 𝑃𝑘,𝑘−1 𝐻𝑘𝑇 + o𝑘 𝑅𝑘 o𝑘 ) , 𝑥̂𝑘 = 𝑥̂𝑘,𝑘−1 + 𝐾𝑘 [𝑧𝑘 − ℎ (𝑥̂𝑘,𝑘−1 )] , 𝑃𝑘 = 𝑃𝑘,𝑘−1 − 𝐾𝑘 𝐻𝑘 𝑃𝑘,𝑘−1 . (27) The state vector x has 6 elements. The matrices L𝑘 and o𝑘 are diagonal. The variable 𝛾𝑘 is the high gain scale (1 ≤ 𝛾𝑘 ≤ 𝛾max ). Define 𝐽𝜏,𝑘 as the innovation sequence, and 𝜏 is the calculation length of moving window. The parameter 𝑛 is the first sampling time with a sampling period. 𝐸𝜏,𝑘 is the variation between the innovation sequence and threshold value at sample time k, which reflects the influence of measurement noise 𝑘 2 𝐽𝜏,𝑘 = ∑ (𝑡𝑖+1 − 𝑡𝑖 ) 𝑧 (𝑡𝑖 ) − 𝑧̃ (𝑡𝑖 ) , (28) 𝐸𝜏,𝑘 = 𝐽𝜏,𝑘 − 𝐽threshold . (29) 𝑖=𝑛 The high gain scale is adjusted by fuzzy controller according to the innovation sequence 𝐽𝜏,𝑘 and variation 𝐸𝜏,𝑘 . 𝛾𝑘 = Rfuzzy (𝐽𝜏,𝑘 ) . (30) The following analysis is given concerning the adjustments of high gain scale 𝛾𝑘 . The performance of the extended Kalman filter applied to nonlinear systems is highly dependent on measurement and process noise variance values. In practical application of the Kalman filter a priory information about measurement noise statistics (R matrix) is usually known, because it depends on the quality of measurement sensors used. The information about system noise statistics (𝑄 matrix) is quite subjective, because it depends on the model representation created by a developer. Kalman filter error does not depend on the selection of 𝑄 or 𝑅 absolute levels but on their relationship [21]. As a result, the reasonable way to analyze the noise statistics influence on the estimation errors is based on 𝑄 matrix influence interpretation when 𝑅 matrix is fixed. The estimation process of the Kalman filter contains two stages. The first stage is a transition process of the estimation; the second one is a steady state. In practical applications, the true behavior of some state vector components is unknown. In order to select the reasonable magnitude of 𝑄 matrix, the following procedure would be suggested. During the first stage of Kalman filter, the larger value of variation 𝐸𝜏,𝑘 is obtained. The high gain scale 𝛾𝑘 should tend to increase its value until it tends to maximum value 𝛾max . With reference to (27), it is equivalent to increase elements of Q matrix in first stage. Correspondingly, the magnitude of the elements of 𝑃𝑘,𝑘−1 and 𝐾𝑘 increase; it means that the weight of measurements in estimation process has to be high in order to reduce the influence of input noise on current estimates. In this way, the effect of faster convergence could be realized; meanwhile, the Kalman filter is in steady state from transition process. Furthermore, the Kalman filter provides the estimates with constant accuracy at the steady state, which cannot be improved with time. Corresponding, the variation 𝐸𝜏,𝑘 would be close to small value from transition process to steady Mathematical Problems in Engineering 7 Table 1: Fuzzy control rules for the output 𝑈. 𝐸 𝑈 NB VS NS S Z M PS B PB VB process. The high gain scale 𝛾𝑘 should tend to reduce and tend to small constant value; the smaller elements of 𝑄 matrix should be chosen. Then the magnitude of the elements of 𝑃𝑘,𝑘−1 and 𝐾𝑘 reduced. Consequently, it is expectable that the gain matrix norm ‖𝐾𝑘 ‖ in the Kalman filter has to be close to small value for the effective measurement noise smoothing. A fuzzy controller with one-dimension is developed to adjust high gain scale 𝛾𝑘 . The inputs of the fuzzy controller are the variation 𝐸𝜏,𝑘 and its output is high gain scale 𝛾𝑘 . According to above analysis of Kalman filter adjustment in transition process and steady state, a set of fuzzy rules to adjust high gain scale 𝛾𝑘 can be produced. The linguistic variables assigned to the variation 𝐸𝜏,𝑘 and high gain scale 𝛾𝑘 are 𝐸 and 𝑈, respectively. The corresponding fuzzy subsets are 𝐴 and 𝐵. The set 𝐴 comprises NB, NS, Z, PS, and PB, which denote negative big, negative small, zero, positive small, and positive big, respectively. The set 𝐵 comprises VS, S, M, B, and VB, which denote very small, small, medium, big, and very big, respectively. The fuzzy rules for input 𝐸 and output 𝑈 are shown in Table 1. The fuzzy rule table of high gain scale is generated according to fuzzy rules offline. The output of fuzzy controller is got using online enquiry rule table for real time control. 3. Experimental Results 3.1. Prototype of Navigation System. The prototype of navigation system in our lab, which was composed of DSP/FPGA microcontroller, power module, and MEMS IMU, was developed. The MEMS sensors comprise MEMS IMU of three single-axis gyroscopes, accelerometers, and triaxis magnetometer. The design of prototype was shown in Figure 2. Six layers for FPGA/DSP microcontroller and a fourlayer PCB for integration of all MEMS sensors are developed in lab. The FPGA controller is responsible for data resolution, processing of a single antenna GPS receiver, and MEMS IMU. The DSP controller is used for processing the calculation of navigation algorithms in real time. The relating algorithms were implemented on both Ti TMS320C6713B and Altera EP2C35F484C6, which were necessary for real time operation. The parallel bus interface is used for data communication between DSP and FPGA controller. The size of embedded navigation system is 𝜙 80 × 30 mm. Micro IMU provides a higher sensor performance after calibration procedure and error compensation. 3.2. Calibration Method and Testing. Due to the limitation of processing technology level, detecting circuit realization, the low cost Micro IMU is restricted to low precision. MEMS gyroscope has larger biases and is less accurate. It is sensitive to environmental effects. The performances of the low cost MEMS IMU are worse than that of the conventional fibreoptic-based or ring laser gyro inertial sensors. It is essential MEMS IMU PCB DSP/FPGA PCB Figure 2: The prototype of navigation system. that the calibration and error compensations of Micro IMU are implemented before attitude estimation. The deterministic errors of low cost Micro IMU, which are composed of bias, scale factors, nonorthogonality, or misalignment errors, should be removed. The raw data from Micro IMU could be preprocessed by means of effective calibration method to eliminate the effects of these errors. Firstly, the Micro IMU device was mounted inside of a metal cylinder. The deterministic error parameters of the gyroscope were determined depending on calibration experiments by mounting the Micro IMU to single rotation table or three-axes rotation table in Figure 3. The different biases in each setting angular speed are calculated by the positive and negative cancellation method. The scale factor and misalignment errors are also estimated through a similar method. After the calibration process offline, the deterministic errors of the raw measurements of gyroscope are compensated online. For the calibration of accelerometer, the sensitive axis of accelerometer was aligned in 18 positions, which comprise 6 orthogonal positions to the ground and the other 12 declined positions. Before calibration, the maximal error of raw measurement for the accelerometer is within 3𝑔. After calibration, the bias has been removed utilizing the method of mean value and the least square method. The compensated norm of three-axis accelerometer using least square method has better than method of mean value. Its norm or calibrated measurements of accelerometer remain close to 1𝑔. The calibration experiment for magnetometer was also implemented [22]. The calibration and compensation procedure for the deterministic errors could be done offline by using rate table before applications. The sensor characteristics of stochastic errors in low MEMS gyro and accelerometer could be identified using Allan variance [23]. The raw sensor data of low cost micro IMU are captured with a sample rate of more than 100 Hz. Utilizing the comparison of the loglog graph of Allan variance, the random walk that appears with a gradient of −0.5 is dominant noise for MEMS gyro and accelerometer. The root mean square random drift errors of Allan variance are calculated, which would be provided to IKF as initial process and measurement noises. 3.3. Experimental Setup and Performance Testing. The experimental platform, which comprises the prototype of DSP/ FPGA micro-controller, low cost MEMS sensors, a single 8 Mathematical Problems in Engineering MEMS IMU MEMS IMU Cylinder 45∘ Inclined Block Three axes rotation table Figure 3: Calibration experiment of MEMS IMU. Navigation prototype Mti-G Sec. 1 Sec. 2 Satellite antenna Sec. 3 End Figure 4: Experimental platform of navigation system. antenna GPS receiver, and the Xsens Mti-G, is set up. The vehicle experiment is performed to verify the effect of the proposed IKF attitude estimation method. It is shown in Figure 4. The Xsens Mti-G device from the Xsens Motion Technologies [24], which is a commercially available Attitude Heading Reference System (AHRS) composed of MEMS IMU and magnetometer, is used to provide reference attitude calculation results for comparison. The maximum sampling frequency 100 Hz is set to collect raw data from Xsens Mti-G. The GPS receiver works in autonomous mode and measurements are provided at 10 Hz sampling frequency. The experimental devices are mounted in car roof. The experimental data are collected by means of a laptop PC with the USB port from converting the RS232 serial port. Vehicle tests were performed to verify the performance of the proposed method. The experiment was carried out on the cases that single antenna GPS worked normally, vehicle was in turning with large sideslip angle, and GPS signals were affected and lost by obstruction. The Vehicle test routing was Start Figure 5: Test field trajectory with single antenna GPS. chosen in a district of countryside outskirt of city. The test trajectory is presented in Figure 5. The start point is located at 123.4479∘ E and 41.6744∘ N. The results of on-vehicle test are presented from 0 s to100 s. The performances of traveling in straight and turning sections are compared considering that the accuracy of attitude calculation suffered from accelerometer’s signals distortion noise disturbance and large sideslip angle. The proposed algorithms are implemented in DSP/FPGA development prototype. The consuming period of calculation in a sampling time is less than 10 ms. The vehicle traveled through two turns during two intervals of 43.3–55.5 s and 68–83 s marked in yellow line in section 1 and section 2. The GPS signal was blocked subjectively by man-made near 89 s in order to verify the effect of the proposed algorithm in GPS outage. It is marked in red line from 89 to 95 s in section 3. The Mathematical Problems in Engineering 9 5 40 0 30 Pitch angle (degree) Roll angle (degree) −5 −10 −15 −20 −25 10 0 −10 −20 −30 −35 20 0 10 20 30 40 50 60 Time (s) 70 80 90 100 −30 0 20 30 40 50 60 Time (s) 100 70 80 90 100 90 100 Acc Gyro-Euler Ref Gyro-Quaternion Acc Gyro-Euler Ref Gyro-Quaternion 10 6 5 Yaw angle error (degree) Yaw angle (degree) 50 0 −50 4 3 2 1 0 −1 −100 0 10 20 30 40 50 60 70 80 90 100 −2 0 10 20 30 Time (s) Ref Gyro-Quaternion 40 50 60 70 80 Time (s) Gps Gyro-Euler GPS Gyro-Euler Gyro-Quaternion Figure 6: Attitude estimates from different sensors. attitudes estimated separately by different sensors, which comprise MEMS gyroscope, accelerometer, and GPS receiver, are shown in Figure 6. Data denoted as “Ref,” “GyroQuaternion,” “Acc,” “Gyro-Euler,” and “GPS,” respectively, are derived from referenced Mti-G, gyroscope integration based on quaternion algorithm, accelerometer, gyroscope integration based on Euler algorithm, and the yaw angle measured by GPS-pseudoattitude algorithm. Figure 6 shows that the gyro-based attitudes derived from quaternion and Euler algorithm have short-term accuracy, which suffers from long-term accumulated errors caused by gyroscope bias drift with reference to (5) and (16). The attitude algorithm by accelerometer could provide roll and pitch estimation of long-term accuracy but suffers from sensor errors and vibration noises due to inevitable disturbances and dynamic motions with reference to (10). The pseudoattitude algorithm based on GPS could provide heading angle as measurement yaw angle via GPS-measured velocity with reference to (17); its maximal measured error of yaw even reaches 4.65 degree in turning section. It is seen that the pitch and roll estimate by accelerometer is better than others. The error of yaw estimate based on GPS is smaller than others in straight line (0–43 s). The statistics of attitude angle estimation errors using different sensors are summarized in Table 2. The estimate attitude accuracy only depending on single type sensor is not satisfied. Then, it is very vital to fuse these sensors and design the fusion filtering methods to improve effectively the estimation performance. According to the results estimated, the attitude algorithm from accelerometer could provide pitch and roll angle in low dynamic condition. The attitude algorithm from gyroscope could estimate Euler angles for only short-term accuracy. In addition, the method fusing the yaw angle derived from single antenna GPS and magnetometer has long-term accuracy. For attitude estimate of adopting fusion algorithms for MEMS INS/GPS system, complementary filter (CF) method based on Wahba’s problem is frequently used for attitude fusion solution in order to achieve a trade-off between static and dynamic performances [25]. In most practical cases, the independent fusion information of different sensors is not 10 Mathematical Problems in Engineering Table 2: Attitude angle estimation errors using different sensors. Different sensors Roll RMS Pitch RMS Yaw RMS Roll STD Pitch STD Yaw STD Gyro-Quaternion Gyro-Euler Acc. GPS 11.7087 15.9387 0.6693 2.7292 12.6745 17.3281 0.6486 3.4792 2.5189 2.9679 8.4046 8.0346 0.3127 0.5783 12.1537 8.5878 0.2969 0.4084 1.6373 1.5683 1.7906 1.535 Table 3: The STD in IKF and EKF. STD Sec. 1 Roll Pitch Yaw EKF IKF EKF IKF EKF IKF 0.556494 0.526692 0.644948 0.335493 1.317089 0.48039 Sec. 2 1.251228 0.377351 0.782464 0.546662 1.078504 0.283883 Sec. 3 Total 0.469554 0.6502 0.176453 0.3035 0.580741 0.7171 0.116373 0.3347 0.723493 1.4932 0.649624 1.1556 sufficient for accurate estimation. Therefore, Kalman filter (KF) is proposed in order to take full advantage of attitude differential equation for reliable estimation. To realize better refusion estimation, the combining methodology of “Fusion + KF” is widely adopted in the literature. The representative methods are categorized as follows. For example, the federated Kalman filters are designed to the dynamic condition to realize global fusion estimation [17]. It has obvious advantages over the traditional centralized EKF algorithm, but heavy calculation burden could not satisfy the demands of real time processing. The other most widely researched method is that the attitude estimate using CF fusion algorithm is utilized as the state coefficient [26] or measurement vector [16, 17, 25] in modified Kalman filter, which could be regarded as one benchmark algorithm in the field of GPS and INS data fusion. Among this category algorithm, the quaternion estimation algorithm (QUEST) [3, 11, 25] is usually used as attitude algorithm for Micro IMU, which could remove the adverse effect by linear acceleration of the navigation system to attain good dynamic performance. In order to verify the effect of the propose method, the fusion algorithm based complementary filter for gyroscope and accelerometer (Fusion Acc&Gyro) and the conventional EKF method of fix gain are compared in Figure 7. Data denoted as “Ref,” “Fusion-Complementary,” “EKF-FixGain,” and “IKF&Fusion,” respectively, are derived from referenced Mti-G, the CF fusion algorithm for fusing gyroscope and accelerometer, the conventional EKF method of fix gain, and the proposed method. For CF fusion algorithm of gyroscope and accelerometer, it can be seen that the complementary filter is able to provide accurate roll and pitch estimate in low dynamic condition utilizing characters of accelerometers that has longterm accuracy and gyroscope that that has only short-term accuracy. The accuracy of the pitch and roll angle is corrupted by the forward acceleration and lateral acceleration. The STD of the pitch and roll angle is 0.63∘ and 0.64∘ , respectively, at all times. Considering yaw angle is unable to be estimated by complementary filter for fusing gyroscope and accelerometer, it is calculated taking advantage of roll and pitch estimates of complementary filter in (24). The estimate yaw angle introduces large error originated from low cost gyro bias drifts and estimate errors of roll and pitch. Moreover, the conventional EKF method of fix gain is implemented in experiments. The same state and measurement equations are adopted in EKF and IKF. The differences lie in control strategies to deal with vehicle turning and GPS outage. Additionally, the fix gain is adopted in EKF. The process and measurement noise covariance matrices are not tuned in transition process. As observed from yaw estimate error, the yaw estimation error results of the IKF and EKF are similar to the reference system in straight line section (0–43 s), but the EKF has large errors in vehicle turning and GPS outage. The maximal yaw error of vehicle turning in EKF is about 4.5 degree due to the influence of large sideslip angle. The STD of EKF is 0.65∘ , 0.71∘ , and 1.49∘ . The STD of the proposed IKF for sections 1, 2, and 3 is 0.48∘ , 0.28∘ , and 0.65∘ . Table 3 lists the STD of estimate errors in EKF and IKF. The high gain scale is in fast adjustment in transition process by fuzzy controller in IKF. It is illustrated in Figure 8. It is shown that the high gain scale 𝛾𝑘 is increased from initial value 1 to maximum 9.52 in transition process and it is decreased in steady process. The sampling time 𝑛 in (28) is assigned to 10, which is used to make sure that all the measurement data could be given in fuzzy controller. The high gain scale is adjusted thrice in one innovation. After the transition process is ended, its value is not adjusted and stable at 1.32 in steady process. Thus, the IKF algorithm has the benefits of adaptability which increases the value of 𝛾𝑘 according to the innovation and thus improves its convergence capabilities. Compared with the above two methods, the proposed IKF can take the advantages of different fusion strategies and provide more accurate and reliable estimation whether in normal condition, unavailable sideslip, and GPS failure. On the straight trajectory of driving situation, GPS-measured yaw angle could be reliable to provide accurate measurement for IKF ignoring the effect of sideslip. In turning sections (43.3–55.5 s, 68–83 s), the valid values of GPS-measured vehicle velocity are not convenient to separate from large Mathematical Problems in Engineering 11 −0.5 5.0 −1.0 4.5 −1.5 4.0 Roll angle (degree) −2.0 Pitch angle (degree) −2.5 −3.0 −3.5 −4.0 −4.5 −5.0 3.5 3.0 2.5 2.0 1.5 −5.5 1.0 −6.0 0.5 −6.5 0 10 20 30 40 50 60 Time (s) Ref Fusion-Complementary 70 80 90 0.0 100 0 10 20 30 40 50 60 70 80 90 100 Time (s) Ref Fusion-Complementary EKF-Fix Gain IKF & Fusion 100 EKF-Fix Gain IKF & Fusion 6 5 Yaw angle error (degree) Yaw angle (degree) 50 0 −50 4 3 2 1 0 −1 −100 0 10 20 30 40 50 60 70 80 90 100 −2 0 20 Time (s) Ref Fusion-Complementary 40 60 80 100 Time (s) EKF-Fix Gain IKF & Fusion EKF-Fix Gain IKF & Fusion Fusion-Complementary Figure 7: Attitude estimates from different fusion algorithms and filters. 10 8 k 6 4 2 0 0 5 10 15 20 Iterative times 25 30 Figure 8: The regulating process of the high gain scale in IKF. noises due to the decreasing velocity in turning section. Its value is not accurate. Besides, the larger sideslip angle occurred, which is the difference between the velocity heading and the vehicle heading. It is not calculated for only single antenna GPS. Depending on switching fusion strategy, the yaw estimate by fusing accelerometer, gyroscope, and magnetometer is used for substituting the GPS-measured yaw angle as measurement in IKF. The same strategy is adapted to GPS outage section (89–95 s). The accurate estimation is provided in turning section or GPS outage. In order to further validate the proposed method, the above-mentioned benchmark algorithm based on QUEST is selected for comparison. The quaternion vector 𝑞 and the bias of gyroscope 𝑏𝑡 are chosen as state vector considering the effect of the model error in benchmark algorithm. The 𝑇 state vectors are expressed in 𝑥 = [𝑞 𝑏𝑡 ]7×1 . Depending on the concept of basic structure of the refusion attitude information “CF + KF”, the attitude estimates based on CF fusion algorithm are utilized as filter measurement to correct the state quaternion vector 𝑞 and the bias of gyroscope 12 Mathematical Problems in Engineering 1.5 1.0 0.5 Pitch angle error (degree) Roll angle error (degree) 1.0 0.5 0.0 −0.5 −1.0 0.0 −0.5 −1.0 −1.5 −1.5 0 20 40 60 80 100 0 20 40 Time (s) 60 80 100 Time (s) Benchmark algorithm IKF & Fusion Yaw angle error (degree) Benchmark algorithm IKF & Fusion 5.5 5.0 4.5 4.0 3.5 3.0 2.5 2.0 1.5 1.0 0.5 0.0 −0.5 −1.0 −1.5 0 20 40 60 80 100 Time (s) Benchmark algorithm IKF & Fusion Figure 9: Attitude estimate errors of the proposed method and benchmark algorithm. 𝑏𝑡 in EKF. The fusion attitudes 𝜙fusion , 𝜃fusion , which have been calculated by CF method based on gyroscope and accelerometer, would be chosen as the measurement vector of filter. Due to the reliable yaw estimation by the GPS receiver, the GPS-measured yaw angle 𝜓GPS by the pseudoattitude method is also chosen as measurement vector in filter. Here, 𝑇 the measurement vector 𝑧 = [𝜙fusion 𝜃fusion 𝜓GPS ] . Figure 9 displays the attitude estimation errors. It is inferred from Figure 9, due to the reliable refusion information as the measurements (𝜙fusion , 𝜃fusion ), selected benchmark algorithms based on QUEST and the proposed method have similar low dynamic performance for estimating the roll and pitch angle. The RMS of roll and pitch in IKF is 0.31∘ and 0.33∘ , respectively. The RMS of roll and pitch in benchmark algorithm is 0.66∘ and 0.6∘ , respectively. The precision of the proposed method is slightly better than benchmark algorithm. For yaw estimation, benchmark algorithm becomes worse in turning sections (43.3–55.5 s, 68–83 s) owing to the effect of gyro drift in quaternion calculation. In the GPS outage section (89–95 s), the estimated value of last sampling time 𝜓̂𝑘−1 is used for replacing the 𝜓GPS in the measurement process. The precision of benchmark algorithm is unaffected due to short GPS outage time. RMS of benchmark algorithm is 2∘ and RMS of independent CF method is 2.97∘ . Benchmark algorithm attains better performance than only CF method for yaw estimation. Compared with the performance of turning sections and GPS outage section, the proposed IKF gets remarkable improvement due to the tuning of high gain scale in filter. It maintains satisfactory estimation performance in whole section. The STD and RMS for the various methods tested in attitude estimation experiments are summarized in Table 4. Compared with different sensors, fusion algorithm of complementary filter, the conventional EKF, and benchmark algorithm, it can provide relatively accurate and reliable attitude estimation. The STD of the yaw estimation errors is Mathematical Problems in Engineering 13 Table 4: Attitude angle estimation errors using fusion and filter. Fusion algorithm EKF-Fix Gain Fusion-Complementary (CF) Benchmark algorithm IKF and Fusion Roll RMS 0.6764 0.6377 0.6603 0.3059 Pitch RMS 0.7324 0.6442 0.6001 0.3337 almost within 1.15∘ ; roll and pitch estimation errors are almost within 0.33∘ in whole section. 4. Conclusions The combined attitude determination approach with fusion estimation algorithm and intelligent Kalman filter on low cost MEMS IMU and magnetometer and single antenna GPS is proposed. The deterministic errors of low cost MEMS sensors are eliminated effectively before attitude estimation through calibration and error compensation. The different control strategies fusing the MEMS multisensors are implemented in case of normal driving, GPS failure, and unavailable sideslip. An independently development prototype is designed for experimental tests. The proposed method provide reliable yaw estimate as measurement for IKF observer, particularly unavailable larger sideslip angle and GPS failure. The performance of the proposed methodology has been evaluated and verified through compared experiments for different sensors attitude algorithms and fusion methods. The proposed method is compared with traditional representative methods, which comprises the gyro based on quaternion, the gyro based on Euler, accelerometer, GPS-pseudoattitude algorithm and fusion algorithm based complementary filter, benchmark algorithm based on QUEST, and so on. The experimental results indicate that the proposed method exhibits reliable and satisfactory performance. Conflicts of Interest The authors declare that they have no conflicts of interest. Acknowledgments This work is supported by Shenyang Science and Technology Plan Project, China. Corresponding experimental tests are carried out at Research Centre of Weaponry Science and Technology in Shenyang Ligong University. The authors would like to thank their colleagues for their support in implementing the experiments. References [1] Z. Wu, M. Yao, H. Ma, W. Jia, and F. Tian, “Low-cost antenna attitude estimation by fusing inertial sensing and two-antenna GPS for vehicle-mounted satcom-on-the-move,” IEEE Transactions on Vehicular Technology, vol. 62, no. 3, pp. 1084–1096, 2013. [2] J. F. Vasconcelos, C. Silvestre, and P. Oliveira, “INS/GPS aided by frequency contents of vector observations with application to autonomous surface crafts,” IEEE Journal of Oceanic Engineering, vol. 36, no. 2, pp. 347–363, 2011. Yaw RMS 1.7355 2.9679 2.006 1.3049 Roll STD 0.6502 0.2359 0.2636 0.3035 Pitch STD 0.7171 0.2539 0.2912 0.3347 Yaw STD 1.4932 1.5691 1.6940 1.1556 [3] Y.-C. Lai and S.-S. Jan, “Attitude estimation based on fusion of gyroscopes and single antenna GPS for small UAVs under the influence of vibration,” GPS Solutions, vol. 15, no. 1, pp. 67–77, 2011. [4] W. Chou, B. Fang, L. Ding, X. Ma, and X. Guo, “Two-step optimal filter design for the low-cost attitude and heading reference systems,” IET Science, Measurement and Technology, vol. 7, no. 4, pp. 240–248, 2013. [5] F. Qin, X. Zhan, and L. Zhan, “Performance assessment of a lowcost inertial measurement unit based ultra-tight global navigation satellite system/inertial navigation system integration for high dynamic applications,” IET Radar, Sonar and Navigation, vol. 8, no. 7, pp. 828–836, 2014. [6] H. Huang, X. Chen, and J. Zhang, “Weight Self-adjustment Adams Implicit Filtering Algorithm for Attitude Estimation Applied to Underwater Gliders,” IEEE Access, vol. PP, no. 99, 2016. [7] R. Zhang, F. Hoflinger, and L. M. Reind, “Calibration of an IMU using 3-D rotation platform,” IEEE Sensors Journal, vol. 14, no. 6, pp. 1778–1787, 2014. [8] Z. Zhang and G. Yang, “Calibration of miniature inertial and magnetic sensor units for robust attitude estimation,” IEEE Transactions on Instrumentation & Measurement, vol. 63, no. 3, pp. 711–718, 2014. [9] P. Aggarwal, Z. Syed, X. Niu, and N. El-Sheimy, “A standard testing and calibration procedure for low cost MEMS inertial sensors and units,” The Journal of Navigation, vol. 61, no. 2, pp. 323–336, 2008. [10] S. Wei, G. Dan, and H. Chen, “Altitude data fusion utilising differential measurement and complementary filter,” IET Science, Measurement & Technology, vol. 10, no. 8, pp. 874–879, 2016. [11] M. B. Del Rosario, N. H. Lovell, and S. J. Redmond, “Quaternion-Based Complementary Filter for Attitude Determination of a Smartphone,” IEEE Sensors Journal, vol. 16, no. 15, pp. 6008– 6017, 2016. [12] H. No, A. Cho, and C. Kee, “Attitude estimation method for small UAV under accelerative environment,” GPS Solutions, vol. 19, no. 3, pp. 343–355, 2014. [13] J.-H. Yoon, S. Eben Li, and C. Ahn, “Estimation of vehicle sideslip angle and tire-road friction coefficient based on magnetometer with GPS,” International Journal of Automotive Technology, vol. 17, no. 3, pp. 427–435, 2016. [14] K. D. Sebesta and N. Boizot, “A real-time adaptive high-gain EKF, applied to a quadcopter inertial navigation system,” IEEE Transactions on Industrial Electronics, vol. 61, no. 1, pp. 495–503, 2014. [15] Z. Wu, Z. Sun, W. Zhang, and Q. Chen, “A Novel Approach for Attitude Estimation Based on MEMS Inertial Sensors Using Nonlinear Complementary Filters,” IEEE Sensors Journal, vol. 16, no. 10, pp. 3856–3864, 2016. [16] F. Hoflinger, J. Muller, and M. Tork, “A wireless micro inertial measurement unit (IMU),” IEEE Transactions on Instrumentation & Measurement, vol. 62, no. 9, pp. 2583–2595, 2013. 14 [17] X. Li, C.-Y. Chan, and Y. Wang, “A Reliable Fusion Methodology for Simultaneous Estimation of Vehicle Sideslip and Yaw Angles,” IEEE Transactions on Vehicular Technology, vol. 65, no. 6, pp. 4440–4458, 2016. [18] M. M. Teshnizi and A. Shirazi, “Attitude estimation and sensor identification utilizing nonlinear filters based on a low-cost mems magnetometer and sun sensor,” Aerospace & Electronic Systems Magazine, vol. 30, no. 12, pp. 20–33, 2015. [19] H. Jie, H. Xianlin, and W. Guofeng, “Design and application of single-antenna GPS/accelerometers attitude determination system,” Journal of Systems Engineering and Electronics, vol. 19, no. 2, pp. 220–227, 2008. [20] R. P. Kornfeld, H. R. John, and J. J. Deyst, “Single-antenna GPSbased aircraft attitude determination,” Navigation: Journal of the Institute of Navigation, vol. 45, no. 1, pp. 51–60, 1998. [21] O. S. Salychec, Applied Inertial Navigation: Problem and Solutions, BMSTU Press, 2004. [22] L. Wang and F. Wang, “Intelligent calibration method of low cost mems inertial measurement unit for an FPGA-based navigation system,” International Journal of Intelligent Engineering & Systems, vol. 4, no. 2, pp. 32–41, 2011. [23] N. El-Sheimy, H. Y. Hou, and X. J. Niu, “Analysis and modeling of inertial sensors using allan variance,” IEEE Transactions on Instrumentation and Measurement, vol. 57, no. 1, pp. 140–149, 2008. [24] http://www.xsens.com/products/mit-g-710. [25] H. Rong, J. Lv, C. Peng et al., “Dynamic Regulation of the Weights of Request Based on the Kalman Filter and an Artificial Neural Network,” IEEE Sensors Journal, vol. 16, no. 23, pp. 8597– 8607, 2016. [26] D. Nemec, A. Janota, M. Hrubos, and V. Simak, “Intelligent Real-Time MEMS Sensor Fusion and Calibration,” IEEE Sensors Journal, vol. 16, no. 19, pp. 7150–7160, 2016. Mathematical Problems in Engineering Advances in Operations Research Hindawi Publishing Corporation http://www.hindawi.com Volume 2014 Advances in Decision Sciences Hindawi Publishing Corporation http://www.hindawi.com Volume 2014 Journal of Applied Mathematics Algebra Hindawi Publishing Corporation http://www.hindawi.com Hindawi Publishing Corporation http://www.hindawi.com Volume 2014 Journal of Probability and Statistics Volume 2014 The Scientific World Journal Hindawi Publishing Corporation http://www.hindawi.com Hindawi Publishing Corporation http://www.hindawi.com Volume 2014 International Journal of Differential Equations Hindawi Publishing Corporation http://www.hindawi.com Volume 2014 Volume 2014 Submit your manuscripts at https://www.hindawi.com International Journal of Advances in Combinatorics Hindawi Publishing Corporation http://www.hindawi.com Mathematical Physics Hindawi Publishing Corporation http://www.hindawi.com Volume 2014 Journal of Complex Analysis Hindawi Publishing Corporation http://www.hindawi.com Volume 2014 International Journal of Mathematics and Mathematical Sciences Mathematical Problems in Engineering Journal of Mathematics Hindawi Publishing Corporation http://www.hindawi.com Volume 2014 Hindawi Publishing Corporation http://www.hindawi.com Volume 2014 Volume 2014 Hindawi Publishing Corporation http://www.hindawi.com Volume 2014 #HRBQDSDĮ,@[email protected] Journal of Volume 201 Hindawi Publishing Corporation http://www.hindawi.com Discrete Dynamics in Nature and Society Journal of Function Spaces Hindawi Publishing Corporation http://www.hindawi.com Abstract and Applied Analysis Volume 2014 Hindawi Publishing Corporation http://www.hindawi.com Volume 2014 Hindawi Publishing Corporation http://www.hindawi.com Volume 2014 International Journal of Journal of Stochastic Analysis Optimization Hindawi Publishing Corporation http://www.hindawi.com Hindawi Publishing Corporation http://www.hindawi.com Volume 2014 Volume 2014

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

Download PDF

advertisement