Abstract
In order to suppress the error caused by the drift of the gyroscope and further improve accuracy of the navigation system, combined with the method of measuring attitude by using the three-axis components of geomagnetic, a new scheme, consisting of Strapdown Inertial Navigation System (SINS)/Geomagnetic Navigation System (GNS), is designed for autonomous integrated navigation systems. The principle of this SINS/GNS integrated navigation system is explored, and the corresponding mathematical model is established. Furthermore, a Marginalized Particle Filter (MPF) is designed for this autonomous integrated navigation system. The simulation experiments are conducted, and the results show that the improved SINS/GNS autonomous integrated navigation system possesses strong robustness and high reliability, thus providing a new reference solution for autonomous navigation technology.
1. Introduction
Since the middle of the last century, the inertial navigation system has been widely used in the navigation of various sports carriers due to its autonomy, concealment, and strong anti-interference ability [1]. However, the inertial navigation system also has shortcomings that cannot be ignored: the error diverges with time, so how to suppress the cumulative error of the inertial navigation system has become a current research hotspot. There are two main ways to improve the performance of inertial systems; one is to improve the measurement accuracy of inertial components and to develop new types of inertial devices, such as laser gyroscopes and quantum gyroscopes; the other is to use external information sources to calibrate the navigation system based on information fusion theory. Commonly used auxiliary information sources include satellite, astronomy, radio, geomagnetic field, and gravitational field [2].
The geomagnetic field is an inherent resource of the earth, which provides a natural reference coordinate system for aviation, aerospace, and navigation. The magnetic navigation technology, using the spatial distribution of the earth’s magnetic field, is simple, efficient, reliable, and has strong anti-interference performance [3], so it is often used with inertial navigation together to form a high-precision autonomous navigation system. Scholars have made extensive research on the application of geomagnetism in navigation. For example, the Cao’s team of NUC early proposed the use of geomagnetic field vector and gyroscope to calculate the attitude of the missile body [4, 5]. Taking the geomagnetic entropy and the geomagnetic difference entropy as the reference criteria for the selection of the geomagnetic auxiliary navigation area, combined with the ICCP algorithm can effectively improve the accuracy of the auxiliary navigation [6]. The geomagnetic matching navigation algorithm based on affine parameter estimation, while ensuring the matching accuracy, effectively improves the real-time performance of the algorithm [7]. Applying intelligent algorithms such as quantum particle swarm algorithm and genetic algorithm to geomagnetic matching navigation can effectively improve the convergence speed and accuracy of geomagnetic matching [8–12].
In this paper, the geomagnetic field component is introduced as observation information on the basis of position correction, and the observation equation directly related to the attitude matrix is established. The principle, scheme, and mathematical model of this autonomous integrated navigation system are established. In view of the nonlinear characteristics of the model, a high-precision nonlinear filtering algorithm for the autonomous navigation system is proposed. Subsequently, all of the models and algorithms are verified by experiments.
2. Materials and Methods
2.1. Scheme of SINS/GNS Integrated Navigation System
The gyroscopes and accelerometers used in Strapdown Inertial Navigation Systems are not ideal measurement components. There are errors in both the components themselves and the installation process. Among them, zero offset and scale factor errors are the main error sources of inertial measurement components, and there is also axis cross coupling error and random noise interference [13]; therefore, usually, there is a divergence when working for a long time.
During the actual navigation, there are angular deviations between the coordinate system of the mathematical platform and the actual navigation coordinate system, which called error angle of mathematical platform , and satisfies the following equation [14]: where is the zero offset of the gyroscope and and are the rotation rate and position rate of the earth, respectively. is the attitude transition matrixes of the body coordinate system to the navigation coordinate system. Based on the specific force equation of the accelerometer The velocity error equation of the carrier can be defined as where is the error of velocity in the navigation coordinate system and is the output of the accelerometer. Correspondingly, the traditional position error equation of SINS is shown as where and represent the radius of curvature of the ellipsoid model of earth in different direct, respectively.
Location information of GNS is often used to assist Strapdown Inertial Navigation Systems, which can suppress the accumulation of errors of sensors. We can get two location information by GNS and by SINS, respectively. Consider the error in the positioning process [15], the matching position can be expressed as where is the real position and and are the position error of GNS and SINS, respectively.
Take the difference of the position between SINS and GNS output as the observation, and the position observation equation is obtained as where is the position observation error and is the position observation matrix.
The geomagnetic field has the following characteristics: the vector of each location is unique. Suppose that the position and the total geomagnetism satisfy the relationship: , so when the position of the inertial navigation system is known, the total amount of geomagnetic field can be obtained: , where is error of geomagnetic matching algorithm.
Take the difference of the geomagnetism between SINS and magnetometer output as the observation, the new observation equation can be obtained as where is the observation error.
Therefore, according to the above method, the accuracy of the inertial navigation system, especially position, can be effectively improved. Based on formulas (6) and (7), the position information is directly involved in the observation, but the attitude information is lacking. In order to make full use of the geomagnetic field component information and further improve the attitude accuracy of the integrated navigation system, this paper takes the three-axis geomagnetic field component as a new observation and proposes an improved integrated navigation algorithm. Figure 1 shows the scheme of the improved integrated navigation system.
In the improved navigation system, the geomagnetic field components are used as the new observation information for the estimation of attitude and velocity; the geomagnetism at a certain point has the following relationship with the geomagnetic components:
Given the nonlinear characteristic of the measurement equation, this paper introduces the particle filter to estimate the position information.
2.2. State Equation of Integrated Navigation System
The improved integrated navigation system involves multiple sensors, including a three-axis magnetometer, a three-axis gyroscope, and a three-axis accelerometer fixedly. Each axis of the sensor has its own error model, and this article defines the error model of all sensors as a random constant plus white noise: . Suppose the random constant error vector of the magnetometer is , the paper selects the following parameters as state variables: attitude angle error , horizontal velocity error , zero offset of accelerometer , magnetometer constant error , gyroscope drift , and horizontal position error
Using the E-N-U (East-North-Up) geographic coordinate system as the base coordinate system for navigation, the system state vector of SINS/GNS autonomous integrated navigation system can be defined as
Based on formula (9) and the error model of SINS, the kinematic model of the improved SINS/GNS autonomous integrated navigation system is described as where is the system state matrix and is the systematic error matrix. The discretized state matrix and error matrix satisfies equation can be expressed as equation (11), where is the cycle of the navigation system.
2.3. Observation Equation of Integrated Navigation System
The selection of the measurement vector of the integrated navigation system is directly related to the measurement accuracy of the entire system. Based on the known matching results, the position and attitude observation equations are established.
(1) Establishment of Position Observation Equation. Based on equations (6) and (9), the position observation matrix can be obtained as
Perform a first-order Taylor expansion of equation (7): where and are the slope values of geomagnetic field in the longitude and latitude directions, respectively. In the actual calculation, the slope value of each direction is calculated based on the data in the geomagnetic database and the measured magnetic field at the position where the carrier has flown. First of all, it is necessary to use plane fitting method to fit the discrete data in the database. In order to improve the accuracy of slope, the least square method is usually used to fit the magnetic field data in a certain range [16]. For the area as shown in Figure 2, let the half length of the fitting region be and , where and are the root mean square of position error in different directions, respectively.
Then, the slope of a point in the area can be expressed as
where , , and is the data grid spacing on the geomagnetic map.
Let , the observation equation can be obtained as where is the error caused by the first-order Taylor expansion. Then, the linearized observation equation can be expressed as , where:
(2) Establishment of Attitude Measurement Equation. Common SINS/GNS does not establish a measurement equation about the attitude. This article uses the three-axis geomagnetic field component as the observation information according to the vector characteristics of the geomagnetic field. Since the geomagnetic field component is measured in the carrier coordinate system, if the local geomagnetic field component in the navigation coordinate system is , the following relational equation can be obtained where is the ideal attitude matrix and is the measurement error matrix of the magnetometer and satisfies the equation . Suppose is the attitude error matrix of the inertial system, equation (18) can be established. when the attitude error angle is very small [17], the following equation holds: , where is the cross-multiplying antisymmetric matrix of , so equation (17) can be expressed as the following:
According to the characteristics of the cross-multiplying antisymmetric matrix: , equation (18) can be organized into the following form: where is the corresponding noise and satisfies the following equation: is the error of geomagnetic matching system, so equation (20) can be organized into where , so the observation equation can be obtained as
Let , the cross-multiplying antisymmetric matrix of can be expressed as
Accordingly, the observation matrix can be expressed as
Based on equations (11), (15), and (23), the observation equation of the integrated navigation system can be obtained: , where the observation matrix satisfies
2.4. Filter Design
In this paper, two filters are designed. According to equations (12), (16), and (24), all observation equations are linear, so the Kalman filter can be designed directly to complete data fusion. However, nonlinear processing was performed during the derivation of formula (13), which produces more error, so an improved particle filter algorithm (Marginalized Particle Filter (MPF)) is proposed in this paper, which is the combination of Kalman filter and particle filter, and the method not only avoids linearization errors but also ensures the real-time performance of the algorithm. The linear part of the model is estimated with a Kalman filter, and the nonlinear part is estimated with a particle filter, which reduces the dimensions of the PF. The state vector of the system can be divided into
Correspondingly, the discretized state matrix of integrated navigation system can be written as follows [18].
According to , we can know that the error model of the magnetometer is in the carrier coordination system, now suppose that the total amount of magnetic field generated by the carrier satisfies the model , is the constant error, and is the white noise. Add the constant error as one of the state variables to improve the stability of system. Then, the new observation model can be established as where:
Then, the general form of the observation equation of MPF is expressed as where: where is the comprehensive observation error and the specific steps of the design of the MPF are as follows [19] (Algorithm 1).
| ||||||||||||||||||||||
2.5. Simulation
In the above section, we have established a new observation model for the integrated navigation system, designed the corresponding filter, and listed the specific implementation. Next, we will perform numerical simulation based on the above model and analyze the characteristics of the proposed algorithm. Table 1 is the sensor error model parameters. The errors can be considered to be composed of constant deviation and white noise in this paper.
In the experiment, the simulation data are from a flight of a spacecraft in Earth-Centered Earth-Fixed coordinate system (ECEF). The initial latitude is 45.7508°, the initial longitude is 122.6715°, and the simulation time is 2000 s. The flight trajectory is shown in Figure 3.
In the simulation process, the initial horizontal position error is 5 m, the initial horizontal velocity error is 1 m/s, the initial attitude angles are 0.1°, 0.1°, and 0.1°. The northbound position error of GNS is 10 m, and the eastbound position error is 20 m. Perform simulation experiments based on the above parameters compare the navigation effects between the traditional model and the improved mathematical model and use MATLAB as the simulation tool to obtain the following simulation results.
3. Results and Discussion
This work has already finished the model building and digital simulation. The digital simulation attitude results of integrated navigation system are shown in Figures 4–6, and the position results are shown in Figures 7 and 8, under the same simulation conditions; the two simulation results are shown in Table 2. Although both methods can converge with the time going, the improved algorithm has smaller errors and better stability.
The simulation results show that the improved GNS/SINS has higher navigation accuracy than traditional GNS/SINS in attitude and position and has better stability.
4. Conclusions
This study focuses on improving the accuracy of the navigation system, especially the attitude angle, through introducing the geomagnetic field vector. Based on the inertial navigation system, this paper proposes to use the geomagnetic field component and total geomagnetic field in the navigation coordinate system as observations to improve the traditional GNS/SINS and use MPF for information fusion to achieve accurate estimation of position and attitude. From the results of numerical simulation, it can be seen that the navigation accuracy of the improved scheme will be improved to a certain extent, especially in attitude correction. With the improvement of geomagnetic detection accuracy, this navigation method will play a greater role. However, some issues, sample impoverishment and degeneracy problem, related to particle filters need to be further optimized.
Data Availability
The raw/processed data required to reproduce the results obtained in this study cannot be shared at this time because they are used in an ongoing study.
Conflicts of Interest
The authors declare no conflict of interest.