Abstract
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 multi-information 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 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. 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
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
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, .
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
The propagation of the Euler angles with time yields is derived in
By (3), neglecting of the earth rate and the angular velocity of the local-level frame for the spherical earth model, the gyroscope rates are approximately equal as
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
The matrix representing scale factor error for accelerometer and the skew symmetric matrix representing the fixing error of accelerometer are given by
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 inwhere is the gravity. 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
Thus, the roll and pitch angles are determined by the following equations, respectively,
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, , is determined by Costa theorem. The vector is the vehicle velocity in body frame
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
The matrix represents the raw measurement in body frame. represents the calibrated magnetometer output. Here, can be decomposed as , where , , and matrix represent effect of sensor imperfection and magnetic disturbance. The subscript “” means the scale factor error, “” means nonorthogonality and misalignment error, and “” 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
Considering the horizontal plane projection of magnetic field , corresponding representation is given by
Eventually, yaw angle is calculated in
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 accumulating errors induced by gyroscopes bias drift. The differential equations of the quaternion vector are expressed as
Here, is the error quaternion vector, . 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 and , in the form . The Euler angle denoting pseudoattitude algorithm is determined in
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. is the estimation of the complementary filter. and are the estimation by gyro and accelerometer, respectively. The transfer function is first-order high pass filter and is first-order low pass filter. The transfer functions of the filters are as follows
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.
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.
The state model is thus
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 where is the measurement angular rate signal of the 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:where is the GPS-measured yaw angle, is the zero Gaussian white noise with variance , 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
The yaw angle is calculated in (25); correspondingly, the corrected yaw angle is used as measurement vector in intelligent Kalman filter.where , .
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 angle could be determined with reference to (15). and are the weight coefficients of the first-order low pass and high pass filter; is cutoff bandwidth of low pass filter; 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
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:
The state vector x has elements. The matrices and are diagonal. The variable is the high gain scale (). 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
The high gain scale is adjusted by fuzzy controller according to the innovation sequence and variation .
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 . With reference to (27), it is equivalent to increase elements of Q matrix in first stage. Correspondingly, the magnitude of the elements of 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 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 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 , , , , and , which denote negative big, negative small, zero, positive small, and positive big, respectively. The set comprises , , , , and , 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 four-layer 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 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 fibre-optic-based or ring laser gyro inertial sensors. It is essential 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 . 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 .
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 log-log 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 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 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 and section . 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 . The attitudes estimated separately by different sensors, which comprise MEMS gyroscope, accelerometer, and GPS receiver, are shown in Figure 6. Data denoted as “Ref,” “Gyro-Quaternion,” “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 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 long-term 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 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 . 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 in EKF. The fusion attitudes , , 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 by the pseudoattitude method is also chosen as measurement vector in filter. Here, the measurement vector . Figure 9 displays the attitude estimation errors.
It is inferred from Figure 9, due to the reliable refusion information as the measurements (, ), 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 is used for replacing the 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 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.