Abstract
The integration of strapdown inertial navigation system and Doppler velocity log (SINS/DVL) is widely used for navigation in automatic underwater vehicles (AUVs). In the integration of SINS/DVL, the velocity measured by DVL in body frame should be projected into navigation frame with the help of attitude matrix calculated by SINS to participate in data fusion. In the process of data fusion based on standard Kalman filter, the errors in calculated attitude matrix are characterized by state variance and process noise while the errors in measurement vector from DVL are by measurement noise. But the above projection will bring process noise into measurement noise, and thus the assumption of the independence between process noise and measurement noise will not stand. In this paper, the forming mechanism of cross-noise in SINS/DVL is studied in detail and Kalman filter for cross-noise is introduced to deal with this problem. Simulation results indicate that navigation accuracy, especially the position accuracy, can be improved when the cross-noise is processed in Kalman filter.
1. Introduction
Automatic underwater vehicles (AUVs) are widely used in missions of science, military, and commerce, such as survey of ocean resources and cable tracking. Navigation is an important requirement for AUVs, because accurate navigation information is essential for safe operation and recovery. Due to the special characteristics of ocean environment, such as attenuation effect on Global Navigation Satellite System (GNSS) signals, navigation has become one of the primary and key challenges in AUV research [1, 2]. Strapdown inertial navigation system (SINS) is one of the mostly used navigation methods in AUV for its merits of complete independence, strong anti-interference ability, comprehensive and high update frequency of navigation messages, small dimensions, and high reliability [3–5]. As a navigation method based on integration operation, navigation errors will accumulate with time due to the existence of inertial sensor biases and initial misalignment angles in SINS [3–5]. A free SINS, even with high precision inertial sensors, will have unacceptable position errors after a long time and long distance travelling [3–5].
Regular or irregular resetting with the help of external reference position information is an effective method to deal with the accumulated location errors in SINS [3]. The resetting information is mainly obtained from GNSS, underwater acoustic position system such as long baseline (LBL) and geophysical navigation system such as terrain-aided navigation system (TA) [4, 5]. To access GNSS satellites, AUVs should rise to water surface because radiofrequency communication is not feasible under water. The surfacing for GNSS not only consumes time and energy, but also debases concealment. At the same time, the option of LBL or TA relies on the presence of a known transponder network or a known terrain. Due to the inherent constraints in GNSS, LBL, and TA, some new aided navigation methods should be found.
Recently, with the development of acoustic correlation velocity measurement technology, the performance of Doppler velocity log (DVL), such as accuracy and range, has been greatly improved. The accuracy of DVL is as high as 0.2% of speed and the range is up to 90 m with a frequency of 600 kHz, and the accuracy is 0.5% and the range is 425–500 m with a frequency of 150 kHz [4, 5]. Currently, DVL is widely used for underwater navigation and the integration of SINS/DVL has even become a standard navigation system of AUV [1–3, 5].
Without other aided navigation systems, the accuracy of SINS/DVL integration with certain sensor precision for long time and long distance mission is mainly determined by data fusion algorithm [4–6]. Recently, the algorithms for SINS/DVL are proliferated in the literature, and the researches mainly include the following aspects: online estimation for the installation between SINS and DVL; online estimation for the scale factors of DVL; alignment methods for SINS with aid of DVL; nonlinear filters, such as extended Kalman filter and unscented Kalman filter and particle filter, to deal with nonlinear problem; and robust filters to deal with disturbance.
The problems addressed in this paper are concentrated on the investigation of cross-noise in SINS/DVL. In general, DVL is installed on the shell and measures the velocity relative to Earth (or current) in body frame [4, 5, 7]. When the velocity matching method is used for SINS/DVL integration, the velocity measured by DVL in body frame should be firstly projected into navigation frame with the help of calculated attitude matrix from SINS [4, 5, 7], and thus added velocity errors, caused by the errors in calculated attitude matrix, will be added to velocity errors measured by DVL [7]. When Kalman filter is used for the data fusion in SINS/DVL, the errors in DVL velocity are characterized by measurement noise, while the errors in SINS attitude are by state variance and process noise [3]. Obviously, the above projection introduces the cross-problem from process noise to measurement noise, which challenges the assumption in standard Kalman filter about the independence of process and measurement noise and decreased the accuracy of SINS/DVL.
Cross-noise (or color noise) problem is a classical issue in data fusion and a large amount of paper published since the 1960s to deal with this problem [8–11]. But to the specific topic about SINS/DVL, the research is insufficient. Cross-noise can be simulated by colored noise with the help of shaping filter driven by white Gaussian noise [8], but the power spectral density of the white noise is difficult to be found in a time variant system such as SINS/DVL integration. Although cross-noise can be processed as the added measurement noise for the initial alignment problem in large initial misalignment angles of SINS/DVL [7], there are defects in principle. In this paper, the forming mechanism of cross-noise in SINS/DVL is studied in detail, a Kalman filter for cross-noise is introduced to deal with this problem; in this filter, cross-noise is added to the standard Kalman filter; and the need for processing this cross-noise is verified by simulation.
The rest of this paper is organized as follows. The integrated navigation system structure and error propagation models for each subsystem of integration system are given in Section 2. Then, the cross-noise problems in SINS/DVL are analyzed in detail in Section 3. In Section 4, a Kalman filter for cross-noise is introduced. In Section 5, the cross-noise problem is simulated and the results indicate that the integrated accuracy can be improved when the cross-noise are considered. Finally, some conclusions are given in Section 6.
2. Integrated Navigation System of SINS/DVL/MCP/PS
Figure 1 shows the integrated navigation system of SINS/DVL/magnetic compass (MCP)/pressure sensor (PS), which is a simplified version presented in [4, 5]. Compared with that in [4, 5], aided positioning navigation systems are abandoned for the consideration of difficulty in obtaining level position in underwater environment.
In Figure 1, SINS acts as the basic navigation system to get the attitude, velocity, and position of AUV, by virtue of the integration operation on the angular rate measured by gyros and acceleration measured by accelerometers while DVL, MCP and PS act as aided navigation systems to provide reference information of velocity, yaw and depth and Kalman filter acts as data fusion unit to estimate the errors of navigation parameters for SINS.
Navigation solution and data fusion are two independent and parallel processes in Figure 1, which are connected with feedback correction mode. The estimations to state vector, that is, the estimation of navigation errors of SINS from data fusion unit, are fed back to SINS and used to reset navigation parameters, and the subsequent navigation solution will be run under initial conditions of the corrected navigation parameters.
2.1. Error Propagation Model of SINS
As a self-depended navigation system, SINS is composed of inertial measurement unit (IMU) and navigation solution. In IMU, three gyros and three accelerometers are orthogonally installed to measure the vehicle’s movement in six degrees, and the navigation solution can calculate vehicle’s attitude, velocity, and position through integration operation on angular rate and acceleration measured by IMU. The equations describing SINS solution are three differential equations as follows [3]: where , , , and denote the navigation, earth, body, and inertial frames, respectively; ; ; the symbol such as denotes the projection of a motion vector from frame to frame in frame ; is attitude matrix; is the velocity in navigation frame; , , and are the latitude, longitude, and height, respectively; such as is the skew symmetric matrix of the vector ; and and denote the radius of Earth in meridian and prime, respectively. In SINS, (1)–(3) should be translated into discrete form for the calculation with computer.
Errors in attitude, velocity, and position will be generated because of initial errors such as misalignment, sensor errors such as gyro and accelerometer biases, and navigation algorithm errors such as cone error. Here, the Euler angles, which describe the rotation between calculated navigation frame and ideal navigation frame , are defined as misalignment angles. The error propagation equations about misalignment angles, velocity errors, and position errors are constructed as [3] where and denote the errors of gyros and accelerometers in body frame, respectively. Different inertial sensors have different characteristics and error propagation equations. Here, sensor errors are divided into constant error and random error [3], and the random errors are assumed to be white Gaussian noise. Thus, the sensor error propagation model can be constructed as
2.2. Error Propagation Model of DVL
Doppler effects are used in DVL to measure velocity. With four acoustic beams in fixed direction that is, Janus configuration, three velocities along body frame axes can be measured. The precision of DVL is affected by many sources, such as installing error, scale factor error, and frequency measurement error [4, 5]. For the purpose of this paper, the above errors are assumed to be accurately compensated, and white Gaussian noise is used to describe the error of DVL measured velocity. Thus, the DVL propagation model can be constructed as [4, 5]
2.3. Error Propagation Model of MCP
Magnetic sensors are used in MCP to measure the direction of Earth’s magnetic field and give the angle between the magnetic meridian direction and AUV’s -direction, that is, yaw. MCP is used for long time and long distance navigation because its navigation error will not accumulate with time. But MCP is not a high accurate system due to its sensitivity to interference, and its error can be up to tens of degrees. In this paper, the constant error of MCP is assumed to be accurately compensated, and the random noise caused by interference is described with white Gaussian noise. Thus, the MCP propagation model can be constructed as [4, 5]
2.4. Error Propagation Model of PS
In the integration system of SINS/DVL/MCP/PS, depth is provided by PS, which measures the depth information based on the principle that liquid internal pressure increases with the depth. In PS, the measurement precision of depth is affected by salinity and temperature. In this paper, the constant error of PS is assumed to be accurately compensated, and white Gaussian noise is used to describe the random error when AUV sails in a determined depth. Thus, the PS propagation model can be constructed as [4, 5]
3. Cross-Noise in SINS/DVL
In this section, the cross-problem caused by the projection from process noise to measurement noise in SINS/DVL will be analyzed in detail.
3.1. Standard Kalman Filter
3.1.1. Assumption and Equations in Kalman Filter
The discrete random linear model without controlling terms is described as follows: where the subscript denotes the th time-step; denotes state vector; denotes measurement vector; denotes the transition matrix of the dynamic model; is the measurement model matrix; is the process noise; and is the measurement noise.
In standard Kalman filter, the process noise and measurement noise are assumed to satisfy the following assumptions: where is Kronecker- function.
If the estimation of and the measurement vector satisfy constrains in (11), process noise and measurement noise satisfy the assumption in (12), and and are nonnegative definite and positive definite, respectively; the estimated for at can be solved by standard Kalman filter. The equations in standard Kalman filter are as follows: where is estimation for ; is state variance; and are one-step prediction at time with and at time , respectively; is the gain matrix; and is unit matrix with corresponding dimensions.
3.1.2. System Equations
As for the integration system described in Section 2, the misalignment angles, velocity errors, position errors, gyro bias, and accelerometer bias in SINS are chosen as state vector, which can be constructed as follows:
The transition matrix in (11) can be calculated by discrete operation on (4)–(7), and the process noise variance in (12) can be set according to the precision of sensors.
3.1.3. Measurement Equations
Take the velocity from DVL, the yaw from MCP, and the depth from PS as reference navigation information, and the measurement vector can be constructed as where is DVL velocity in frame, is the yaw from MCP, and is the depth from PS.
According to the relationship between measurement and state vectors, can be expressed as
In the integrated system, DVL, MCP, and PS are independent systems and the reference information from them is also independent, so the measurement noise variance in (12) can be set according to the precision of each of them, respectively.
3.2. Noise Cross-Problem
As shown in Figure 2(a), DVL is usually installed on the AUV’s shell, and the velocity is measured in DVL instrument frame, which is denoted by frame. Thus, the projection velocity of DVL in frame can be expressed as where is the attitude matrix between frame and frame and should be accurately calibrated when DVL is installed; is attitude matrix and can be calculated by (1). In this paper, the relationship between and frames is assumed to be fixed and is without errors.
(a) The relationship between DVL, body, and navigation frame
(b) Objection processes of DVL velocity
When considering the measurement error in DVL, in (17) should be expressed as where the superscript of “” denotes the measurement value of a vector; is the true velocity of AUV in frame; is the measurement error in DVL and satisfies constrains in (8).
When considering the calculating error as shown in Figure 2(b), in (17) should be expressed as where the superscript of “” denotes the calculated value; is the true attitude matrix of SINS; denote misalignment angles as defined in (4).
Then, the error in can be expressed as where the second-order small terms is ignored. Further, the velocity error caused by the error of can be expressed as
The above equation indicates that the errors in the DVL projection velocity include not only the DVL measured error described by measurement noise but also SINS misalignment angles described by process noise and state variance. In this way, measurement noise is coupled by process noise and the assumption of independence between measurement noise and process noise in (12) will not stand. When this cross-noise cannot be neglected, data fusion accuracy will decrease and navigation accuracy will also decrease.
4. Kalman Filter for Cross-Noise in SINS/DVL
4.1. Kalman Filter for Cross-Noise
Equation (12) can be rewritten after the introduction of cross-noise as where . Here, is correlated with because, at time , the attitude matrix from SINS at is used for projection.
Define the prediction error of state vector as , estimation error of state vector as and prediction error of measurement vector as , and then the variance and covariance can be calculated, respectively, as follows.
Covariance of the prediction for state vector and measurement noise can be constructed as
Variance of the prediction for state vector can be constructed as
Covariance of the prediction for state vector and measurement vector can be constructed as
Variance of the prediction for measurement vector can be constructed as
Gain matrix can be constructed as
Variance of the state vector can be constructed as
Here, physical meanings of and are the same as and in (13).
The subequations in (24a), (24b), (24c), (24d), and (24e) construct the Kalman filter for cross-noise, in which the calculation for and is the same as that in (13). The differences between (24a), (24b), (24c), (24d), and (24e), and (13) indicate that introduction of cross-noise in Kalman filter will take effect on and and further on , but no effect on and . In Kalman filter, the value of determines the utility degree of and for estimating . In (24d), cross-noise term appears in the numerator and denominator simultaneously. Compared with the method of only changing the magnitude of , which takes effect only on denominator, the method in (24d) may be more reasonable in principle.
4.2. Analysis on the Elements of Cross-Noise in SINS/DVL
According to the definition of state vector and measurement vector described in Sections 3.1.2 and 3.1.3, the cross-noise term in (22) should be a nonnegative definite matrix with a dimension of . As described in Section 3.2, misalignment angles only cause the error in projected velocity, so the expression of can be constructed as where are cross-values.
5. Simulation
5.1. Simulation Setting
The gyro constant biases are all set as 0.01°/h and random biases are white Gaussian noise with zero mean and standard variance of 0.01°/h; the accelerometer constant errors are 500 μg and random errors are also white noise with zeros mean and standard variance of 500 μg.
AUV is with a swinging motion. The swinging amplitudes of pitch, roll, and yaw are set as 1.2°, 1.2°, and 1.8°, respectively, and the corresponding swinging cycles are 8 s, 10 s, and 6 s. The initial positions of AUV are 118° in longitude, 32° in latitude, and −20 m in height. The velocities of AVU along body frame axes are 0, 5 m/s and 0.
Based on the above ideal motion, the ideal sensor output can be gotten by backstepping of navigation solution. When sensor errors are added into ideal sensor output, the real sensor data can be simulated. In this simulation, the update frequencies of sensor data and navigation solution are both 100 Hz.
The errors in DVL, MCP, and PS are all set as white Gaussian noise with zero mean and standard variance of 0.02 m/s, 10°, and 0.5 m, respectively. When the above noise is added into ideal motion, the real reference velocity, yaw, and depth from DVL, MCP, and PS can be simulated, respectively. In this simulation, the update frequencies of reference data are all 1 Hz.
Straight line trajectory is chosen in simulation for the consideration that error growth is correlated with mission pattern and straight line trajectory has no cancelling effect on error growth [4, 5]. The initial yaw of AUV is set as 45° north to east.
Four filtering schemes are compared: in Scheme 1, the ideal AUV attitude matrix is used to project DVL velocity from body frame to navigation frame, and standard Kalman filter is adopted; in Scheme 2, the attitude matrix from SINS is used for DVL velocity projection and standard Kalman filter is adopted; Scheme 3 is the same as Scheme 2, but in Scheme 3, the measurement noise value is added; in Scheme 4, the attitude matrix from SINS is used while the Kalman filter for cross-noise is adopted. To evaluate the different effects of different schemes, the ideal motion is used as a reference.
After setting the initial parameters, the integrated navigation including initial alignment is run. The initial velocity and position parameters of SINS have no initial errors, while the attitude has misalignment angles of 0.5°, 0.5°, and 1.2° in pitch, roll, and yaw.
The initial parameters of Kalman filter are set as
For Scheme 3, the measurement noise is set as
For Scheme 4, the cross-noise is set as
Here, m denotes the Earth radius.
5.2. Simulation Results
The simulation lasts for 3600 s. The errors of attitude, velocity, and position of different schemes are shown in Figures 3, 4, and 5, respectively, where the solid lines, dot-dashed lines, double dot-dashed lines and dashed lines denote Schemes 1, 2, 3, and 4, respectively.
(a) Pitch
(b) Roll
(c) Yaw
(a) East velocity
(b) North velocity
(c) Up velocity
(a) East location
(b) North location
(c) Up location
The curves in Figure 3 indicate that, with Scheme 1, misalignment angles can converge to zero at 500 s and stabilize in following navigation process; with Scheme 2, level misalignment angles can converge but with large amplitude while yaw cannot even with the help of MCP; with Schemes 3 and 4, misalignment angles can converge at 800 s but with smaller amplitude than that in Scheme 2. In addition, the error of yaw from Scheme 4 is smaller than that from Scheme 3.
The curves in Figure 4 indicate that, with Scheme 1, velocity errors converge directly and stabilize in the whole navigation process; with Scheme 2, level velocities oscillate and cause the oscillation of level attitude in Figure 3; with Schemes 3 and 4, level velocities also oscillate but both with smaller amplitudes than that in Scheme 2, and, correspondingly, the amplitude of level attitude is smaller than that of Scheme 2 in Figure 3. Compared with Scheme 3, the level velocities from Scheme 4 oscillate with smaller amplitude and shorter cycle, and the amplitude of level attitude from Scheme 4 is correspondingly smaller. The reason for this difference is that the calculation method for gain matrix of Kalman filter is different between Schemes 3 and 4. In addition, the curves in Figure 4(c) indicate that there is a constant error in up velocities with Schemes 2, 3 and 4, but that in Scheme 4 is smaller than those in Schemes 2 and 3.
The curves in Figure 5 indicate that there are level position drift trends for all the four schemes. But due to larger attitude errors, especially the error of yaw, the level position errors with Scheme 2 are far larger than those with Schemes 1, 3, and 4. Statistic level position errors of the four schemes in 3501–3600 s are listed in Table 1, which indicates that the level position accuracy is significantly improved with the Kalman filter for cross-noise. In addition, the curves in Figure 5(c) indicate that the height errors with four schemes are more or less the same, due to the operation of using the height from PS directly as measurement information.
From the above analysis, it can be concluded that, in the integration of SINS/DVL, the error in calculated attitude matrix will cause error in the projected DVL velocity, which will further decrease the accuracy of SINS/DVL, and the cross-noise should be processed to increase navigation accuracy; when the cross-noise is processed either by adding measurement noise or adding cross-noise term to standard Kalman filter, the accuracy is increased, but the effect of latter one is better than the first one.
6. Conclusions
The cross-noise problem in the integration of SINS/DVL is studied in this paper. Analysis indicates that in the projection of DVL velocity from body frame to navigation one, attitude errors from SINS will be also projected into DVL projected velocity, which causes cross-noise from process noise to measurement noise and destroys the assumption about the independence of process noise and measurement noise in standard Kalman filter. When this cross-noise cannot be neglected, data fusion accuracy will decrease and navigation accuracy will also decrease.
To deal with the cross-noise in SINS/DVL, a Kalman filter for cross-noise is introduced. In this filter, the cross-noise term appears in the numerator and denominator of gain matrix simultaneously, so the calculation for gain matrix is more reasonable, and the utility degree between measurement vector and state prediction vector is more reasonable.
Simulation results based on straight line trajectory indicate that when the cross-noise is processed in Kalman filter, navigation accuracy, especially level position accuracy, can be significantly increased.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This work was supported in part by the National Natural Science Foundation (61273056) and Chinese University Research and Operation Expenses (104.205.2.5).