Outlier rejection for reducing position error in navigation system based on multi sensors

Abstract

A navigation system is critical for the operation of autonomous vehicles. Various sensors must be used to build navigation systems. To fuse multiple sensors, a highly advanced filter algorithm is required. Navigation systems have difficulties in signal-degraded scenarios, where the navigation solution can be degraded by errors such as multipath and obscured signals. This paper proposes a navigation system using a technique to reject the outliers measured by sensors. The navigation system was designed to include sensor modeling, navigation filters, and outlier rejection. The sensors used in the navigation system are the accelerometer, gyroscope, magnetometer, and global positioning system. Before the experiment, a navigation simulation was performed to evaluate the results and validate the proposed navigation algorithm. The results of the experiment verified that the navigation system, including outlier rejection, had less positional error than that of the general navigation system.

Introduction

Recently, extensive research has been devoted toward the development of robots with increased autonomy. Robots that can perform certain tasks autonomously are valuable in many industries in our society. Autonomous robots are particularly important in marine industries. The use and development of diverse marine resources are crucial for the continued economic growth and construction of a highly industrialized society in the future. There is an infinite growth potential in industries related to the development and utilization of various marine spaces. In particular, many countries worldwide are devoting efforts to various marine industries, including the collection of deep ocean resources, marine construction, and offshore plants.

The oceans are a repository of an infinite amount of resources. The vitalization of marine energy development and a sharp increase in the subsea plant market accompanied with the expansion of the construction market for developing oceans requires the execution of an array of tasks in more varied forms in poorer conditions, demanding unmanned surface vehicles (USVs) to perform more diverse and articulate functions. ¹ Such USVs are mainly operated autonomously based on a navigation system. ²

The Kalman filter (KF) is an optimal estimator for a linear system, which is applied to data integration algorithms. ³ However, it operates in an uncertain marine environment and consists of a nonlinear system. Therefore, the extended Kalman filter (EKF), which can be applied in a nonlinear system owing to its nonlinear characteristics, is widely used for calculating fused navigation.^4,5

In general, positions can be tracked for a short period of time using dead reckoning (DR) based on an attitude heading reference system (AHRS) and a speedometer. ⁶ However, since navigation errors are accumulated if only DR is used, research has consistently been conducted on calibrating velocity and position using auxiliary sensors.^7–9 In recent years, there have been ongoing discussions on identifying and removing outliers by fusing the signals of multiple navigation sensors.^10,11 Furthermore, another study has been conducted on developing a navigation system by fusing navigation sensors and preparing against blackouts by identifying outliers. ¹²

In this study, a navigation algorithm that integrates an inertial measurement unit (IMU), a magnetic compass, and a global positioning system (GPS) is designed. Moreover, the GPS was designed to reduce the position error by applying a technique to remove outliers. Finally, an experiment was conducted using the navigation system developed in this study to confirm the effectiveness of the proposed navigation algorithm.

Design of navigation algorithm

Design of inertial navigation system

Figure 1 shows the global and vehicle coordinate systems fixed on a vehicle. The origin of the Earth-fixed coordinate system (North-East-Down, NED) lies on the ground surface and rotates along with the Earth. The x-axis, y-axis, and z-axis face the direction of the Earth’s center. The data of the IMU attached to a vehicle are based on the vehicle coordinate system, whereas the position of the vehicle obtained through GPS is based on the NED.

Figure 1.

Coordinate systems involved in the study.

The inertial navigation system (INS) continuously calculates the posture change from the initial posture by using the angular velocity observation value of the gyroscope. ¹³ First, the angular velocity due to the rotation of the earth measured by the gyro sensor can be expressed by equation (1):

$${{\mathrm{\omega }}_{\mathrm{ie}}}^{\mathrm{n}}=\left[\begin{array}{c}{\mathrm{\omega }}_{\mathrm{e}}\cos \phi \hfill \\ \multicolumn{1}{c}{0}\\ \multicolumn{1}{c}{-{\mathrm{\omega }}_{\mathrm{e}}\sin \phi }\end{array}\right]$$

(1)

where ${\mathrm{\omega }}_{\mathrm{e}}$ is the angular velocity of the earth’s rotation, and $\phi$ is the latitude at which the object is located on the Earth.

The angular velocity due to the motion of an object can be expressed by equation (2):

$${{\mathrm{\omega }}_{\mathrm{en}}}^{\mathrm{n}}=\left[\begin{array}{c}\stackrel{·}{\lambda }\cos \phi \hfill \\ \multicolumn{1}{c}{-\stackrel{·}{\phi }}\\ \multicolumn{1}{c}{-\stackrel{·}{\lambda }\sin \phi }\end{array}\right]$$

(2)

If the angular velocity of the body is calculated by removing the angular velocity component due to the rotation of the earth and the movement of the vehicle from the measured value of the gyro, it can be expressed by the following equation (3):

$$\mathrm{\omega }=\left[\begin{array}{c}{\mathrm{\omega }}_{\mathrm{x}}\hfill \\ \multicolumn{1}{c}{{\mathrm{\omega }}_{\mathrm{y}}}\\ \multicolumn{1}{c}{{\mathrm{\omega }}_{\mathrm{z}}}\end{array}\right]={{\mathrm{\omega }}_{\mathrm{ib}}}^{\mathrm{b}}-{\mathrm{\omega }}_{\mathrm{bias}}-{{\mathrm{C}}_{\mathrm{n}}}^{\mathrm{b}}({{\mathrm{\omega }}_{\mathrm{ie}}}^{\mathrm{n}}+{{\mathrm{\omega }}_{\mathrm{en}}}^{\mathrm{n}})$$

(3)

where $\mathrm{\omega }$ is the actual angular velocity, ${{\mathrm{\omega }}_{\mathrm{ib}}}^{\mathrm{b}}$ is the gyro measurement value, ${\mathrm{\omega }}_{\mathrm{bias}}$ is the gyro bias error, and ${{\mathrm{C}}_{\mathrm{n}}}^{\mathrm{b}}$ is the transformation matrix from the body coordinate system to the navigation coordinate system.

In this paper, the posture was calculated using the quaternion method, which has little error in posture change and does not cause gimbal lock. The composition of the quaternions is as shown in equation (4):

$$\mathrm{q}=[\begin{array}{cccc}{\mathrm{q}}_0\hfill & {\mathrm{q}}_1\hfill & {\mathrm{q}}_2\hfill & {\mathrm{q}}_3\hfill \end{array}{]}^{\mathrm{T}}$$

(4)

The quaternary differential equation to calculate the amount of change of posture from the calculated angular velocity of the body is as equation (5):

$${\stackrel{.}{\mathrm{q}}}_{\mathrm{k}+1}=\frac{1}{2}\mathrm{\Omega }(\mathrm{\omega }){\mathrm{q}}_{\mathrm{k}},\mathrm{\Omega }(\mathrm{\omega })=\left[\begin{array}{cccc}0& -{\mathrm{\omega }}_{\mathrm{x}}& -{\mathrm{\omega }}_{\mathrm{y}}& -{\mathrm{\omega }}_{\mathrm{z}}\\ {\mathrm{\omega }}_{\mathrm{x}}& 0& {\mathrm{\omega }}_{\mathrm{z}}& -{\mathrm{\omega }}_{\mathrm{y}}\\ {\mathrm{\omega }}_{\mathrm{y}}& -{\mathrm{\omega }}_{\mathrm{x}}& 0& {\mathrm{\omega }}_{\mathrm{x}}\\ {\mathrm{\omega }}_{\mathrm{z}}& {\mathrm{\omega }}_{\mathrm{y}}& -{\mathrm{\omega }}_{\mathrm{x}}& 0\end{array}\right]$$

(5)

where $\stackrel{·}{\mathrm{q}}$ means the variance of quaternions.

If the current posture of the quaternion is calculated using the calculated quaternary variance, it can be expressed by equation (6) ¹³ :

$${\mathrm{q}}_{\mathrm{k}+1}={\mathrm{q}}_{\mathrm{k}}\left[\begin{array}{c}{\mathrm{I}}_{3\times 3}(1-\mathrm{\Delta }\frac{{{\mathrm{\theta }}_{\mathrm{k}}}^2}{8})+\frac{1}{2}\mathrm{\Omega }(\mathrm{\omega }){\mathrm{T}}_{\mathrm{s}}\hfill \end{array}\right]{\mathrm{q}}_{\mathrm{k}}$$

(6)

It is possible to calculate the gravity vector of the measured value from the current attitude, and the current roll and pitch angles can be known from the gravity vector. First, an error function is used to know the error that occurs when calculating the gravity vector from the accelerometer.

Equation (7) represents the error function:

$${\mathrm{f}}_{\mathrm{error}}(\mathrm{q},\mathrm{g},\mathrm{a})={{\mathrm{C}}_{\mathrm{b}}}^{\mathrm{n}}(\mathrm{q})\mathrm{g}=({{\mathrm{C}}_{\mathrm{n}}}^{\mathrm{b}}(\mathrm{q}){)}^{\mathrm{T}}\mathrm{g}-({\mathrm{a}}_{\mathrm{b}}-{\mathrm{a}}_{\mathrm{bias}})$$

(7)

where $\mathrm{g}$ is the gravity vector, ${\mathrm{a}}_{\mathrm{b}}$ is the acceleration vector normalized to the measured acceleration measured from the body, and ${\mathrm{a}}_{\mathrm{bias}}$ is the bias error of the acceleration sensor, as in equation (8):

$$\mathrm{g}=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right],{\mathrm{a}}_{\mathrm{b}}=\frac{1}{\sqrt{{{\mathrm{a}}_{\mathrm{x}}}^2+{{\mathrm{a}}_{\mathrm{y}}}^2+{{\mathrm{a}}_{\mathrm{z}}}^2}}\left[\begin{array}{c}{\mathrm{a}}_{\mathrm{x}}\\ {\mathrm{a}}_{\mathrm{y}}\\ {\mathrm{a}}_{\mathrm{z}}\end{array}\right]$$

(8)

The gradient descent method is used to remove the quaternary error by using the acceleration error measured from the error function. The equation for removing the error of the current time from the posture of quaternion of the previous time can be expressed as the following equations (9)–(11):

$${\mathrm{q}}_{\mathrm{z}(\mathrm{k}+1)}={\mathrm{q}}_{\mathrm{k}}-\mu \left(\frac{\nabla {\mathrm{f}}_{\mathrm{error}}(\mathrm{q},\mathrm{g},\mathrm{a})}{\parallel \nabla {\mathrm{f}}_{\mathrm{error}}(\mathrm{q},\mathrm{g},\mathrm{a})\parallel }\right)$$

(9)

$$\nabla {\mathrm{f}}_{\mathrm{error}}(\mathrm{q},\mathrm{g},\mathrm{a})={\mathrm{J}}^{\mathrm{T}}(\mathrm{q}){\mathrm{f}}_{\mathrm{err}}(\mathrm{q},\mathrm{g},\mathrm{a})$$

(10)

$$\mathrm{J}(\mathrm{q})=2\left[\begin{array}{ccc}-{\mathrm{q}}_2\hfill & {\mathrm{q}}_3\hfill & -{\mathrm{q}}_0{\mathrm{q}}_1\hfill \\ \multicolumn{1}{c}{{\mathrm{q}}_1}& {\mathrm{q}}_0\hfill & {\mathrm{q}}_3{\mathrm{q}}_2\hfill \\ \multicolumn{1}{c}{{\mathrm{q}}_0}& -{\mathrm{q}}_1\hfill & -{\mathrm{q}}_2{\mathrm{q}}_3\hfill \end{array}\right]$$

(11)

$\mathrm{\mu }$ in equation (9) is the step size for the gradient descent method. Equation (10) is the equation for calculating the gradient of the error function, and $\mathrm{J}(\mathrm{q})$ in equation (11) is a Jacobian matrix for quaternions.

The posture observation values of the two complementary sensors are fused using the Kalman filter. Figure 2 is the formula showing the structure of the Kalman filter that calculates the posture.

Figure 2.

Kalman filter for orientation calculation.

The state variable estimated by the Kalman filter is set as the posture of the quaternion, and the measured value model $\mathrm{H}$ can be expressed as equation (12):

$$\mathrm{H}=\left[\begin{array}{cccc}1\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 1\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 1\hfill \end{array}\right]$$

(12)

The state transition matrix and the covariance matrix for observation values were set as the error covariance of each posture observation value. In order to obtain numerical stability for the updated posture, a normalization process is performed as in equation (13):

$${\mathrm{q}}^{\ast }=\frac{\mathrm{q}}{\parallel \mathrm{q}\parallel }$$

(13)

In order to calculate the current Euler posture and the coordinate transformation matrix from the finally calculated quaternions, it should be calculated using the coordinate transformation matrix as in equations (14)–(17):

$${{\mathrm{C}}_{\mathrm{b}}}^{\mathrm{n}}=\left[\begin{array}{ccc}{{\mathrm{q}}_0}^2+{{\mathrm{q}}_1}^2+{{\mathrm{q}}_2}^2+{{\mathrm{q}}_3}^2\hfill & 2({\mathrm{q}}_1{\mathrm{q}}_2-{\mathrm{q}}_0{\mathrm{q}}_3)\hfill & 2({\mathrm{q}}_1{\mathrm{q}}_3+{\mathrm{q}}_0{\mathrm{q}}_2)\hfill \\ \multicolumn{1}{c}{2({\mathrm{q}}_1{\mathrm{q}}_2+{\mathrm{q}}_0{\mathrm{q}}_3)}& {{\mathrm{q}}_0}^2-{{\mathrm{q}}_1}^2+{{\mathrm{q}}_2}^2-{{\mathrm{q}}_3}^2\hfill & 2({\mathrm{q}}_2{\mathrm{q}}_3-{\mathrm{q}}_0{\mathrm{q}}_1)\hfill \\ \multicolumn{1}{c}{2({\mathrm{q}}_1{\mathrm{q}}_3-{\mathrm{q}}_0{\mathrm{q}}_2)}& 2({\mathrm{q}}_2{\mathrm{q}}_3+{\mathrm{q}}_0{\mathrm{q}}_1)\hfill & {{\mathrm{q}}_0}^2-{{\mathrm{q}}_1}^2-{{\mathrm{q}}_2}^2+{{\mathrm{q}}_3}^2\hfill \end{array}\right]$$

(14)

$$\begin{array}{c}\mathrm{\theta }=-\mathrm{ta}{\mathrm{n}}^{-1}\left(\frac{-\sin \mathrm{\theta }}{\cos \mathrm{\theta }}\right)=\hfill \\ -\mathrm{ta}{\mathrm{n}}^{-1}\left(\frac{2({\mathrm{q}}_1{\mathrm{q}}_3-{\mathrm{q}}_0{\mathrm{q}}_2)}{\sqrt{{(2({\mathrm{q}}_2{\mathrm{q}}_3+{\mathrm{q}}_0{\mathrm{q}}_1))}^2+{({{\mathrm{q}}_0}^2-{{\mathrm{q}}_1}^2-{{\mathrm{q}}_2}^2+{{\mathrm{q}}_3}^2)}^2}}\right)\hfill \end{array}$$

(15)

$$\begin{array}{c}\mathrm{\phi }=\mathrm{ta}{\mathrm{n}}^{-1}\left(\frac{\sin \mathrm{\phi }}{\cos \mathrm{\phi }}\right)\hfill \\ =\mathrm{ta}{\mathrm{n}}^{-1}\left(\frac{2({\mathrm{q}}_2{\mathrm{q}}_3+{\mathrm{q}}_0{\mathrm{q}}_1)}{{{\mathrm{q}}_0}^2-{{\mathrm{q}}_1}^2-{{\mathrm{q}}_2}^2+{{\mathrm{q}}_3}^2}\right)\hfill \end{array}$$

(16)

$$\begin{array}{c}\mathrm{\psi }=\mathrm{ta}{\mathrm{n}}^{-1}\left(\frac{\sin \mathrm{\psi }}{\cos \mathrm{\psi }}\right)\hfill \\ =\mathrm{ta}{\mathrm{n}}^{-1}\left(\frac{2({\mathrm{q}}_1{\mathrm{q}}_2-{\mathrm{q}}_0{\mathrm{q}}_3)}{{{\mathrm{q}}_0}^2+{{\mathrm{q}}_1}^2-{{\mathrm{q}}_2}^2-{{\mathrm{q}}_3}^2}\right)\hfill \end{array}$$

(17)

In order to obtain the posture of the moving body by inertial navigation, information on the initial posture of the moving body is required. In this paper, the initial posture was calculated using the coarse alignment method. At rest, acceleration has only the gravitational acceleration component. As shown in equations (18) and (19), since the direction of gravity always faces the ground, the initial posture for roll and pitch angles can be calculated from the direction of the gravity vector ¹⁴ :

$$\mathrm{\phi }=\mathrm{ta}{\mathrm{n}}^{-1}\left(\frac{{\mathrm{a}}_{\mathrm{y}}}{{\mathrm{a}}_{\mathrm{z}}}\right)$$

(18)

$$\mathrm{\theta }=\mathrm{ta}{\mathrm{n}}^{-1}\left(\frac{{\mathrm{a}}_{\mathrm{x}}}{\sqrt{{{\mathrm{a}}_{\mathrm{y}}}^2+{{\mathrm{a}}_{\mathrm{z}}}^2}}\right)$$

(19)

To calculate the yaw at rest, the angular velocity of the earth’s rotation is measured from the gyroscope. The earth’s rotation angular velocity ${{\mathrm{\omega }}_{\mathrm{ib}}}^{\mathrm{b}}$ measured by the gyroscope is calculated as follows through the coordinate transformation process from the Earth’s rotational angular velocity ${{\mathrm{\omega }}_{\mathrm{ib}}}^{\mathrm{n}}$ of the navigation coordinate system. ${\mathrm{C}}_1$ is the coordinate transformation matrix of the roll, and ${\mathrm{C}}_2$ is the coordinate transformation matrix of the pitch. By rearranging equation (20), it can be expressed as equation (21):

$${{\mathrm{\omega }}_{\mathrm{ib}}}^{\mathrm{b}}={{\mathrm{C}}_{\mathrm{n}}}^{\mathrm{b}}{{\mathrm{\omega }}_{\mathrm{ib}}}^{\mathrm{n}}={\mathrm{C}}_1{\mathrm{C}}_2{{\mathrm{\omega }}_{\mathrm{ie}}}^{\mathrm{n}}$$

(20)

$${{\mathrm{C}}_1}^{\mathrm{T}}{{\mathrm{\omega }}_{\mathrm{ib}}}^{\mathrm{b}}={\mathrm{C}}_2{{\mathrm{\omega }}_{\mathrm{ib}}}^{\mathrm{n}}$$

(21)

The component of $\mathrm{C}$ can be obtained using the previously obtained roll and pitch, and the yaw calculated from equation (22) is the same as equation (23):

$${{\mathrm{C}}_1}^{\mathrm{T}}\left[\begin{array}{c}{\mathrm{\omega }}_{\mathrm{x}}\hfill \\ \multicolumn{1}{c}{{\mathrm{\omega }}_{\mathrm{y}}}\\ \multicolumn{1}{c}{{\mathrm{\omega }}_{\mathrm{z}}}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\omega }}_1\hfill \\ \multicolumn{1}{c}{{\mathrm{\omega }}_2}\\ \multicolumn{1}{c}{{\mathrm{\omega }}_3}\end{array}\right]={\mathrm{C}}_2{{\mathrm{\omega }}_{\mathrm{ib}}}^{\mathrm{n}}$$

(22)

$$\phi =-\mathrm{ta}{\mathrm{n}}^{-1}\left(\frac{{\mathrm{\omega }}_2}{{\mathrm{\omega }}_1}\right)$$

(23)

In INS, the velocity can be calculated as equation (24):

$${\stackrel{·}{\mathrm{V}}}^{\mathrm{n}}={{\mathrm{C}}_{\mathrm{b}}}^{\mathrm{n}}{\mathrm{a}}_{\mathrm{b}}-\left[\begin{array}{c}2({{\mathrm{w}}_{\mathrm{ie}}}^{\mathrm{n}}\times )+({{\mathrm{w}}_{\mathrm{en}}}^{\mathrm{n}}\times )\hfill \end{array}\right]{\mathrm{V}}^{\mathrm{n}}+{\mathrm{g}}^{\mathrm{n}}$$

(24)

where ${\mathrm{V}}^{\mathrm{n}}$ is the vehicle speed in the navigation coordinate system, ${\mathrm{g}}^{\mathrm{n}}$ is the gravitational acceleration in the navigation coordinate system, and $({{\mathrm{\omega }}_{\mathrm{ie}}}^{\mathrm{n}}\times )$ and $({{\mathrm{\omega }}_{\mathrm{en}}}^{\mathrm{n}}\times )$ are ${{\mathrm{\omega }}_{\mathrm{ie}}}^{\mathrm{n}}$ and ${{\mathrm{\omega }}_{\mathrm{en}}}^{\mathrm{n}}$ are skew-symmetric matrices.

Since the accelerometer receives gravitational acceleration in the direction of the earth’s surface, it is necessary to measure the acceleration by removing the gravitational acceleration component to accurately measure the velocity. In order to know the gravitational acceleration according to the latitude, the gravitational acceleration of the vehicle is calculated from the gravitational model. ¹⁵ If the gravity model for the current gravity according to latitude is arranged by substituting earth-related variables, it can be expressed as equation (25):

$${\mathrm{g}}^{\mathrm{n}}=-9.7803253359\frac{1+0.00193185265241\mathrm{si}{\mathrm{n}}^2\phi }{\sqrt{1-0.00669437999014\mathrm{si}{\mathrm{n}}^2\phi }}$$

(25)

A skew-symmetric matrix with respect to a three-dimensional vector $\mathrm{X}$ having ${\mathrm{x}}_1,{\mathrm{x}}_2,{\mathrm{x}}_3$ as elements can be created as in equation (26):

$$(\mathrm{X}\times )=\left[\begin{array}{ccc}0\hfill & -{\mathrm{x}}_3\hfill & {\mathrm{x}}_2\hfill \\ \multicolumn{1}{c}{{\mathrm{x}}_3}& 0\hfill & -{\mathrm{x}}_1\hfill \\ \multicolumn{1}{c}{-{\mathrm{x}}_2}& {\mathrm{x}}_1\hfill & 0\hfill \end{array}\right]$$

(26)

The movement of the vehicle moving near the ground surface can be obtained from the speed of each axis in the navigation coordinate system. If the amount of change with respect to the current latitude, longitude, and altitude is calculated from the velocity calculated above, it can be expressed as equation (27):

$$\stackrel{·}{\phi }=\frac{{\mathrm{v}}_{\mathrm{n}}}{\mathrm{M}+\mathrm{h}},\stackrel{·}{\lambda }=\frac{{\mathrm{v}}_{\mathrm{e}}}{(\mathrm{N}+\mathrm{h})\cos \phi },\stackrel{·}{\mathrm{h}}=-{\mathrm{v}}_{\mathrm{d}}$$

(27)

where ${\mathrm{v}}_{\mathrm{N}},{\mathrm{v}}_{\mathrm{E}},{\mathrm{v}}_{\mathrm{D}}$ means the speed in each axis in the ground coordinate system, and $\mathrm{h}$ means the altitude. $\mathrm{M}$ is the average radius of curvature of the earth according to latitude and can be calculated from equation (28):

$$\mathrm{M}=\frac{{\mathrm{r}}_{\mathrm{e}}(1-{\mathrm{e}}^2)}{{(1-{\mathrm{e}}^2\mathrm{si}{\mathrm{n}}^2\phi )}^{3/2}}$$

(28)

where ${\mathrm{r}}_{\mathrm{e}}$ means the earth’s semi-major, and e is the earth’s eccentricity, which can be calculated from the earth’s semi-major and semi-minor as in equation (29):

$$\mathrm{e}={\left(\begin{array}{c}1-\frac{{{\mathrm{r}}_{\mathrm{p}}}^2}{{{\mathrm{r}}_{\mathrm{e}}}^2}\hfill \end{array}\right)}^{1/2}$$

(29)

where ${\mathrm{r}}_{\mathrm{p}}$ means the earth’s semi-minor.

$\mathrm{N}$ is the radius of curvature of the cross section of the earth, and it can be expressed by the following equation (30):

$$\mathrm{N}=\frac{{\mathrm{r}}_{\mathrm{e}}}{{(1-{\mathrm{e}}^2\mathrm{si}{\mathrm{n}}^2\phi )}^{1/2}}$$

(30)

In INS, navigation errors occur due to errors in the sensors used for implementation, errors in the initial measured posture, and errors in calculations. In order to compensate for navigation errors, information about the errors is required.

Equation (27), which is a differential equation for position change, can be expressed by differentially linearizing it with the Taylor series. Classifying the components of error in equation (27) can be divided into speed error, latitude error, altitude error, earth’s average radius of curvature error, and earth’s cross-sectional radius of curvature error. The results of error factors are summarized in equations (31)–(33) ¹⁶ :

$$\mathrm{\delta }\stackrel{·}{\phi }=\frac{-{\mathrm{v}}_{\mathrm{n}}{\mathrm{M}}^{\prime }}{(\mathrm{M}+\mathrm{h})}\mathrm{\delta }\phi -\frac{{\mathrm{v}}_{\mathrm{n}}}{{(\mathrm{M}+\mathrm{h})}^2}\mathrm{\delta }\phi +\frac{1}{\mathrm{M}+\mathrm{h}}\mathrm{\delta }{\mathrm{v}}_{\mathrm{n}}$$

(31)

$$\begin{array}{c}\mathrm{\delta }\stackrel{·}{\lambda }=(\tan \phi -\frac{\mathrm{N}\prime }{\mathrm{N}+\mathrm{h}})\mathrm{\delta }\phi +\frac{{\mathrm{v}}_{\mathrm{e}}}{{(\mathrm{N}+\mathrm{h})}^2\cos \phi }\mathrm{\delta }\mathrm{h}\hfill \\ +\frac{1}{{(\mathrm{N}+\mathrm{h})}^2\cos \phi }\mathrm{\delta }{\mathrm{v}}_{\mathrm{e}}\hfill \end{array}$$

(32)

$$\mathrm{\delta }\stackrel{·}{\mathrm{h}}=-\mathrm{\delta }{\mathrm{v}}_{\mathrm{d}}$$

(33)

In equation (31), $\mathrm{M}\prime$ is the average radius of curvature error of the earth due to latitude error, and it can be expressed as equation (34):

$$\mathrm{M}\prime =\frac{3{\mathrm{r}}_{\mathrm{e}}(1-{\mathrm{e}}^2){\mathrm{e}}^2\sin \phi \cos \phi }{{(1-{\mathrm{e}}^2\mathrm{si}{\mathrm{n}}^2\phi )}^{5/2}}$$

(34)

$\mathrm{N}\prime$ in equation (32) is an error in the radius of curvature of the earth’s cross section due to latitude error and can be expressed as equation (35):

$$\mathrm{N}\prime =\frac{{\mathrm{r}}_{\mathrm{e}}{\mathrm{e}}^2\sin \phi \cos \phi }{{(1-{\mathrm{e}}^2\mathrm{si}{\mathrm{n}}^2\phi )}^{3/2}}$$

(35)

In the velocity calculation formula of equation (24), error factors can be divided into torsional error of current posture, bias error of acceleration measurement, error of angular velocity vector due to earth rotation vector and antibody motion, and velocity error. The velocity error model is shown in equation (36):

$$\begin{array}{c}\mathrm{\delta }{\mathrm{V}}^{\mathrm{n}}={{\mathrm{C}}_{\mathrm{b}}}^{\mathrm{n}}\nabla +{{\mathrm{C}}_{\mathrm{b}}}^{\mathrm{n}}{\mathrm{f}}_{\mathrm{c}}\mathrm{\delta }\mathrm{\Phi }\hfill \\ -[2(\mathrm{\delta }{{\mathrm{\omega }}_{\mathrm{ie}}}^{\mathrm{n}}\times )+(\mathrm{\delta }{{\mathrm{\omega }}_{\mathrm{en}}}^{\mathrm{n}}\times )]{\mathrm{V}}^{\mathrm{n}}\hfill \\ -[2({{\mathrm{\omega }}_{\mathrm{ie}}}^{\mathrm{n}}\times )+({{\mathrm{\omega }}_{\mathrm{en}}}^{\mathrm{n}}\times )]\mathrm{\delta }{\mathrm{V}}^{\mathrm{n}}\hfill \end{array}$$

(36)

where ∇ is the random bias error of the accelerometer, $\mathrm{\delta }\mathrm{\Phi }$ is the torsional error with respect to attitude, $\mathrm{\delta }{\mathrm{V}}^{\mathrm{n}}$ is the velocity error, and $\mathrm{\delta }{{\mathrm{\omega }}_{\mathrm{ie}}}^{\mathrm{n}}$ and $\mathrm{\delta }{{\mathrm{\omega }}_{\mathrm{en}}}^{\mathrm{n}}$ are the Earth rotation vector and the vehicle motion. It means the error of the angular velocity vector. At this time, the errors for $\mathrm{\delta }{{\mathrm{\omega }}_{\mathrm{ie}}}^{\mathrm{n}}$ and $\mathrm{\delta }{{\mathrm{\omega }}_{\mathrm{en}}}^{\mathrm{n}}$ can be expressed by equations (37) and (38):

$${{\mathrm{\omega }}_{\mathrm{ie}}}^{\mathrm{n}}=\left[\begin{array}{c}-{\mathrm{\omega }}_{\mathrm{e}}\sin \phi \mathrm{\delta }\phi \hfill \\ \multicolumn{1}{c}{0}\\ \multicolumn{1}{c}{-{\mathrm{\omega }}_{\mathrm{e}}\cos \phi \mathrm{\delta }\phi }\end{array}\right]$$

(37)

$${{\mathrm{\omega }}_{\mathrm{en}}}^{\mathrm{n}}=\left[\begin{array}{c}\stackrel{·}{\lambda }(\frac{-{\mathrm{N}}^{\prime }\cos \phi }{\mathrm{N}+\mathrm{h}})\mathrm{\delta }\phi +\frac{\stackrel{·}{\lambda }\cos \phi }{\mathrm{N}+\mathrm{h}}+\frac{1}{(\mathrm{N}+\mathrm{h})\cos \phi }\mathrm{\delta }{\mathrm{v}}_{\mathrm{e}}\hfill \\ \multicolumn{1}{c}{\frac{{\mathrm{v}}_{\mathrm{n}}{\mathrm{M}}^{\prime }}{{(\mathrm{M}+\mathrm{h})}^2}\mathrm{\delta }\phi +\frac{{\mathrm{v}}_{\mathrm{n}}}{{(\mathrm{M}+\mathrm{h})}^2}\mathrm{\delta }\mathrm{h}-\frac{1}{(\mathrm{N}+\mathrm{h})\cos \phi }\mathrm{\delta }{\mathrm{v}}_{\mathrm{n}}}\\ \multicolumn{1}{c}{-\stackrel{·}{\lambda }\left(\begin{array}{c}\tan \phi -\frac{{\mathrm{N}}^{\prime }}{\mathrm{N}+\mathrm{h}}+\cos \phi \hfill \end{array}\right)\mathrm{\delta }\phi -\stackrel{·}{\lambda }\frac{\sin \phi }{\mathrm{M}+\mathrm{h}}\mathrm{\delta }\mathrm{h}-\frac{\tan \phi }{{(\mathrm{N}+\mathrm{h})}^2}\mathrm{\delta }{\mathrm{v}}_{\mathrm{e}}}\end{array}\right]$$

(38)

The posture error is in the form of a differential equation of the torsional error generated from the reference posture, and can be expressed by equation (39):

$$\mathrm{\delta }\stackrel{·}{\mathrm{\Phi }}=-(\left[\begin{array}{c}{{\mathrm{\omega }}_{\mathrm{ie}}}^{\mathrm{n}}+{{\mathrm{\omega }}_{\mathrm{en}}}^{\mathrm{n}}\hfill \end{array}\right]\times )\mathrm{\Phi }-{{\mathrm{C}}_{\mathrm{b}}}^{\mathrm{n}}\mathrm{\epsilon }+\mathrm{\delta }({{\mathrm{\omega }}_{\mathrm{ie}}}^{\mathrm{n}}+{{\mathrm{\omega }}_{\mathrm{en}}}^{\mathrm{n}})$$

(39)

The error between the accelerometer and the gyro is an irregular error and can be expressed by equations (40) and (41):

$$\mathrm{\delta }{\mathrm{a}}_{\mathrm{b}}=\nabla +{\mathrm{\omega }}_{\mathrm{a}}$$

(40)

$$\mathrm{\delta }{{\mathrm{\omega }}_{\mathrm{ib}}}^{\mathrm{b}}=\mathrm{\epsilon }+{\mathrm{\omega }}_{\mathrm{g}}$$

(41)

where ∇ is the random bias error of the accelerometer, $\mathrm{\epsilon }$ is the random bias error of the gyro, and ${\mathrm{\omega }}_{\mathrm{a}}$ and ${\mathrm{\omega }}_{\mathrm{g}}$ are the white noise of the accelerometer and gyro.

The system model uses the error model of the INS.^7,17 The EKF is used to fuse the INS and the GPS. The state variable is estimated by the Kalman filter is as shown in equation (42) ¹⁸ :

$$\mathrm{X}={\left[\begin{array}{c}\mathrm{\delta }{\mathrm{P}}_{\mathrm{INS}}\mathrm{\delta }{\mathrm{V}}_{\mathrm{INS}}\mathrm{\delta }\mathrm{\Phi }\nabla \mathrm{\epsilon }\hfill \end{array}\right]}^{\mathrm{T}}$$

(42)

where $\mathrm{\delta }{\mathrm{P}}_{\mathrm{INS}}$ is the position error vector of the INS, $\mathrm{\delta }{\mathrm{V}}_{\mathrm{INS}}$ is the velocity error vector of the INS, $\mathrm{\delta }\mathrm{\Phi }$ is the position torsional error of the INS, ∇ is the random bias error of the accelerometer, and $\mathrm{\epsilon }$ is the random bias error of the gyroscope.

The equations of state of the Kalman filter are as shown in equations (43) and (44) ⁷ :

$$\mathrm{X}(\mathrm{k})={\stackrel{`}{\mathrm{F}}}_{\mathrm{INS}}(\mathrm{k})+{\mathrm{w}}_{\mathrm{INS}}$$

(43)

$${\mathrm{F}}_{\mathrm{INS}}(\mathrm{k})=\left[\begin{array}{ccccc}{\mathrm{F}}_{\mathrm{pp}}\hfill & {\mathrm{F}}_{\mathrm{pv}}\hfill & 0_{3\times 3}\hfill & 0_{3\times 3}\hfill & 0_{3\times 3}\hfill \\ \multicolumn{1}{c}{{\mathrm{F}}_{\mathrm{vp}}}& {\mathrm{F}}_{\mathrm{vv}}\hfill & {\mathrm{F}}_{\mathrm{va}}\hfill & {\mathrm{F}}_{\mathrm{v}{\mathrm{b}}_{\mathrm{a}}}\hfill & 0_{3\times 3}\hfill \\ \multicolumn{1}{c}{{\mathrm{F}}_{\mathrm{ap}}}& {\mathrm{F}}_{\mathrm{av}}\hfill & {\mathrm{F}}_{\mathrm{aa}}\hfill & 0_{3\times 3}\hfill & {\mathrm{F}}_{\mathrm{a}{\mathrm{b}}_{\mathrm{g}}}\hfill \\ \multicolumn{1}{c}{0_{3\times 3}}& 0_{3\times 3}\hfill & 0_{3\times 3}\hfill & 0_{3\times 3}\hfill & 0_{3\times 3}\hfill \\ \multicolumn{1}{c}{0_{3\times 3}}& 0_{3\times 3}\hfill & 0_{3\times 3}\hfill & 0_{3\times 3}\hfill & 0_{3\times 3}\hfill \end{array}\right]$$

(44)

The process noise ${\mathrm{w}}_{\mathrm{INS}}$ has an average of 0, which can be expressed as shown in equation (45) below, using the gyroscope with the covariance ${\mathrm{Q}}_{\mathrm{INS}}$ and the white noise of the accelerometer:

$${\mathrm{w}}_{\mathrm{INS}}=\left[\begin{array}{cccccccc}{0}_{1\times 3}\hfill & {\mathrm{w}}_{{\mathrm{a}}_{\mathrm{n}}}\hfill & {\mathrm{w}}_{{\mathrm{a}}_{\mathrm{e}}}\hfill & {\mathrm{w}}_{{\mathrm{a}}_{\mathrm{d}}}\hfill & {\mathrm{w}}_{{\mathrm{g}}_{\mathrm{n}}}\hfill & {\mathrm{w}}_{{\mathrm{g}}_{\mathrm{e}}}\hfill & {\mathrm{w}}_{{\mathrm{g}}_{\mathrm{d}}}\hfill & 0_{1\times 6}\hfill \end{array}\right]$$

(45)

${\mathrm{F}}_{\mathrm{INS}}$ can be expressed using equations (46)–(55) from the error model depicted above:

$${\mathrm{F}}_{\mathrm{pp}}=\left[\begin{array}{ccc}\frac{-{\mathrm{v}}_{\mathrm{n}}{\mathrm{M}}^{\prime }}{{(\mathrm{M}+{\mathrm{M}}^{\prime +}\mathrm{h})}^2}\hfill & 0\hfill & \frac{-{\mathrm{v}}_{\mathrm{n}}}{{(\mathrm{M}+\mathrm{h})}^2}\hfill \\ \multicolumn{1}{c}{\frac{-{\mathrm{v}}_{\mathrm{e}}}{{(\mathrm{N}+\mathrm{h})}^2}(\begin{array}{c}\frac{{\mathrm{N}}^{\prime }}{\cos \phi }-(\mathrm{N}+\mathrm{h})\tan \phi \hfill \end{array})}& 0\hfill & \frac{-{\mathrm{v}}_{\mathrm{e}}}{{(\mathrm{N}+\mathrm{h})}^2\cos \phi }\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill \end{array}\right]$$

(46)

$${\mathrm{F}}_{\mathrm{pv}}=\left[\begin{array}{ccc}\frac{1}{\mathrm{M}+\mathrm{h}}\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& \frac{1}{(\mathrm{N}+\mathrm{h})\cos \phi }\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & -1\hfill \end{array}\right]$$

(47)

$${\mathrm{F}}_{\mathrm{vp}}=\left[\begin{array}{ccc}2{\mathrm{w}}_{\mathrm{e}}{\mathrm{v}}_{\mathrm{e}}\cos \phi -\frac{{{\mathrm{v}}_{\mathrm{e}}}^2}{(\mathrm{N}+\mathrm{h})\cos \phi }\left(\begin{array}{c}\frac{\sin \phi }{\mathrm{N}+\mathrm{h}}{\mathrm{N}}^{\prime }-\frac{1}{\cos \phi }\hfill \end{array}\right)+\frac{{\mathrm{v}}_{\mathrm{n}}{\mathrm{v}}_{\mathrm{d}}{\mathrm{M}}^{\prime }}{\mathrm{M}+\mathrm{h}}\hfill & 0\hfill & \frac{{\mathrm{v}}_{\mathrm{n}}{\mathrm{v}}_{\mathrm{d}}}{{(\mathrm{M}+\mathrm{h})}^2}-\frac{{{\mathrm{v}}_{\mathrm{e}}}^2\tan \phi }{{(\mathrm{N}+\mathrm{h})}^2}\hfill \\ \multicolumn{1}{c}{2{\mathrm{w}}_{\mathrm{e}}({\mathrm{v}}_{\mathrm{d}}\sin \phi -{\mathrm{v}}_{\mathrm{n}}\cos \phi )+\frac{{\mathrm{v}}_{\mathrm{n}}{\mathrm{v}}_{\mathrm{e}}}{(\mathrm{N}+\mathrm{h})\cos \phi }\left(\begin{array}{c}\frac{\sin \phi }{\mathrm{N}+\mathrm{h}}{\mathrm{N}}^{\prime }-\frac{1}{\cos \phi }\hfill \end{array}\right)+\frac{{\mathrm{v}}_{\mathrm{e}}{\mathrm{v}}_{\mathrm{d}}{\mathrm{N}}^{\prime }}{{(\mathrm{N}+\mathrm{h})}^2}}& 0\hfill & \frac{{\mathrm{v}}_{\mathrm{e}}({\mathrm{v}}_{\mathrm{n}}\tan \phi +{\mathrm{v}}_{\mathrm{d}})}{{(\mathrm{N}+\mathrm{h})}^2}\hfill \\ \multicolumn{1}{c}{-\frac{{{\mathrm{v}}_{\mathrm{n}}}^2}{{(\mathrm{m}+\mathrm{h})}^2}-2{\mathrm{w}}_{\mathrm{e}}{\mathrm{v}}_{\mathrm{e}}\sin \phi -\frac{{{\mathrm{v}}_{\mathrm{e}}}^2{\mathrm{N}}^{\prime }}{{(\mathrm{N}+\mathrm{h})}^2}}& 0\hfill & \frac{{\mathrm{v}}_{\mathrm{e}}{\mathrm{v}}_{\mathrm{d}}}{{(\mathrm{N}+\mathrm{h})}^2}-\mathrm{v}\hfill \end{array}\right]$$

(48)

$${\mathrm{F}}_{\mathrm{vv}}=\left[\begin{array}{ccc}-\frac{{\mathrm{v}}_{\mathrm{d}}}{\mathrm{M}+\mathrm{h}}\hfill & -\frac{{\mathrm{v}}_{\mathrm{e}}\tan \phi }{\mathrm{N}+\mathrm{h}}\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& \frac{-{\mathrm{v}}_{\mathrm{n}}\tan \phi -{\mathrm{v}}_{\mathrm{d}}}{\mathrm{N}+\mathrm{h}}\hfill & 0\hfill \\ \multicolumn{1}{c}{\frac{{\mathrm{v}}_{\mathrm{n}}}{\mathrm{M}+\mathrm{h}}}& \frac{{\mathrm{v}}_{\mathrm{e}}}{\mathrm{N}+\mathrm{h}}\hfill & 0\hfill \end{array}\right]$$

(49)

$${\mathrm{F}}_{\mathrm{va}}={{\mathrm{C}}_{\mathrm{b}}}^{\mathrm{n}}{\mathrm{a}}_{\mathrm{b}}$$

(50)

$${\mathrm{F}}_{\mathrm{v}{\mathrm{b}}_{\mathrm{a}}}={{\mathrm{C}}_{\mathrm{n}}}^{\mathrm{b}}$$

(51)

$${\mathrm{F}}_{\mathrm{ap}}=\left[\begin{array}{ccc}-{\mathrm{w}}_{\mathrm{e}}\sin \phi -\frac{{\mathrm{v}}_{\mathrm{e}}{\mathrm{N}}^{\prime }}{{(\mathrm{N}+\mathrm{h})}^2}\hfill & 0\hfill & \frac{-{\mathrm{v}}_{\mathrm{e}}}{\mathrm{N}+\mathrm{h}}\hfill \\ \multicolumn{1}{c}{\frac{{\mathrm{v}}_{\mathrm{n}}{\mathrm{M}}^{\prime }}{\mathrm{M}+\mathrm{h}}}& 0\hfill & \frac{{\mathrm{v}}_{\mathrm{n}}}{{(\mathrm{M}+\mathrm{h})}^2}\hfill \\ \multicolumn{1}{c}{-{\mathrm{w}}_{\mathrm{e}}\cos \phi +\frac{{\mathrm{v}}_{\mathrm{e}}}{(\mathrm{N}+\mathrm{h})\cos \phi }\left(\begin{array}{c}\frac{\sin \phi }{\mathrm{N}+\mathrm{h}}{\mathrm{N}}^{\prime }-\frac{1}{\cos \phi }\hfill \end{array}\right)}& 0\hfill & \frac{{\mathrm{v}}_{\mathrm{e}}\tan \phi }{{(\mathrm{N}+\mathrm{h})}^2}\hfill \end{array}\right]$$

(52)

$${\mathrm{F}}_{\mathrm{av}}=\left[\begin{array}{ccc}0\hfill & \frac{1}{\mathrm{N}+\mathrm{h}}\hfill & 0\hfill \\ \multicolumn{1}{c}{-\frac{1}{\mathrm{M}+\mathrm{h}}}& 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& -\frac{\tan \phi }{\mathrm{N}+\mathrm{h}}\hfill & 0\hfill \end{array}\right]$$

(53)

$${\mathrm{F}}_{\mathrm{aa}}=\left[\begin{array}{ccc}0\hfill & {\mathrm{w}}_{\mathrm{e}}\sin \phi +{\mathrm{v}}_{\mathrm{e}}\frac{\tan \phi }{\mathrm{N}+\mathrm{h}}\hfill & \frac{-{\mathrm{v}}_{\mathrm{n}}}{\mathrm{M}+\mathrm{h}}\hfill \\ \multicolumn{1}{c}{-{\mathrm{w}}_{\mathrm{e}}\sin \phi -{\mathrm{v}}_{\mathrm{e}}\frac{\tan \phi }{\mathrm{N}+\mathrm{h}}}& 0\hfill & -{\mathrm{w}}_{\mathrm{e}}\cos \phi +\frac{{\mathrm{v}}_{\mathrm{e}}}{\mathrm{N}+\mathrm{h}}\hfill \\ \multicolumn{1}{c}{\frac{{\mathrm{v}}_{\mathrm{n}}}{\mathrm{M}+\mathrm{h}}}& {\mathrm{w}}_{\mathrm{e}}\cos \phi +\frac{{\mathrm{v}}_{\mathrm{e}}}{\mathrm{N}+\mathrm{h}}\hfill & 0\hfill \end{array}\right]$$

(54)

$${\mathrm{F}}_{\mathrm{a}{\mathrm{b}}_{\mathrm{g}}}={{\mathrm{C}}_{\mathrm{b}}}^{\mathrm{n}}$$

(55)

In this study, the GPS measurement was determined as an outlier if the following two conditions were satisfied ¹² : the difference between the position ( $X_{k\mathrm{GPS}},Y_{k\mathrm{GPS}}$ ) measured at the $\mathrm{k}$ time step and the position ( $X_{p\mathrm{GPS}},Y_{p\mathrm{GPS}}$ ) measured at the previous time step without an outlier is greater than or equal to 5 $\mathrm{\sigma }$ ; and the estimated position ( ${\hat{X}}_k,{\hat{Y}}_k$ ) obtained from the navigation system at the moment when the GPS measurement signal is acquired, and the error radius of the GPS measured position exceeds the threshold of the outlier decision (TOD):

$$\left|X\right|>K_1,\left|Y\right|>K_1$$

(56)

$$\mathrm{\Delta }{R}_{\mathrm{estimate}}=\sqrt{{({\hat{X}}_k-X_{k\mathrm{GPS}})}^2+{({\hat{Y}}_k-Y_{k\mathrm{GPS}})}^2}>\mathrm{TOD}$$

(57)

where $\mathrm{\Delta }$ is the difference between the position measured when the outlier did not occur and the position measured at the current time, $K_1$ is a constant, and $\mathrm{\Delta }{R}_{\mathrm{estimate}}$ is the distance between the estimated position on the GPS position. TOD takes a greater value between $\mathrm{\alpha }$ multiples of the standard deviation from the error covariance matrix on the $\mathrm{X}-\mathrm{Y}$ stage at $\mathrm{k}$ time steps and $K_2$ :

$$\mathrm{TOD}=\max \left(\alpha \sqrt{P_k(1,1)+P_k(2,2)}\right)$$

(58)

where $K_2$ is a constant, $P_k(1,1)$ and $P_k(2,2)$ are $\mathrm{X},\mathrm{Y}$ position error covariances at the $\mathrm{k}$ time step, and $\mathrm{\alpha }$ is the weight. An appropriate value was selected for $\mathrm{\alpha }$ through an experiment.

The measurement errors of the two navigation systems in a loosely coupled method can be calculated based on the difference in position and velocity data. Here, $\mathrm{H}$ can be expressed as shown in equation (60) as a scoring matrix:

$$\mathrm{z}=\left[\begin{array}{c}{\mathrm{P}}_{\mathrm{INS}}\hfill \\ \multicolumn{1}{c}{{\mathrm{V}}_{\mathrm{INS}}}\end{array}\right]-\left[\begin{array}{c}{\mathrm{P}}_{\mathrm{GPS}}\hfill \\ \multicolumn{1}{c}{{\mathrm{V}}_{\mathrm{GPS}}}\end{array}\right]=\mathrm{HX}+{\mathrm{w}}_{\mathrm{m}}$$

(59)

$$\mathrm{H}=\left[\begin{array}{cc}{\mathrm{I}}_{6\times 6}\hfill & 0_{6\times 9}\hfill \end{array}\right]$$

(60)

A block diagram of the proposed navigation algorithm is shown in Figure 3. As mentioned above, the INS is configured through the IMU, where the velocity and position deduced from the INS are calibrated using GPS, while the direction angle is calibrated using a magnetic compass. Finally, the final outputs (velocity, position, and direction angle) were deduced using the EKF.

Figure 3.

Block diagram of the navigation system.

Simulation results

A simulation was performed to examine the performance of the system before conducting an experiment on navigation. The sensors used in the simulation include an accelerometer and a gyroscope of the IMU, magnetic compass, and GPS. Table 1 lists the major parameters of the IMU, Table 2 lists the major parameters of the magnetic compass, and Table 3 lists the major parameters of the GPS used in the simulation. The parameters of all the sensors were selected based on the specifications of the sensors used in the navigation system developed in this study.

Table 1.

IMU specifications used for simulation.

Index	Accelerometer	Gyroscope
Measurement range	362.846  $\mathrm{m}/{\mathrm{s}}^2$	18.7448  $\mathrm{rad}/\mathrm{s}$
Resolution	3.6576e-12  $\mathrm{m}/{\mathrm{s}}^2$	1.2e-16  $\mathrm{rad}/\mathrm{s}$
Constant bias	0.0098  $\mathrm{m}/{\mathrm{s}}^2$	0.8727  $\mathrm{rad}/\mathrm{s}$
Bias instability	0.0001  $\mathrm{m}/{\mathrm{s}}^2$	4.48e-6  $\mathrm{rad}/\mathrm{s}$
Noise density	8.3357e-4  $\mathrm{m}/{\mathrm{s}}^2/\sqrt{\mathrm{Hz}}$	0.0785  $\mathrm{rad}/\mathrm{s}/\sqrt{\mathrm{Hz}}$
Update rate	100  $\mathrm{Hz}$

Table 2.

Magnetic compass specifications used for simulation.

Index	Magnetic compass
Measurement range	80  $\mathrm{\mu }\mathrm{T}$
Resolution	0.05  $\mathrm{\mu }\mathrm{T}$
Constant bias	1  $\mathrm{\mu }\mathrm{T}$
Noise density	0.0239  $\mathrm{\mu }\mathrm{T}/\sqrt{\mathrm{Hz}}$
Update rate	30  $\mathrm{Hz}$

Table 3.

GPS specifications used for simulation.

Index	GPS
Horizontal position accuracy	1.3  $\mathrm{m}$
Vertical position accuracy	1.9  $\mathrm{m}$
Horizontal velocity accuracy	0.8  $\mathrm{cm}/\mathrm{s}$
Vertical velocity accuracy	1.3  $\mathrm{cm}/\mathrm{s}$
Global position noise decay factor	0.5
Update rate	1  $\mathrm{Hz}$

First, the simulation inputs such as acceleration, angular velocity, and direction angle of a vehicle based on which a path the vehicle moves were generated for comparison with the navigation results. Second, measurements were generated by inputting acceleration, angular velocity, and direction angle to the sensor using the IMU model. ¹³ Third, the GPS position data were generated by utilizing the moving path generated by the GPS model as an input. The final outputs (position, velocity, and direction angle) were deduced based on the data of the accelerometer, gyroscope, magnetic compass, and GPS generated previously using the navigation method explained above.

Figure 4 shows the sensor measurements resulting from the angular velocity, acceleration, and direction angle generated by the IMU model as inputs. Measurements of the sensors were computed according to the parameters of each sensor in which the gravitational component was added in the z-axis direction for acceleration, while noise was added to the sensors.

Figure 4.

Sensor data generated using the IMU model.

The position results of the navigation are shown in Figure 5. The initial position is [0 0 0] m, and the final position after two rotations is [0 90 0] m. The trajectory is a position reference value that indicates the moving path of a vehicle that has been generated previously. GPS is the position measurement generated from the GPS model based on the position reference value as an input. The estimate shows the position result of the navigation. Navigation finds the direction angle at 100 Hz using an accelerometer and gyroscope and at 30 Hz using the position, velocity, and magnetic compass. The position and velocity data were calibrated to 1 Hz using the EKF. Therefore, the result of navigation does not have a significant error, even if the GPS position data has a large error, as shown in Figure 5. Figure 6 shows the direction angle of the navigation, which is similar to the actual direction angle.

Figure 5.

Position of navigation.

Figure 6.

Orientation of navigation versus trajectory.

The velocity results of the navigation are shown in Figure 7. Real shows the moving velocity of the actual vehicle calculated using the accelerometer, while estimate shows the velocity of navigation. The velocity of the accelerometer was integrated once for the representation. Because the velocity result of navigation is calibrated at 1 Hz through GPS data, the velocity changes according to the update rate, as shown in the graph. Figure 7 shows that the velocity result of the navigation is adequately estimated.

Figure 7.

Each velocity of navigation.

Figure 8 shows a comparison between the position result of navigation and the actual position. For each axis, the trajectory shows the actual position, whereas the estimate shows the position information of navigation. The greatest error of each axis was −1.4 m for the x-axis, –2 m for the y-axis, and 1.2 m for the z-axis; the root mean square (RMS) error of the position was [0.7 1.46 0.71] m.

Figure 8.

Position of navigation versus trajectory.

Experiments and results

System configuration

To verify the performance of the navigation algorithm proposed in this study, the navigation system shown in Figure 9 was developed. Using an aluminum box shape, a GPS antenna was mounted at the top. The system was equipped with sensors for navigation, an Ethernet hub for communications, and an interface board for providing power for sensors and data communications. Table 4 presents the specifications and devices used in the navigation system. The IMU, GPS, and magnetic compass were used as sensors in the navigation system. A ring laser gyroscope (RLG) was selected as the IMU to improve the computation performance of the INS.

Figure 9.

Developed navigation system.

Table 4.

Specifications of the navigation system.

Index	Value	Units
Length	420	mm
Width	300	mm
Height	300	mm
Weight	8	kg
Standard components	IMU, GPS, magnetic compass, Ethernet hub

Performance experiment

The basic performance of the navigation system was verified before conducting the experiment. The navigation system was moved eastward by 20 m to measure the position of the vehicle, and the measurement results were compared with the navigation system results. At this time, as I was moving on the road using a vehicle, it was not possible to move in a straight line in the east direction. In Figure 10, the GPS result represents the GPS measured position (star-dotted line), navigation result represents the proposed navigation estimated position (red solid line), DR represents the position estimated by DR (solid dotted line), and trajectory represents the actual position (black solid line).

Figure 10.

Positions of trajectory and navigation.

The experimental results in Figure 10 show that the estimated position of the proposed navigation is similar to the actual position but varies slightly from the position estimated by DR. The measured position of the GPS acquired during the experiment showed that a large error was produced owing to the horizontal precision of the vehicle. The experimental results showed that the proposed navigation position result is most similar to the actual position compared to other navigation results.

Figure 11 highlights the error between the actual position and the proposed navigation position. The north axis is represented with a + symbol, while the east axis is represented by an * symbol. The comparison results show that the maximum error for each axis is [0.267 0.5503] m, and the RMS error is [0.13 0.22] m. The experiment proved that the proposed navigation system estimates the position within an RMS error of 0.3 m.

Figure 11.

The error between positions of trajectory and navigation.

Navigation experiment

The navigation performance experiment was conducted by moving the developed navigation system along a horizontal trajectory. The navigation performance was tested on land using a navigation system and a small vehicle. In order to increase the GPS reception sensitivity, an experiment was conducted in a playground with no tall buildings nearby. In Figure 12, the estimated position of the proposed navigation algorithm is shown in terms of a horizontal trajectory in which GPS represents the measured position of the GPS (dotted line), while the navigation represents the estimated position of the proposed navigation (solid line). The vehicle started from the [0 0] m, moved counterclockwise in a distorted square trajectory, and stopped at the [0 –6] m. The results showed that the estimated navigation position was similar to the GPS measured position; the data near the point ([–16 −7.5] m) showed vibrating patterns, possibly due to the vibration that occurred when the vehicle moved from a polyurethane track to grass. The proposed navigation system removed the outliers to estimate the position even when outliers occurred in the GPS position during the experiment. Figure 13 shows the difference between the GPS measured position and the estimated navigation position for each axis; the largest error of each axis is 2 m for both the x-axis and y-axis, and the RMS error of the position is [1.18 0.94] m.

Figure 12.

Position of navigation.

Figure 13.

Difference between the position of navigation and GPS.

Figure 14 shows the velocity component estimated using the proposed navigation system. The results show that the maximum velocity in each direction is [0.6 0.3] m/s, while the mean velocity is [–0.05 −0.01] m/s. At approximately 180 s, the velocity in the x-axis abruptly changes by ±0.6 m/s, which is presumed to be the vibration generated when the vehicle moves from a polyurethane track to grass. The results confirmed the difference between the height of the polyurethane track and grass in which the polyurethane track is positioned higher than the grass. The velocity was estimated well even when the navigation system vibrated substantially owing to the ground state.

Figure 14.

Velocity of navigation.

Figure 15 shows the angular components estimated by the proposed navigation system. Previous experimental results showed that the progression direction changed considerably at approximately 70, 150, and 240 s. Moreover, the heading and pitch instantaneously changed at 180 s as the vehicle moved from a polyurethane track to grass, as explained earlier.

Figure 15.

Orientation of navigation.

Conclusions

Extensive research is ongoing on autonomous exploration and operation which requires navigation. DR, which is a general type of navigation, has limited estimation within a short period of time. Therefore, integrated navigation using auxiliary navigation sensors is required. However, a technique for detecting and removing outliers is required owing to the presence of navigation sensors that generate outliers. Therefore, in this study, a navigation algorithm with a technique for removing outliers of GPS was designed to develop a navigation system. Furthermore, an experiment was conducted using the developed navigation system to verify the effectiveness of the proposed navigation algorithm.

First, the INS was configured through the IMU, where the velocity and position deduced from the INS were calibrated using GPS, while the direction angle was calibrated using a magnetic compass. Finally, the final outputs (velocity, position, and direction angle) were deduced using EKF. A simulation was also performed before conducting the experiment on the proposed navigation algorithm. The parameters of all the sensors used in the simulation were selected based on the specifications of the sensors used in the navigation system developed in this study. The simulation confirmed that the navigation system effectively estimated the position.

A navigation system was developed to conduct an experiment to verify the performance of the proposed navigation algorithm, which was tested as follows. The experimental conditions were ground surfaces of the polyurethane track and grass. The first experiment result showed that the RMS error (compared to the actual position) of the navigation system was within 0.3 m; the second experiment on the navigation performance showed that the proposed navigation estimated the position more effectively than the conventional navigation method, DR. Even when outliers occurred in the GPS position during the experiment, the proposed navigation system removed the outliers to estimate the position.

Previous experimental results showed that the position data measured in real-time from the GPS, used for calibrating the position, a large amount of noise, whereas the position data collected by the proposed navigation algorithm included much less noise. The proposed navigation estimated the velocity more effectively, thus reducing the error in position estimation, which is the final output. This paper presents a simple and inexpensive navigation system. With these advantages, the performance can show a degree similar to that of the systems already on the market.