Abstract
In the integrated navigation system with inertial base, the update frequency of Strapdown Inertial Navigation System (SINS) is always higher than those of aided navigation systems; thus updating inconsistency among subsystems becomes an issue. The analysis indicates that the state transition matrix in Kalman filter is essentially a function of carrier motion. Based on this understanding, a simplified Kalman filter algorithm for integrated navigation is designed for those carriers with low-dynamic motions. With this simplified algorithm, when the filter is without aided information updating, only calculation and accumulation on state transition matrix are executed, and when the filter is with updating, normal time and measurement update are done based on the averaged state transition matrix. Thus the calculation load in the simplified algorithm will be significantly lessened. Furthermore, due to cumulative sum and average operation, more accurate state transition matrix and higher fusion accuracy will arrive for the smoothing effect on random noise of carrier motion parameters. Simulation and test results indicate that when the carrier is with a low-dynamic motion, the simplified algorithm can complete the data fusion of integrated system effectively with reduced computation load and suppressed oscillation amplitude of state vector error.
1. Introduction
The integrated navigation system is widely used for various vehicles to provide speed, position, and (or) attitude. It is generally believed that the integrated system can give full play to each navigation system constructing integrated system and achieve advantages of complementary or (and) combination among each other [1–3]. Among many integrated navigation systems, the inertial integrated navigation system may be the most typical one [2–7]. The most commonly used integrated systems with inertial base are Strapdown Inertial Navigation System (SINS)/Global Navigation Satellite System (GNSS), SINS/Vision, SINS/earth field (such as terrain, magnetic, and gravity) integrated navigation, and so forth. Kalman filter (KF) and other improved filters based on the structure of KF, such as extended KF, unscented KF, and particle KF, are the main tools for data fusion in integrated systems [8–11]. When the models describing system and measurement processes are accurate and the statistical properties of noise are known, optimal estimation of the state vector can be obtained with KF for data fusion [8]. For convenience, all the above filters are named as Kalman-liking filters in the following text and only KF is analyzed.
In the integrated system, the problems of time and spatial inconsistency caused should be first solved before the information from different navigation systems can be used for data fusion [12, 13]. The time inconsistency is mainly caused by the inconsistency of clocks used in different systems while spatial inconsistency is caused by different installing position of each system. To cope with the first problem, a unified standard time signal is usually introduced to synchronize each system. For example, the second-pulse signal from GNSS is often used to synchronize the clock of SINS in the integrated system of SINS/GNSS [12]. To cope with the second one, lever-arm compensation methods are always used. For example, in the integrated SINS/GNSS, lever-arm length and the measurement from SINS are often used to construct and compensate lever-arm velocity when using velocity matching algorithm [13]. Thus, after unified time signal and lever-arm compensation method are introduced, the inconsistent update frequency among each system becomes an important issue. Due to different working mechanism, the data update frequency of each navigation system is quite different [1, 8]. For example, the update frequency of SINS is always 100 Hz or 200 Hz, even up to 1 kHz [1], and the frequency of GNSS is about 1 Hz~20 Hz [7], and the frequency of earth field navigation system always depends on the characteristic of earth field and there exists some uncertainty [2, 3]. In general, the update frequency of inertial navigation system always used as main system in the integrated system with inertial base is much higher than those of aided navigation systems.
From the aspects of system evolution and measurement correction, KF can be divided into time update and measurement update processes which provides a potential solution to the inconsistency problem of update frequency [1, 8]. When the update frequencies between the main and aided systems are consistent, time update and measurement update operation will be carried out successively. When those are inconsistent, only time update operation will be run without measurement update of aided system and both operations will be run with measurement update [8].
In theory, the separated operation of time update and measurement update can solve the inconsistency between main system and aided system in integrated system effectively. In the time update process of KF, the update calculation for state vector, error covariance of state vector, and some related variance, such as state transition matrix, will be executed. The calculation amount is relatively smaller than that of measurement update that needs inversion matrix operation. However, higher update frequency of SINS will bring large calculation load because the same update frequency as that of SINS is needed. When the frequency of SINS is 100 Hz, the calculation amount caused by time update is heavier than that of measurement update with 1 Hz measurement update frequency.
In this paper, the system state equation of integrated system is analyzed, and the analysis indicates that the time update process in KF is essentially the update of parameters related to state matrix, while the variables of state transition matrix are the motion functions of the carrier. When the vehicle is with low-dynamic motion, the changes of motion parameters are slow, and then the changes in time update are slow. In this paper, low-dynamic motion is defined as follows: a vehicle is with a constant velocity in the same direction and (or) with a perturbation of acceleration but without constant acceleration. Based on this understanding, a simplified KF for those carriers with low-dynamic motion is presented. In this simplified algorithm, only the update and summation on state transition matrix without the standard time update process are carried out when there is no measurement update. The case of integrated system of master/slave INS (M/S INS) is studied, and the simulation and turntable tests indicate that the simplified algorithm can fulfill data fusion with reduced studied, and the simulation and turntable tests indicate that the simplified algorithm can fulfill data fusion with reduced calculation but suppressed error oscillation of state vector, when the carrier is with a constant velocity or with a perturbation of acceleration.
The rest of this paper is organized as follows. The integrated system of M/S INS for ship condition is taken as example, and system and measurement equations are studied in Section 2. In Section 3, KF is introduced to fulfill data fusion for M/S INS integration. In Section 4 the time update process is analyzed and the simplified KF is designed and verified with simulation for those vehicles with low-dynamic motion. In Section 5, the results from turntable test are given and the conclusion is in Section 6.
2. Integration System of M/S INS for Ship
As mobile platforms, ships are usually equipped with gimbal inertial navigation system or gimbal compass/log and other high accuracy navigation equipment to provide the ship motion information including speed, position, and attitude. With the excitation of wind wave, temperature difference, and so forth, deflection of ship deck will be generated. In this case, the navigation information from gimbal inertial navigation system cannot be used for weapons and (or) observation equipment because these types of equipment are installed at the head, tail, and highest position while MINS are at the center of ship [14]. Aiming to solve this problem, SINS with middle or low accuracy sensors are introduced and installed at the basis of the above-mentioned equipment to provide information excluding deck deformation. Generally, the accuracy of gimbal system is always higher than that of SINS with one order of magnitude. In order to use higher accuracy information from gimbal system for SINS including initial alignment and error correction, SINS and gimbal system are always integrated [14–16]. In this case, the gimbal system is defined as Master INS (MINS) while SINS is as Slave INS (SINS). In the rest of this paper, Strapdown INS is used as Slave INS and the difference is not distinguished.
In the M/S INS integrated system, MINS can provide various navigation information including velocity, position, and attitude with a lower update frequency than that of SINS. Note that when gyrocompass is used as MINS, the velocity and position can be provided with log. In the integration of M/S INS, matching methods of velocity plus attitude and velocity plus heading are always used [14–16]. The purpose of this paper is to find a simplified KF algorithm; thus only velocity plus heading is selected for analysis.
2.1. System State Equation
In the integrated system of M/S INS, errors of SINS can be selected as state variables to construct state vector and navigation information from MINS can be regarded as aided information to construct measurement vector. The error of velocity, position, and attitude of SINS and the bias of gyroscope are all observable variables with velocity plus heading matching. Observability analysis on integrated navigation system is a complex and big issue but it will not be discussed because of the purpose of this paper. Here the error of velocity, misalignment angles, and bias of gyroscope are selected to construct state vector to simplify the analysis. The state vector of M/S INS integrated system is as follows [16]:where and are the east and north velocity errors, respectively, , and are the misalignment angles of pitch, roll and yaw, respectively, and , and are gyro bias along -, and -axis respectively.
The system state equation can be constructed as follows [1, 8]: where is the system state transition matrix, is the system process noise, and is the noise interference input matrix. According to the error propagation equations of velocity error, misalignment angle, and gyro bias, the system state transition matrix can be expressed as follows:where and are east and north velocity, respectively, and are angular velocity and radius of the earth, respectively, is the geographical latitude of the carrier, , and denote the projection of the accelerometer measured data in navigation frame along east, north, and up direction, respectively, and are the elements of direct cosine matrix (DCM) of SINS.
2.2. System Measurement Equation
Take the differences of velocity and yaw between MINS and SINS as the measurement vector, where , and are the east and north velocity and heading from SINS, respectively, and , , and are those from MINS, respectively.
The system measurement equation can be constructed as follows [1, 8]:where is the measurement matrix and is the measurement noise. According to the relationship between the measurement vector and the state vector, the measurement matrix can be expressed as
According to (2), (5), and the KF introduced in Section 3, the data fusion between the MINS and SINS can be fulfilled.
3. Data Fusion with Kalman Filter
Kalman filter is an optimal filtering algorithm based on iterative calculation. In order to facilitate the iterative calculation of computer, the continuous system as (2) and (5) should be discretized and can be expressed as [1, 8]where is the th iterative update, is the state transition matrix, and is the noise input transition matrix.
When the system is accurately modeled and process noise and measurement noise are both white noises without correlation between each other, optimal estimation of state vector can be obtained by KF. If the statistic characteristics of and can be assumed as and , respectively, the iterative update of KF can be expressed as shown in Figure 1, where is the estimation of state vector, is the one-step predictive value of state vector, is the error covariance matrix of state vector, is the one-step predictive value of error covariance matrix of state vector, and is the filter gain matrix. From the calculation process, as shown in Figure 1, the filtering process can be divided into the loops of gain calculation and filter calculation. From the update process, the filtering process can be divided into the time update and measurement update stages.
In integrated navigation system with inertial base, there exists a case that KF will run without measurement update since the update frequency of SINS is always higher than that of aided system. In the standard KF algorithm, during time update process, and should be calculated, and should be iteratively updated, and also and should be updated for the following time update. The equations for time update are as follows [1, 8]:
Specific to the topic studied in this paper, the update frequency of SINS is always as 100 Hz while that of MINS is as 1 Hz. That means, during one measurement update period (1 s), 100 times of time update is needed, among which 99 times is only used to maintain the iterative calculation of KF. Note that when close-correction mode is used, the values of state vector will be cleared after the feedback correction is done; that is to say, the state vector and the predictive one are always zero before the next measurement update is executed and there is no need to update these vectors during time update but the calculation for , and , is needed.
Compared with that in measurement update, the calculation amount in time update is smaller, but even so the amount of computation for numbers of time update cannot be negligible. Take PC104 with a 333 MHz CPU as an example, and test results show that 15 ms is taken to complete a full KF process [16], 5 ms is needed for the calculations of , and , and 1 ms is taken to complete one navigation calculation. That is to say, during one update period of SINS, 6 ms is taken to complete the navigation calculation and the time update calculation for KF. When the update frequency is higher than 167 Hz, it is unable to complete the above calculatoins in one SINS update period, never minding the calculation for measurement update.
Reference [17] introduces the real-time multitasking operation system to solve the time-consuming problem of KF. The resources of computer can be made full use of by dividing tasks with different priorities, and then data fusion can be dispersed and fulfilled in multiple SINS navigation update periods. But if there is no idle resource in SINS navigation period, the above method cannot solve the problem. If the calculation amount of time update in the process of no measurement update can be reduced, the idle resources in the navigation calculation cycle can be guaranteed and the design difficulty of the system can be reduced. In the next section, the calculation for state transition matrix will be analyzed and a simplified KF algorithm will be presented.
4. A Simplified Kalman Filter for Integrated Navigation
4.1. A Simplified Kalman Filter with the Low-Dynamic Motion Constraint
Discretization should be carried out on the continuous error propagation equations as shown in (2) to get discrete equations as shown in Figure 1. Discretizing and ignoring higher order terms, there exists [1, 8]where is the unit matrix and is the filtering period. Specific to the integrated system of M/S INS, is the navigation update period of SINS.
Similar to operation as (9) on , there exists [1, 8]
Note that and in (8) are all functions of and is the function of which can be traced back to the fixed value at the zero point. Then it can be concluded that and are mainly determined by the elements of which is determined by the elements of while is the function of carrier motion parameters. From the parameters in , it can be seen that the changing speed of is decided by the dynamic of carrier. Taking the first element in as an example, denotes the coefficient of east velocity error to the changing rate of east velocity error. When the carrier is with low-dynamic condition, this coefficient is approximated as a constant.
Based on this understanding, in this paper, time update of KF is not executed but the variables in state transition matrix in one measurement update period are recorded. When the measurement is updated, time update of KF will be carried out with the recorded state transition matrix and at the last measurement update time.
Therefore, the process of time update in KF is changed to solve and accumulate it; then will be averaged at the measurement update time as follows:where is the total number of SINS calculations in one measurement update period. In this paper, M/S INS is studied and is set as 100.
Discretize (11) as follows:where is the measurement update period. With , related calculation in KF can be carried out. Note that the symbol used in (12) and all related equations in KF denote the number of iterative calculations. And the interval between and is in simplified algorithm while is used before simplification.
4.2. Performance Analysis on the Simplified Kalman Filter
For the convenience of comparison, the algorithm in Section 3 is defined as standard KF while that in Section 4.1 is defined as simplified KF. Obviously, compared with that in standard KF, times calculation of and times of matrix summation and one time of average calculation are needed in the simplified KF but matrix multiplication is omitted. Compared with the calculation amount in matrix multiplication, the above calculation in simplified KF can be ignored. Then, it can be concluded that the simplified KF can reduce computation amount of KF effectively.
Meanwhile, as analyzed in Section 4.1, elements in are the functions of carrier motion; thus both advantages and disadvantages will be brought in simplified KF as follows. When the noise in the parameters of carrier motion is big, the noise in will be directly influenced because the parameters are used to calculate directly in standard KF; then the variance of state estimation error will be directly influenced, while, in the simplified KF, negative effect of the noise in motion parameters will be smoothed for the summation and average operation on ; then it can be deduced that, with simplified KF, the estimation error in state vector will be suppressed and filtering accuracy will be increased. But when the carrier carries a sustained acceleration or deceleration, the standard KF can track the motion effectively but the simplified one cannot because the motion parameters are directly used in the former and average operations are in the latter. This means that simplified KF has a certain lag when the carrier has a sustained motion change. Obviously, the interval for summation and average, that is, the parameter in (11), plays an important role deciding the effect of smoothing and tracking and big interval takes better smoothing effects but brings large lag.
Take the elements and in as examples. When the carrier is with a constant velocity, can be expressed as , in which and are theatrical date and noise. With summation and average operation, can be smoothed, and the same conclusion on can be obtained; thus filter accuracy will be improved. When the carrier is with a constant velocity, can be expressed as , in which is the initial data of current summation period, is acceleration, and denotes the noise. With summation and average operation, and can also be smoothed, but the averaged at the point of measurement update are not the real-time velocity; thus a certain lag in filter result will be generated.
4.3. Simulation of Simplified Kalman Filter Algorithm
4.3.1. Simulation Condition
Assume that the ship swinging motion obeys the law of sine function and the swinging parameters are shown in Table 1. The translational motion is divided into two cases. In the first case, the ship sails along -axis of the carrier’s body frame with the speed of 10 m/s. In the second one, the ship motion is divided into three stages: () firstly it stills for 10 min; () then it accelerates along -axis with an acceleration of 0.05 m/s lasting for 100 s; () lastly it sails with a constant velocity. When white noises are added to ship’s theoretical motion, ship motion including noises can be taken as the aided information from MINS. The variances of white noise in east, north, and heading are set as , , and , respectively. It is assumed that the initial attitudes of ship are all zeros.
The precision of the sensors (including gyro and accelerometer) in SINS is set as in Table 2. The ideal output of sensor can be generated with the theoretical ship motion and reversal navigation solution. When the noise is added to these ideal outputs, actual outputs of sensor can be simulated. In order to simplify analysis, the lever-arm, deck deformation, and time delay between MINS and SINS are not considered in simulation. Then the theoretical value of ship motion can be used as references to evaluate the errors of SINS. During simulation, the update frequencies of SINS and MINS are set as 100 Hz and 1 Hz, respectively.
The parameters for standard KF and simplified KF are both set as follows:
4.3.2. Simulation Results and Analysis
Case 1 (without acceleration). The simulation lasts for 1200 s and the results are shown in Figures 2 and 3. Figures 2 and 3 are the errors curves of attitude and velocity, respectively, where the dotted lines and solid lines denote the results from standard KF and simplified KF, respectively.
The curves in Figures 2 and 3 clearly showed that both standard KF and simplified KF can fulfill data fusion of M/S INS integration when the ship sails with a constant velocity. During the whole process, error curves keep convergent and stable. But the curves also show that the amplitude of error curves obtained from standard KF is significantly larger than that of simplified one. Considering the adjustment stage of KF, the data during 201 s~1200 s are collected and the statistical results are shown in Table 3. Obviously, statistical data verified the deduction in Section 4.2 effectively. That is to say, the simplified algorithm can smooth the noise in carrier motion and improve the accuracy of data fusion.
Note that, in Case 1, the ship is sailing along -axis of body frame with a constant velocity, but the ship is swinging obeying sine function; when the motions are projected into navigation frame, there exist irregular sine motions along all axes along navigation frame. So, Case 1 can be regarded as a motion with approximate constant velocity and periodic changes of acceleration (or interference of acceleration). In other words, the simplified KF can be used when the carrier is with a constant velocity or with a small interference of acceleration.
(a) Pitch
(b) Roll
(c) Yaw
(a) East velocity
(b) North velocity
Case 2 (with acceleration). The simulation also lasts for 1200 s and the results are shown in Figures 4 and 5. Figures 4 and 5 are the errors curves of attitude and velocity, respectively, where the dotted lines and solid lines denote the results from standard KF and simplified KF, respectively.
The curves in Figures 4 and 5 clearly show that () when the ship is still or with a constant velocity, the errors obtained from two algorithms are similar to those in Figures 2 and 3, respectively, and there are no significant differences: () but at the beginning and stopping point of acceleration, there is a jump in north velocity and pitch; this phenomenon can be analyzed as the jump of north acceleration brings the jump of north velocity which further generates the jump in pitch, and the jump in standard KF is larger than that in simplified KF; and () during the acceleration and uniform motion stages, the jumps will both converge slowly but that from standard KF owns faster convergence speed. The above phenomena verified the deduction in Section 4.2 effectively. Namely, when the carrier is with a continuous acceleration or deceleration, the parameters of carrier motion will change; then the estimation with simplified KF has a certain delay.
(a) Pitch
(b) Roll
(c) Yaw
(a) East velocity
(b) North velocity
5. Turntable Test
5.1. Test Setting
The turntable used in this test is shown in Figure 6. In this turntable, the angle information of all frames can be provided via serial port as a response to external time-synchronization signal. In the test, the inner, intermediate, and outer frames are used to simulate the ship’s roll, pitch, and yaw, respectively. The swinging parameters are shown in Table 4.
The SINS used in the test is shown in Figure 6, in which flexible gyroscope and quartz accelerometers are used with the precision shown in Table 5. The SINS can receive external aided navigation data through network. In this test, the information from turntable is used as aided information; then the data of turntable should be sent firstly through serial port and resent through network. During the test, both SINS and turntable use the same time signal; after the time delays caused by data transitions, the data of turntable can be used as aided data among which only simulated heading is used as measurement. The update frequencies of the measurements from SINS and turntable test are 100 Hz and 1 Hz, respectively.
As shown in Figure 6, the SINS is installed in turntable with x-, y-, and -axis coinciding with the intermediate, inner, and outer frames, respectively. According to [16], sensors’ zero bias, scale factors, coupling coincident, installing error, and the variables of the flexible gyro related to gravity can be calculated and compensated by the exact calibration, so that the above-mentioned errors can be ignored if the test is followed by the calibration. Because the turntable cannot simulate translational motion which is with an ideal zero velocity, white noise with variance is used as the velocity measurement. And the swing data of turntable is used as the heading reference values.
5.2. Test Results and Analysis
The test lasts 1200 s and the results are shown in Figures 7 and 8 with the errors of attitude and velocity, respectively, where the dotted lines and solid lines denote the standard KF and simplified KF, respectively.
(a) Pitch
(b) Roll
(c) Yaw
(a) East velocity
(b) North velocity
Similar to those in Figures 2 and 3, the curves in Figures 7 and 8 show that both KFs can fulfill data fusion in M/S INS integration effectively. During the whole process, the curves keep convergent and stable. The main difference between two KFs is that the error amplitude from standard KF is larger than that from simplified one. Take the data from 201 s~1200 s for statistic and results are shown in Table 6. It can be seen that the results are similar to that in Section 4.3.2.
From the results in Figures 2, 3, 7, and 8 and the statistics in Tables 3 and 6, it can be concluded that the simplified KF can reduce calculation amount of KF effectively and improve data fusion accuracy when carrier is with a constant velocity or with a small interference of acceleration.
6. Conclusion
In this paper, the inconsistency problem caused by different update frequency of subnavigation systems in integrated navigation system is studied. The analysis indicates that standard KF needs a lot of time update calculation to maintain the iterative process because the update frequency of SINS is always higher than those of aided systems.
In order to lessen computation load caused by time update, the system state equation of the integrated system with inertial base is analyzed in detail, and the analysis indicates that the elements in state transition matrix are the functions of carrier motion. When the carrier is with low-dynamic motion, the changes of elements in state transition matrix are very slow. Based on this, this paper changes the time update process in standard KF with the calculation for state matrix and summation of it; when the measurement is updated, the accumulated state matrix will be averaged and time/measurement updates are carried out.
Further analysis indicates that, with the average operation in simplified KF, the noise caused by carrier motion will be smoothed and the accuracy of integrated system is improved, but when the carrier is with a continuous acceleration and deceleration, the estimation with simplified KF has a certain delay because of the average operation. Simulation and turntable test verified the advantage and disadvantage of the proposed simplified KF. The results show that when the carrier is with a constant velocity or a small interference of acceleration, the variance of estimation error can be effectively suppressed with a reduced calculation amount.
Competing Interests
The authors declare that they have no competing interests.
Acknowledgments
This work was supported in part by the National Natural Science Foundation (61273056).