A constrained low-cost SINS/OD filter aided with magnetometer is proposed in this paper. The filter is designed to provide a land vehicle navigation solution by fusing the measurements of the microelectromechanical systems based inertial measurement unit (MEMS IMU), the magnetometer (MAG), and the velocity measurement from odometer (OD). First, accelerometer and magnetometer integrated algorithm is studied to stabilize the attitude angle. Next, a SINS/OD/MAG integrated navigation system is designed and simulated, using an adaptive Kalman filter (AKF). It is shown that the accuracy of the integrated navigation system will be implemented to some extent. The field-test shows that the azimuth misalignment angle will diminish to less than 1°. Finally, an outliers detection algorithm is studied to estimate the velocity measurement bias of the odometer. The experimental results show the enhancement in restraining observation outliers that improves the precision of the integrated navigation system.
Foundation of National Key Laboratory of Automotive Simulation and Control201211071. Introduction
The recent advances in sensors technologies and sensor fusion algorithms contributed to the realization of highly integrated low-cost navigation systems. In most systems, GPS signal and the inertial measurement unit (IMU), which consists of three-axe accelerometers and three-axe rate gyros, are fused to estimate the position and velocity of the vehicle [1, 2]. Nevertheless, the SINS/GPS is not a complete self-contained system since the GPS signal may be disturbed or unavailable under some circumstances. The odometer is a common navigation instrument for ground vehicles. High-resolution odometer can afford accurate distance increment, and the velocity measured is bounded and of high accuracy. References [3–5] present the algorithms that use odometer as the external information source to revise the accumulated SINS velocity error.
In an attempt to cut down on the cost of navigation systems, MEMS based IMU are always used; see [6, 7]. Today’s MEMS sensors are still much less precise than the expensive one to meet the requirement of pure SINS. In [6], an attitude determination system which is based on a low-cost MEMS IMU, a triad of magnetometers, and a commercial global positioning system (GPS) receiver is studied. In [7], an effective adaptive Kalman filter (AKF) is proposed to improve the computational efficiency and dynamic performance of low-cost MIMU/magnetometer integrated Attitude and Heading Reference Systems (AHRS).
The vehicle attitude can be estimated by using gyros, accelerometers, magnetometers, and the global positioning system (GPS) [8]. In [9], tailored filtering and extended Kalman filters (EKF) are applied for the fusion of inertial measurement system (the gyros, accelerometers, and magnetometers) in the prediction of the vehicle attitude. A MEMS IMU and a two-antenna GPS are fused to determine the vehicle attitude for vehicle-mounted satellite-communication Satcom-on-the-move (SOTM), using an adaptive Euler angle based unscented Kalman filter (UKF) [10]. The observability analysis for MEMS based IMU/GPS from many researches shows that the azimuth angle is unobservable in straight motion with constant vehicle acceleration. To solve this problem, [11] presents integration of low-cost SINS/GPS aided with odometer and heading from two-antenna GPS receiver. The test results show that the integrated systems’ accuracy in both position and heading is improved significantly.
Integrated navigation systems often use the Kalman filter which requires correct measurement noise covariance and system model [12]. But the environment of land vehicles may be extremely complex due to bumpiness roads they suffer. Reference [13] proposes an adaptive Kalman filter (AKF) to avoid the nonliner problem, considering the impact of the dynamic acceleration on the filter. In [14], an adaptive Kalman filter for SINS/OD is studied. In the AKF formulation, the SINS/OD uses the velocity integrate form. Reference [14] also does some research on inspecting and correcting outliers of sensors. The odometer measurement and SINS have been fused with a 9-state Kalman filter when low-cost sensors are used; see [15]. The velocity constraint uses the fact that a vehicle moves along its longitudinal axis only and no vehicle side slip motion is possible. If the precision of velocity measurement degrades over time or the IMU cannot be precisely located at the center of the vehicle, then the fault detection method is necessary to assure the accuracy of the navigation system. In [16], techniques based on fuzzy logic are applied for adjusting the measurement matrix based on its relative quantity. The proposed algorithm is tested and compared to other faulty detection methods and it shows its capability of detecting both soft and hard sensor errors.
In this paper, a fault-tolerant AKF is proposed for low-cost SINS/OD/MAG. The velocity’s differences between SINS and odometer in the navigation frame and azimuth error are regarded as the system’s measurement vector. A closed Kalman filter is adopted to estimate and correct system errors. A fault detection method is proposed to detect possible bias faults of the measurement from sensors. A sensor of Xsens MTi is chosen and studied to conduct vehicle experiments. Results of vehicle tests with simulated biases estimation on the odometer measurement are shown in the paper. The format of this paper is as follows. In Section 2, the orientation determination method and SINS error model are described. In Section 3, the measurement equation of MIMU/OD/MAG is derived. In Section 4, an AKF with fault detection method is proposed. In Section 5, experimental results are presented. Finally, a short conclusion of this paper is drawn in Section 6.
The main task of SINS is to determine orientation angles, velocity, and position of vehicles. The accelerometer measures the vehicle’s specific force fb, in three directions of the body frame relative to the inertial frame. The gyroscope measures the vehicle’s angular velocity ωibb. The superscript b represents the vector measured in the body frame. Both velocity and position of the vehicle are calculated by integrating the specific force once and twice in the navigation frame. An attitude direction cosine matrix (DCM) is often used to transform the specific force to the navigation coordinate frame. The INS attitude, velocity, and position differential equations are discussed in [4]. However, due to the large bias and sensor noises of the MEMS sensors, the orientation angles results may drift quickly. Therefore, a magnetometer aided inertial navigation algorithm is discussed to estimate the gyros’ drift.
When the system is stationary, the direction of the gravity and local magnetic field can be used to stabilize the attitude [17] and the attitude angles θ and ξ can be calculated as(1a)θξ=sin-1fxgsin-1fyfz.
Then, the magnetic heading is determined as (1b)Ψc=-tan-1mycosθsinξ-mzsinθmxcosξ+mysinθsinξ+mzcosθsinξ,where fx, fy, and fz are the accelerometer sensed specific force in right-forward-up body frame. mx, my, and mz are the geomagnetic vector measured in the body frame.
In practice, the magnetometer needs to be calibrated for soft iron and hard iron effects which has already been carried out by a calibration procedure based on ellipse-specific fitting error compensation method (MFM) inside the MTi; see [18]. Actually, (1a) and (1b) only work effectively under stationary or low acceleration conditions. The dynamic acceleration of the vehicle will influence the gravity measurement in the body frame and that will lead to an inaccurate inclination (roll/pitch). Therefore, the electric compass (which consists of three-direction accelerometers and magnetometers) and gyros are usually fused as the Attitude and Heading Reference Systems (AHRS) to get a better estimate of the attitude angle.
2.2. Error State Model
The SINS error model is commonly used by the design of Kalman process model. In the present proposed system, the errors are always defined in the navigation frame. Choose the local geographic frame ENU (East-North-Up) as the navigation frame. The body frame is defined by the VBF frame (the vehicle-fixed body frame, which is aligned with its longitudinal, lateral, and vertical axes), assuming that the IMU is exactly located at the center of the vehicle. The error propagation equations for attitude, velocity, and position in the navigation coordinate system can be expressed by the following equations:(2)ϕ˙n=-ωien+ωenn×ϕn+δωien+δωenn-Cbnεgb,δv˙n=Cbnfb×ϕn-2δωien+δωenn×vn-2ωien+ωenn×δvn+Cbnεab,δP˙=δξvn+ξδvn,where ϕn=θξψ, ϕ is the Euler angle error of the attitude; δvn and δPT represent the velocity and position error, respectively; ωien, ωenn are the earth rotation rate vector and transport rate vector in n frame; Cbn is the DCM from b frame to n frame; εgb and εab are the random constant error of gyros and accelerometers, respectively; fb is the specific force vector measured in the b frame. ξ is the transformation matrix from the vehicle velocity to the angular: (3)ξ=1RM+h000secLRN+h0001,where RM and RN are the meridian and uniaxial circle radius of the earth, respectively.
In order to estimate the biases of the gyroscopes and the accelerometers and the error of the scale factor, the 16-dimension state vector is chosen as follows:(4)X=ϕnTδvnTδPTεgbTεabTδKT,where δK is the scale factor of odometer which contains both constant and random error [19]. The error of gyroscopes and accelerometers and the scale factor of odometer can be regarded as a constant within a certain period time. Then we have(5)ε˙g=0ε˙a=0δk˙=0.
The error state model is then defined as(6)X˙=ΦX+w,where Φ is the state matrix [20, 21] and w is the state noise vector which is assumed to be a Gaussian white noise.
3. Measurement Equation of SINS/OD/MAG3.1. Odometer Measurements
The output of odometer is the distance increment ΔSm at time interval tm-1,tm along the vehicle forward direction. The odometer measurement is defined as vyb=ΔSm/tm-tm-1, which can be seen as a continuous variable without loss of generality. Assuming that the vehicle does not slip and will not leave the ground, the velocity in the x- and z-axes (body frame), vxb and vzb, is constrained to zero mean. Then its measurement equation can be represented as(7)vodb=0vyb0=0ΔSmtm-tm-10.
Considering the attitude error ϕ and the scale factor error δK, the odometer measurement in the n frame can be represented as(8)v~odn=I3×3-ϕ×Cbn1+δKvodb,where ϕ× is the skew-symmetric matrix of ϕ.
Expanding the above equation,(9)v~odn=vodn+δKvodn+vodn×ϕ-δKvodn×ϕ.
With the two-order small quantity term -δKvodn×ϕ assumed negligible, the above equation can be written as(10)v~odn-vodn=δKvodn+vodn×ϕ.
Considering the velocity v~sinsn calculated by SINS,(11)v~sinsn=vsinsn+δvsinsn,where δvsinsn is the SINS velocity error in the n frame.
Velocity residual between SINS and odometer in the n frame is(12)zv=v~sinsn-v~odn=δvsinsn-vodn×ϕ-δKvodn.
Define the measurement matrix of zv as (13)H1=I3×3-vodn×03×9-vodn.Then, (12) can be given in matrix form:(14)zv=H1X+v1,where v1 is the observation noise vector of the odometer.
3.2. Magnetometer Constraints
The heading residual between the electronic compass and gyroscopes can be written as(15)zψ=Ψ~sins-Ψc=-ψ,where Ψ~sins is the azimuth angle calculated by SINS and Ψc is calculated by (1b). Ψ is the azimuth error of the SINS.
However, (1a) and (1b) are only valid under stationary or low acceleration conditions. In [10, 12], a dynamic scalar α is used to yield the optimal performance: (16)α=fb-g,where g is the gravity vector 00gT.
The vehicle dynamic acceleration has the following scenarios:(17)αk=0,α≤σax2+σay2+σaz2,1,σax2+σay2+σaz2≤α≤Th,∞,otherwise,where σax2, σay2, and σaz2 are the variances of accelerometer measurements in the body frame. Th is the trigger threshold determined by experiments and the design requirements. The rule is interpreted as follows: (1) nonacceleration mode: when αk=0, there is no vehicle acceleration, and the yaw angle can be determined by the acceleration and magnetometers; (2) low-acceleration model: when αk=1, the dynamic acceleration could be regarded as a part of measurement noise; (3) high-acceleration model: when αk=∞, the accelerometers measurements are far from the value of the gravity, and then, the yaw angle calculated by (1b) is far away from the truth and the yaw angle is actually determined by the gyros [12].
3.3. System Observation Model
Therefore, with (14) and (15), the measurement vector is given as(18)Zk=zVEzVNzψT.
Then, the measurement equation is obtained as(19)Zk=HkX+v,where(20)X=ϕnTδvnTδPTεgbTεabTδKT,H=M1M203×7M3,M1=100010000,M2=0-vodUnvodNnvodUn0-vodEn00-1,M3=-vodEn-vodNn0,v is the observation noise vector which is assumed to be a Gaussian white noise.
4. Measurements Fusion Using AKF4.1. Adaptive Kalman Filter
An adaptive Kalman filter is designed to get a better vehicle’s position, velocity, and attitude. The brief structure of the integrated system is shown in Figure 1. Consider the following linear discrete system:(21)Xk=Φk,k-1Xk-1+Wk-1,Zk=HkXk+Vk,where Φk,k-1 is the state transition matrix; Wk-1 and Vk are the independent Gaussian white noise with zero mean, respectively: Wk~N0,Q; Vk~N0,R.
Structure of the proposed SINS/OD/MAG.
In conventional KF, the measurement noise covariance matrix R and process noise covariance matrix Q are fixed and known a priori. However, actual circumstances may be complex and beyond predictable. In this paper, an adaptive algorithm is proposed to improve R, considering that the dynamic acceleration in the land vehicles mainly influences the measurement procedures [22]. The proposed algorithm adopts the innovation sequence to estimate R online. The innovation sequence is calculated as(22)εk=Zk-HkX^k/k-1,where X^k/k-1 is the state vector determined by one-step algorithm.
The estimation error and its corresponding covariance accumulate during the propagated stage. The propagated covariance matrix, Pk/k-1, is obtained as(23)Pk/k-1=Φk,k-1Pk-1Φk,k-1T+Qk-1,where Qk-1 is the process noise covariance.
According to KF theory, Pk/k-1 can also be estimated by the innovation sequence with following equation:(24)εkεkT=HkPk/k-1HkT+Rk,where Rk is the measurement noise covariance.
Using above equations, the measurement noise matrix at time k is given as(25)Rk=1N∑k-N+1kεkεkT-HkPk/k-1HkT,where N is the number of samples.
Using above equations, the measurement noise covariance matrix can be estimated online and unexpected bias on the odometer and electric compass can be detected.
4.2. Online Fault Detection for Odometer and Magnetometer
Even so, the accuracy of velocity estimate will still degrade when vehicles slipping or sliding, or when the magnetic field is interfered. Then, a threshold is applied to detect this situation:(26)εki≤CiHkPk/k-1HkT+Rkii,where i=1,2,3 and C is the threshold decided by experiments.
The measurements of sensors are determined as normal when the above formula is established; otherwise, the measurements are outliers. The threshold C is decided by experiment in advance. When outliers appear, the innovation sequence is calculated as(27)ε^k=λkεk,where λk is the correction factor which is given as(28)λk=mini=1,2,3CiHkPk/k-1HkT+Rkiiεki.
By using (27), biases presented in the measurements can be removed. Figure 2 summarizes the flowchart of the proposed algorithm. To test the proposed navigation solution, experimental results are presented in the next section.
Flowchart of the proposed algorithm.
5. Experimental Testing
The integrated navigation system was tested onboard a vehicle which was driven along the path shown in Figure 3. The experimental data were logged by an AHRS (Xsens MTi-30) sensor and the odometer and a precise GPS. These sensors were installed on the experimental platform shown in Figure 4. The Xsens MTi-30 is an AHRS sensor comprised of three-axis MEMS accelerometers, gyros, and magnetometers. The MTi-30 can output original MIMU and magnetometer data and precise attitude angle. The algorithm inside the MTi works well, and then, the attitude information is used to provide the reference attitude to an accuracy of 0.4° (in dynamic). The specifications of the MTi unit are shown in the Table 1 [18]. A magnetic calibration procedure was executed before the experiment, and then the initial attitude was obtained from the MTi. The odometer data was obtained from the CAN bus, using VECTOR CAN bus equipment. A precise GPS was installed on the roof of the vehicle to provide the reference position to an accuracy of 1 m (1σ STD) for comparison. The algorithm was running in a Texas Instruments’ C6000 series digital signal processor (DSP) that was connected to a laptop to display and record the results in real time. The data refresh frequency of the MTi-30 was set to 100 Hz. The filter updating frequency was 50 Hz. In Figures 5–8, the results of the test are shown.
MTi specifications.
Gyroscopes
In-run bias stability
18°/h
Noise density
0.03°/s/√ Hz
Nonlinearity
0.03% FS
Accelerometers
In-run bias stability
40 μg
Noise density
80 μg/√ Hz
Nonlinearity
0.03% of FS
Magnetometer
Noise density
200 μG/√ Hz
Nonlinearity
0.1% FS
Attitude accuracy (dynamic)
Roll/pitch
<0.4°
Yaw
<1°
The path taken by the vehicle. “S” represents the starting point and “E” represents the end point.
Experiment platform. “A” shows the MTi and GPS that are installed on the roof of the vehicle. “B” shows the DSP connected to a laptop.
Attitude estimated from different sensors. Data denoted as “MTi,” “SINS,” and “filtering result” are, respectively, the Euler angles derived from the MTi, SINS, and the proposed SINS/OD/MAG.
Position estimation.
Position estimation performance in the east and north axis, compared with GPS.
Velocity estimation. (a, b) Velocity estimation along the navigation frame east and north axis from AKF and GPS, respectively. (c, d) Mean error of the velocity estimation of the east and north axes with a bias of 10 km/h on the odometer, respectively. (e, f) The velocity estimate results with a bias of 10 km/h on the odometer.
5.1. No Bias on the Odometer
In this test, the attitude, velocity, and position are estimated separately by pure SINS and SINS/OD/MAG with no bias on the odometer. Figure 5 shows the attitude estimation with Euler angle derived from the AHRS, SINS, and the proposed navigation system. It is shown that the attitude derived from the pure SINS which is actually calculated by gyroscopes suffers from the long-term accumulated errors caused by large sensor biases. The pitch and roll angles estimated by gyros are not increasing all the time because of the presence of large vehicle vibration. The yaw angle has nothing to do with vehicle acceleration, but it fluctuates when turning occurs and that will increase the yaw angle error. Thus, it is necessary to fuse these sensors together. By using SINS/OD/MAG, the attitude, velocity, and position error will be implemented to some extent. Seen from Figure 5, the attitude calculated output from the proposed SINS/OD/SINS is stable and can track the reference value. It is shown that the mean error of the pitch, roll, and yaw angle are below 0.44°, 0.39°, and 1°, respectively. The path taken by the vehicle is estimated and shown in Figure 6. To illuminate the effect of the filter, the positions decomposed in east and north axes are compared with a precise GPS in Figure 7. The vehicle travels about 1 km after 150 s. It can be seen that the path estimated by the AKF closely follows the real path and the differences between the GPS and filtering results in east and north directions are less than 10 meters. Figures 8(a) and 8(b) show the velocity estimate with time. From Figures 8(a) and 8(b) it can be seen that, throughout the journey, velocity errors in east and north are around 0 km/h. This validates the performance of the AKF algorithm implemented in this paper. To validate the function of the fault detection method, a bias of 10 km/h on the odometer is tested in the next section.
5.2. A Bias of 10 km/h on the Odometer
In this test, a bias of 10 km/h is simulated on the odometer velocity readings to simulate a faulty odometer. The fault detection algorithm implemented should detect this bias and successfully remove it from the measurement. Figures 8(c) and 8(d) show the accuracy of velocity estimate. Figures 8(e) and 8(f) show the estimate of velocity with and without fault detection method. The bias is simulated on the odometer velocity readings in 50–150 s. During the first 50 seconds, the velocity is precisely estimated and the mean error is around 0.08 m/s. The fault detection method starts functioning soon after the time a bias added. The error increases slowly with time which is as expected owing to the presence of faulty odometer measurement. The threshold is decided by experiment as C=0.950.950.8 before applying the fault detection algorithm. The results validate the performance of the threshold used in the AKF algorithm. It can be seen from Figures 8(e) and 8(f) that the velocity estimated with fault detection method closely follows the actual velocity and that validates that the filter provides high-accuracy velocity estimate even when the vehicle is slipping or sliding.
6. Conclusion
In this paper, a low-cost SINS/OD filter aided with magnetometer is proposed. An adaptive Kalman filter with an outliers detection method is utilized for the fusion of sensors. The error model of SINS/OD has been derived, and the measurement equation is established by analyzing the azimuth error and velocity residuals between the SINS and odometer. A threshold is put up to detect innovation sequence so that any biases presented in the measurements can be removed. Experiment results are shown to demonstrate the performance of the filter. The results are compared with a precise GPS, showing that the integrated system provides high-accuracy position and attitude estimates even under the presence of a bias on the odometer measurement.
When low-cost MIMU is used for SINS, the error will accumulate quickly because of the large MEMS sensor measurement noise. To solve this problem, the magnetometer and odometer are fused together to estimate and revise SINS accumulating errors. The odometer and magnetometer are common navigation instruments for ground vehicles. Therefore, the filter could be realized onboard a car with a core of a Texas Instruments’ C6000 series digital signal processor [23] for real time applications. As a result, the costs for the whole system will be much cheaper than a commercial integrated navigation system [24].
However, a limitation of the proposed method is that the magnetometers cannot be precisely calibrated for the reason that magnet field environment onboard vehicles may be extremely complex. This problem will be focused in our future research.
Competing Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This work was supported by the Foundation of National Key Laboratory of Automotive Simulation and Control (20121107), China.
SahawnehL. R.Al-JarrahM. A.AssalehK.Abdel-HafezM. F.Real-time implementation of GPS aided low-cost strapdown inertial navigation system201161152754410.1007/s10846-010-9501-02-s2.0-79951511877NoureldinA.KaramatT. B.EbertsM. D.El-ShafieA.Performance enhancement of MEMS-based INS/GPS integration for low-cost navigation applications20095831077109610.1109/TVT.2008.9260762-s2.0-64049100405ZhaoC.QinY.YanG.On-the-move alignment for strap-down inertial navigation systemProceedings of the IEEE International Conference on Information and Automation (ICIA '08)June 2008Changsha, ChinaIEEE1428143210.1109/icinfa.2008.46082262-s2.0-54249121437YanG. M.2006Xi’an, ChinaNorthwestern Polytechnic UniversityAbdel-HafezM. F.SaadeddinK.Amin JarrahM.Constrained low-cost GPS/INS filter with encoder bias estimation for ground vehicles applications20155828529710.1016/j.ymssp.2014.12.0122-s2.0-84923108674LiD.LandryR.Jr.LavoieP.Low-cost MEMS sensor-based attitude determination system by integration of magnetometers and GPS: a real-data test and performance evaluationProceedings of the IEEE/ION Position, Location and Navigation Symposium (PLANS '08)May 200810.1109/plans.2008.45700052-s2.0-55349114354ZhaoL.WangQ. Y.Design of an attitude and heading reference system based on distributed filtering for small UAV20132013849873910.1155/2013/4987392-s2.0-84880898303WangL.XuJ.HaoY.LiH.Attitude determination method and experimental research based on MEMS IMU and magnetoresistive sensorProceedings of the 11th IEEE International Conference on Mechatronics and Automation (ICMA '14)August 2014Tianjin, ChinaIEEE1440144510.1109/icma.2014.68859112-s2.0-84906972253CocoS.ChisariG.FalcoP.IraciE.MilitelloS.LaudaniA.Accurate estimation of vehicle attitude for satellite trackingProceedings of the 2014 European Modeling Symposium (EMS '14)2014Pisa, ItalyIEEE40941410.1109/EMS.2014.57WuZ.YaoM.MaH.JiaW.TianF.Low-cost antenna attitude estimation by fusing inertial sensing and two-antenna GPS for vehicle-mounted satcom-on-the-move20136231084109610.1109/TVT.2012.22293062-s2.0-84885086597IlyasM.YangY.-C.QianQ. S.ZhangR.Low-cost IMU/odometer/GPS integrated navigation aided with two antennae heading measurement for land vehicle applicationProceedings of the 25th Chinese Control and Decision Conference (CCDC '13)May 2013Guiyang, ChinaIEEE4521452610.1109/ccdc.2013.65617502-s2.0-84882755230YuanS. M.YangX. D.ChengJ. H.2012Changsha, ChinaNational Defense Industry PressLiW.WangJ.Effective adaptive kalman filter for MEMS-IMU/magnetometers integrated attitude and heading reference systems20136619911310.1017/S03734633120003312-s2.0-84870931157LiW. L.WuW. Q.2008Changsha, ChinaGraduate School of National University of Defense TechnologyDissanayakeG.SukkariehS.NebotE.The aiding of a low-cost strap-down inertial measurement unit using vehicle model constraints for land vehicle applications2001173731747LiX.ZhangW.An adaptive fault-tolerant multisensor navigation strategy for automated vehicles20105962815282910.1109/TVT.2010.20500142-s2.0-77954597353HanS.WangJ.A novel method to integrate IMU and magnetometers in attitude and heading reference systems201164472773810.1017/S03734633110002332-s2.0-825551759302007Document MT0606P Fhttps://www.xsens.com/ products/mti-10-series/WuY. X.GoodallC.EI-SheimyN.Self-calibration for IMU/odometer land navigation: simulation and test resultsProceedings of the International Technical Meeting of the Institute of NavigationSeptember 2010Long Beach, Calif, USA839849SukkariehS.2000Sydney, AustraliaUniversity of SydneyQinY. Y.20064Beijing, ChinaScience PressSetoodehP.KhayatianA.FarjahE.Attitude estimation by separate-bias Kalman filter-based data fusion200457226127310.1017/s037346330400270x2-s2.0-2442510212C6000 Multicore DSP & ARM, TEXAS INSTRUMENTS, December 2016, http://www.ti.com/lsds/ti/processors/dsp/c6000_dsp-arm/overview.pageRIG 300INS/GPS Integrated Navigation Systems, Shanxi Right M&C Technology, December 2016, http://www.xartck.com/English/product.asp