This paper describes integrated navigation system that is based on a low cost inertial sensor, global positioning system (GPS) receiver, magnetometer and a barometer, in order to improve accuracy of complete attitude and navigation solution. The main advantage of integration consists in availability of reliable navigation parameters during the intervals of absence of GPS data. The magnetometer and the barometer are applied for the attitude calibration and vertical channel stabilization, respectively. The acceptable accuracy of inertial navigation system (INS) is achieved by the proper damping of INS errors. The integration is made by the implementation of an extended Kalman filter (EKF) with control signal that is designed appropriate for low accuracy sensors noise characteristics. The analysis of integrated navigation system performances is made experimentally and the results show that integrated navigation system provides continuous and reliable navigation solutions.

Keywords:  Navigation,  strapdown inertial navigation system,  global positioning system,  extended Kalman filter,  Barometer,  Magnetometer 

Inertial navigation system is an autonomous system for determining the position and velocity of an object using three linear accelerometers and three rate gyroscopes¹. In this paper ‘Strap-down’ INS (SDINS) is used for analyze and testing. The tradeoff for a low cost INS is its high noise and low accuracy². Errors in SDINS have the character of slow variable oscillations and are not influenced by external factors^2,3. Many papers had discussed the integration of navigation sensors⁴, Kalman filter (KF) for fusion of INS and GPS^4,5 and fusion of different sensors^6,7.

Recent research efforts have been focused on using a low-cost strap-down IMU. Farrell and Barth² introduced a very general method to establish an error process model for the INS/GPS navigation system. Salychev³, et al. used external heading information to align the IMU. Some techniques for attitude determination are based on a blending of accelerometers and magnetometers to compute the attitude⁸. Although this method is particularly suitable for low-cost IMU’s whose gyroscopes are not sensitive to the Earth’s rotation. Ramalingam⁹, et al. presents error modeling and error analysis for a low-cost SDINS.

The mail goal of this research is to develop a module that can be customized for use in different applications and test different algorithms for increasing performance and to reducing cost by using low cost sensors and to improve accuracy of navigation system. Work in this innovation is the integration of GPS, INS, barometric altimeter and magnetometer using the EKF filter. The improvement of EKF estimation was achieved by a control signals with adaptive function.

In this paper has been applied the standard loosely coupled integration model. The integrated navigation system works in two regimes. During first regime, system starts GPS data transferring regarding to the geographical coordinates and velocity. The next step is the initialization of the blocks used for corrections of deterministic errors of inertial instruments (biases, scale factor errors, non-orthogonality, etc.). The next step is alignment in azimuth (using the magnetic compass), calibration of gyro drifts, horizontal and vertical alignment (using baro-altimeter), initial estimation of angular attitude, determination of quaternion parameters, and calculation of transform matrix (DCM) coefficients and determination of accelerometer biases. The last step consists in the initializations of Kalman filters matrices.

The whole alignment phase is done in stationary conditions and its duration was 6 min. The EKF is used for the estimation of INS errors. During the GPS unavailability (620 s - 625 s, 700 s - 705 s, 795 s - 800 s, 840 s - 850 s and 860 s - 885 s (injected for test only)) the EKF works in prediction mode.

The integrated INS/GPS/Barometer/Magnetometer system is based on usage of three mechanical rate gyros (Sfim I1426, of a range up to 20 °/s) and three linear accelerometers (Sfim JT211, of a range up to 20 m/s²). Sampling frequency of inertial sensor data is 100 Hz. Two additional angular sensors (Sfim JC30, _30,) are used for the initial alignment purposes. GPS receiver is of ‘µ-blox GPS-PS1E’ type (C/A code, working frequency L1, updating frequency 1 Hz, and declared accuracy of 5 m). Pressure sensor is BMP 180 with absolute accuracy pressure max 2 hPa in range of  300 hPa - 1100 hPa and temperature range of  0-65 °C. Magnetometer is TCM2-50 with accuracy of magnetic field ±0.2 μT and accuracy of heading when level ±1.0 ° RMS and when tilted ±1.5° RMS with resolution 0.1°.

The position is expressed in geodetic coordinates, where φ, λ and h represent the latitude, longitude and altitude are expressed in radians and altitude in meters. The velocities in the (NED) coordinates, computed in (m/s), are given by [v_N, v_E, v_Ds]. The motion of a vehicle can be described by equations that involve INS kinematics¹⁰. The vector $\Omega -|\varphi ,\theta ,\psi |$  consists of the Euler angles (Roll, Pitch and Yaw). The orientation angles are computed by exploiting the gyroscopes, accelerometers and magnetometer sensors and EKF estimations

In practical applications the accuracy of the INS, caused by the presence of errors depending on the sources of errors can be damping as present in follows.

3.1   Damping of Vertical Channel Errors in SDINS

Equation which describes the behavior of the vertical channels SDINS¹¹, given in local NED coordinate system, can be written as:

${\dot{V}}_D=f_D-\left[\left(2{\omega }_e\cos \varphi +\frac{V_E}{R_p+h}\right)V_E-\left(-\frac{V_N}{R_M+h}\right)V_N\right]+g_{\varphi }(h),$ (1)

where  ${\dot{V}}_D=\frac{d{V}_D}{dt}$- vertical component of velocity changes in time, V_N, V_E – component speed toward the north and east, respectively,$f_D$ - specific force along the vertical axis, φ, h - latitude and altitude,$g_{\varphi }(h)$ - vector of gravitational acceleration. The Eqn. (1), we can write in the form:

${\dot{V}}_D=f_D-\Delta f_D^c+\delta g_{\varphi }(h),$ (2)

where $\Delta f_D^c$ is the member indicated in the square brackets of Eqn. (1) and $\delta g_{\varphi }(h)$  is the gravitational acceleration, can be written in simplified form as:

$\delta g=-2g_0\frac{R_0^2}{{(R_0+h)}^2}\frac{\delta h}{(R_0+h)}=\frac{-2g\delta h}{(R_0+h)}=\frac{-2g\delta h}{R}=-2{\omega }_s^2\delta h,$ (3)

where  ${\omega }_s=\sqrt{\frac{g}{R}}$ is Schuler’s frequency. Based on the above equation to ensure the stability of the vertical channels used an external source of information about the altitude (barometric altimeter), which the measurements are independently filtered¹².
Attenuation of errors in the vertical channel, by using of external information of the altitude, can be described as follows:

$\begin{array}{l}{\dot{V}}_D=f_D-\Delta f_D^c+g_{\varphi }(h)+C_2\delta h,\\ \dot{h}=C_1\delta h-V_{D,}\end{array}$ (4)

where $\delta h=h^{barometer}-h^{INS}$ - a height difference between extern (barometric) altimeter and SDINS, h-height of which is calculated in INS, C₁= 0.51, C₂= 0.8, are constant coefficients

.

3.2   Damping of Horizontal Channel Errors in SDINS

For damping errors in the horizontal channel¹¹, can be used outward speed information that we obtained from the GPS:

$\begin{array}{l}\delta {\dot{V}}_E=-g{\Phi }_N-K_{1N}\delta V_E+B_E,\\ {\dot{\Phi }}_N=-\frac{\delta V_E}{R_p}+K_{2N}\delta V_E-{\omega }_N^{dr},\end{array}$ (5)

where are coefficients K_1N = K_2N = 0.22, R_p- radius of the Earth,  ${\omega }_N^{dr}$ - gyro drift,$B_E$ - ‘bias” accelerometers. By analogy with the Eqn. (5), the correction coefficients K_1E = K_2E = 1.25e-5, are introduced for $\delta {\dot{V}}_E$ and ${\dot{\Phi }}_N$ correction. The optimal choice of coefficients K is based on a compromise between the size of the system static errors and required system bandwidth relative to high-frequency components of gyro errors, on the other.

In this paper the quaternion parameterization is being used. If q_k-1 is the quaternion representing the prior value of attitude, Δq is the quaternion representing the change in attitude, and qk is the quaternion representing the updated value of attitude, than the update equation for quaternion representation of attitude¹³ is

$q_k=\Delta q\times q_{k-1}\times \Delta q^{*},$ (6)

where the postsuperscript * represents the conjugate of a quaternion.

For ‘coning correction’ we used rotation vector technique based on ‘Bortz’ model for attitude dynamics. For low accuracy gyro sensors we determined the damping coefficients for gyro output correction, Kg₁=, Kg₂=0.0124, Kg₃=0.124, as:

${\omega }^b=\left[\begin{array}{ccc}{k}_{g1}{\omega }_x^b& k_{g2}{\omega }_y^b& k_{g3}\end{array}{\omega }_z^b\right]$ (7)

The values of coefficients Kg₁, Kg₂, Kg₃ have been chosen experimentally, based on experience of lot of measurements in stationary and dynamics regime.

4.1   Magnetometer Approach

Integration of magnetic sensor and INS is performed based on Madgwick¹⁴ . Calibration technique for magnetometer is presented by Gebre-Egziabher¹⁵. In our solution we determine coefficients to control influence between outputs of gyro, acceleration and magnetic sensor, Kix= Kiy=1, Kiz=2, (integral gain governs rate of convergence of gyroscope biases) and Kpmx=Kpmy=5, Kpmz=15, (proportional gain governs rate of convergence to accelerometer/magnetometer).

Reference direction of magnetic flux is:

$b_x=\sqrt{h_x^2+h_y^2},b_z=h_z$

$h_{x,y,z}=2\overrightarrow{m}\left[\begin{array}{ccc}0.5-q_2{q}_2-q_3{q}_3& q_1{q}_2-q_0{q}_3& q_1{q}_3+q_0{q}_2\\ q_1{q}_2+q_0{q}_3& 0.5-q_1{q}_1-q_3{q}_3& q_2{q}_3-q_0{q}_1\\ q_1{q}_3-q_0{q}_2& q_2{q}_3+q_0{q}_1& 0.5-q_1{q}_1-q_2{q}_2\end{array}\right],$ (8)

where $\overrightarrow{m}$ is vector of geomagnetic field. Estimated direction of gravity and flux (v and w) is:

$\begin{array}{l}{v}_x=2(q_1{q}_3-q_0{q}_2),\\ v_y=2(q_0{q}_1-q_2{q}_3),\\ v_z=q_0^2-q_1^2-q_2^2+q_3^2,\\ w_x=2b_x(0.5-q_2^2-q_3^2)+2b_z(q_1{q}_3-q_0{q}_2),\\ w_y=2b_x(q_1{q}_2-q_0{q}_3)+2b_z(q_0{q}_1+q_2{q}_3),\\ w_z=2b_x(q_0{q}_2+q_1{q}_3)+2b_z(0.5-q_1^2-q_2^2).\end{array}$ (9)

where b_x and b_z are magnetic fluxes in the horizontal and vertical planes, respectively. Error is sum of cross product between reference direction of fields and direction measured by sensors:

$\begin{array}{l}{e}_x=(a_y{v}_z-a_z{v}_y)+(m_y{w}_z-m_z{w}_y),\\ e_y=(a_z{v}_x-a_x{v}_z)+(m_z{w}_x-m_x{w}_z),\\ e_z=(a_x{v}_y-a_y{v}_x)+(m_x{w}_y-m_y{w}_x).\end{array}$ (10)

where a?_x,y,z – are the measurements of accelerometers. Adjusted gyroscope measurements are:

$\begin{array}{l}{\omega }_x={\omega }_{gyrox}+K_{pmx}\cdot e_x+K_{ix}\cdot e_x+e_{xInt},\\ {\omega }_y={\omega }_{gyroy}+K_{pmy}\cdot e_y+K_{iy}\cdot e_y+e_{yInt},\\ {\omega }_z={\omega }_{gyroz}+K_{pmz}\cdot e_z+K_{iz}\cdot e_z+e_{zInt},\end{array}$ (11)

where e_{x,y,z Int} is integral of error in current iteration.
Corrected value of Δq as the quaternion representing the change of attitude,

$\Delta q=\left[\begin{array}{c}-q_1{\omega }_x-q_2{\omega }_y-q_3{\omega }_z\\ q_0{\omega }_x+q_2{\omega }_z-q_3{\omega }_y\\ q_0{\omega }_y-q_1{\omega }_z+q_3{\omega }_x\\ q_0{\omega }_z+q_1{\omega }_y-q_2{\omega }_x\end{array}\right]\Delta T.$ (12)

In this paper we used EKF where the error model given in Eqns. (1) to (5) has the form¹¹:

$\dot{x}=F\cdot x+G\cdot w.$ (13)

where the state vector is defined as: 

           

$x=\left[\begin{array}{ccccccccccccccc}\delta r_N& \delta r_E& \delta r_D& \delta V_N& \delta V_E& \delta V_D& \delta \phi & \delta \theta & \delta \psi & B_N& B_E& B_D& {\omega }_N^{dr}& {\omega }_E^{dr}& {\omega }_D^{dr}\end{array}\right].$ (14)

Based on matrix F, state transition matrix Φ is:

${\Phi }_k=I+F\Delta t+\frac{1}{2}{F}^2{(\Delta t)}^2,$ (15)

where Δt is a sampling interval. Measurement model is defined as:

$z_k=H_k\cdot x_k+v_k,$ (16)

with values ones and zeroes of terms in H_k, while vector ν_k represents Gaussian measurement noise with zero mean value and covariance matrix R.

Estimated values of error states are obtained by EKF with control signals in the form:

${\widehat{x}}_k^{+}={\Phi }_k{\widehat{x}}_{k-1}^{+}+K_k(z_k-H_k{\Phi }_k{\widehat{x}}_{k-1}^{+}-H_{k}L{u}_k),$ (17)

where L is a matrix consisting from zeroes and ones, multiplying vector of control signals $u_{k-1}=\left[\begin{array}{ccccccccc}0& 0& 0& u_N^{\nu }& u_E^{\nu }& u_D^{\nu }& u_N^{\phi }& u_E^{\phi }& 0\end{array}\right],$ and K_k is a matrix of Kalman gains as , $K_k=P_k^{-}{H}^T{\left[H{P}_k^{-}{H}^T+R_k\right]}^{-1}$ obtained in a standard recursive procedure using prior and posterior state covariance matrices$P_k^{-}={\Phi }_k{P}_{k-1}^{+}{\Phi }_k^T+G_k{Q}_k{G}_k^T$   and$P_k^{+}=(I-K_{k}H)P_k^{-},$ while $Q_k=E\left[w_k{w}_k^T\right],$ is a covariance matrix of system noise.

Control signals uare defined as:

$\begin{array}{l}{u}_N^{\nu }=-k_{1NKF}^{}\tanh (\delta {\widehat{V}}_N),u_E^{\nu }=-k_{1EKF}^{}\tanh (\delta {\widehat{V}}_E),u_D^{\nu }=-k_{DKF}\tanh (\delta h_{}),\\ u_N^{\phi }=-k_{2NKF}^{}\tanh (\delta {\widehat{V}}_E),u_E^{\phi }=-k_{2EKF}^{}\tanh (\delta {\widehat{V}}_N),\end{array}$ (18)

where $k_{1NKF}=k_{2NKF}=0.15,k_{1EKF}=k_{2EKF}=0.125,k_{DKF}=\mathrm{0.005.}$ Velocity components are corrected using the estimates of velocity errors:

$V_N^c=V_N^{INS}-\delta {\widehat{V}}_N,V_E^c=V_E^{INS}-\delta {\widehat{V}}_E,V_D^c=V_D^{INS}-\delta {\widehat{V}}_D,$ (19)

where$\delta {\widehat{V}}_N,\delta {\widehat{V}}_E,\delta {\widehat{V}}_D,$ are velocity error estimates as EKF outputs. Corrections of position components are done as:

${\varphi }^c={\varphi }^{INS}-\delta \widehat{\varphi },{\lambda }^c={\lambda }^{INS}-\delta \widehat{\lambda },h^c=h^{INS}-\delta \widehat{h},$ (20)

where $\delta \widehat{\varphi },\delta \widehat{\lambda },\delta \widehat{h},$ are the estimates of errors of geographical latitude, longitude, and height, obtained as the outputs of EKF. Attitude corrections are made as:

${\text{C}}_B^N=C_P^N{C}_B^P$ (21)

where estimated orientation errors are used to form transformation matrix, $C_P^N$ the transformation matrix $C_B^P$ is obtained as the output of INS, and is $C_B^N$ the resultant transform matrix relating body fixed and navigation coordinate frames, after correction.

Adaptation the values of K_N,KF, K_E,KF and K_D,KF during the intervals of GPS signal absence is done according to the changes of appropriate kinematic parameters sensed by inertial instruments. Therefore, on this step in algorithm, the separation and adaptation of these gain coefficients has been made, introducing K_N1,2, K_E1,2, and C_1,2 as the coefficients for INS correction purpose, and K_N,KF, K_E,KF and K_D,KF for the EKF implementation purpose. This separation becomes effective when there are no available measurements from GPS.

For estimation of sensor’s stochastic models using Allan dispersion and autocorrelation function, test platform ‘CARCO T-922’ was used. Tables 1 and Table 2 summarised the results of error parameters of accelerometers and gyros, respectively. The second experiment consisted in tests where the sensors were mounted on the car moving along accurately pre-specified trajectory. During the overall 11 min interval, the system worked 6 min in the initialization regime while the remaining 5 min worked in navigation regime. There were 4 check points (CP₁-CP₄) along the trajectory where geographical coordinates had been previously determined via DGPS. The number of available satellites was more than four.

Figure 1 illustrates the results obtained when we integrated magnetometer and INS. In case INS measurements one can see that the attitude angles deviate with sudden changes, because of sensors that have low accuracy class. The dashed line represents the vehicle orientation angles. At some points in time, appear sudden deviation angles of orientation, which represent measurement error, caused by vibration and imprecise sensors themselves, which is especially expressed in the roll angle and pitch angles. Yaw angle does not follow the trajectory of the vehicle. In a solution of the integrated system, the orientation angles are not changed suddenly. Azimuth of the vehicle is in accordance with the planned trajectory of the vehicle, as shown by a solid line.

Figure 2 shows the trajectory of the vehicle in geographic coordinates. As can be seen the integrated system successfully follows the trajectory of the vehicle and in the absence of GPS data, which is the main goal, and there are no deviations from the control points. In the absence of GPS information prediction of the EKF is with a margin of error, so that the rms of North position error is 1.31 m and East position error is 1.66 m, under conditions when the vehicle maneuver.

Figure 3 illustrates the results of the altitude along the path of the vehicle. The vehicle is initially went down and then climbed to the highest point and then went back down to the starting position. During the absence of GPS data, the EKF correctly predict the path of movement using barometer.

Vertical position error is 0.635 m (rms). This example shows damping of vertical channel errors in SDINS (solid line) using a barometric altimeter and control signal u_D. Root-mean-square error of vertical velocity in integrated system (bold line) respect to the GPS velocity (dotted line) is 0.023 m/s. Root-mean-square errors of vehicle velocity in north and east direction are 0.046 m/s and 0.042 m/s, respectively.

The main goal of this research was to develop a module that can be customized for use in different applications and to improve accuracy of navigation system which has been confirmed by experiment. By integrating magnetometer, reduction in attitude error has been confirmed. There we determine error dumping coefficients for low accuracy gyro sensors error correction and proportional gain coefficients governs rate of convergence to accelerometer/magnetometer. Experiments have shown that the proposed method can improve the attitude and navigation accuracy performance of low-cost sensors. For error dumping in vertical and horizontal channel, we defined gain coefficients that are different from the coefficients in EKF control signal. In that way we get very well measurements in GPS data absence. Also we develop software in Matlab environment for integrated system that can be used in real applications.

1.     Zhang, P.; Gu, Jason; Milios, E. E. & Huynh, P. Navigation with IMU/GPS/Digital Compass with unscented kalman filter. In Proceedings of the IEEE International Conference on Mechatronics & Automation Niagara Falls, Canada, 2005.

2.     Farrell, J. A. & Barth, M. The global positioning system and inertial navigation. McGraw-Hill, 1998.

3.     Salychev, O.S.; Voronov, V.V.; Cannon, M.E. & Lachapelle, G. Low cost INS/GPS integration: Concepts and testing. In Proceeding of the Institute of Navigation National Technical Meeting, Anaheim, CA, 2000. pp. 98-105.

4.     Allerton, D.J. & Jia, H. A review of multisensor fusion methodologies for aircraft navigation systems. Journal Navigation, 2005, 58(3), 405-417. [Full text via CrossRef]

5.     Seraji, H. & Serrano, N. A multisensor decision fusion system for terrain safety assessment. IEEE Trans.  Robotics, 2009, 25(1), 99-108. [Full text via CrossRef]

6.     Babu, R. & Wang, J. Ultra-tight GPS/INS/PL integration: a system concept and performance analysis. GPS Solutions, 2009, 13(1), 75-82. [Full text via CrossRef]

7.     Reddy, G.S. & Saraswat, V.K. Advanced navigation system for aircraft applications. Def. Sci. J., 2013, 63(2), 131-137. [Full text PDF]

8.     Knedlik, S.; Zhou, J.; Dai, Z.; Ezzaldeen, E. & Loffeld, O. Analysis of low-cost GPS/INS for bistatic SAR experiments. In Proceedings of the 21st International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS 2008), Savannah, USA, September 2008; pp. 2115–2122.

9.     Ramalingam, R.; Anitha, G. & Shanmugam, J. Microelectromechanical systems inertial measurement unit error modeling and error analysis for low-cost strapdown inertial navigation system. Def. Sci. J., 2009, 59(6), 650-658. [Full text PDF]

10.   Falco, G.; Einicke, G.A.; Malos, J.T. & Dovis, F. Performance analysis of constrained loosely coupled GPS/INS integration solutions. Sensors, 2012, 12(11), 15983-16007. [Full text via CrossRef]

11.   Stancic, R. & Graovac, S. The integration of strap-down INS and GPS based on adaptive error damping. Robotics Autonomous Syst., 2010, 58(10), 1117-1129. [Full text via CrossRef]

12.   Daduc, V.; Reddya, B.V.; Sitaraa, B.; Chandrasekhara, R.S. & Reddya, G.S. Baro-INS integration with kalman filter. In Proceedings of National conference on Advances in Sensors for Aerospace applications, Hyderabad, December 2007.

13.   Titerton, D.H. & Weston, J.L. Strapdown inertial navigation technology. Ed 2. The Institution of Electrical Engineers, Herts UK, 2004.

14.   Madgwick, S.O.H.; Vaidyanathan, R. & Harrison, A. J.L. An efficient orientation filter for IMU and MARG sensor arrays. Department of Mechanical Engineering, University of Bristol, 2010.

15.   Gebre-Egziabher, D.; Elkaim, G.H.; David Powell, J. & Parkinson, B. W. Calibration of strapdown magnetometers in magnetic field domain. J. Aerospace Eng., 2006, 19(2), 87–102. [Full text via CrossRef]