Abstract
To meet the real-time and low power consumption demands in MEMS navigation and guidance field, an improved Kalman filter algorithm for GNSS/INS was proposed in this paper named as one-step prediction of matrix. Quantitative analysis of field test datasets was made to compare the navigation accuracy with the standard algorithm, which indicated that the degradation caused by the simplified algorithm is small enough compared to the navigation errors of the GNSS/INS system itself. Meanwhile, the computation load and time consumption of the algorithm decreased over 50% by the improved algorithm. The work has special significance for navigation applications that request low power consumption and strict real-time response, such as cellphone, wearable devices, and deeply coupled GNSS/INS systems.
1. Introduction
For the general dynamic navigation or mid- and low-end accuracy navigation applications, such as land-vehicle navigation (LVN), the equipment is required to be small, of low cost, and rugged; meanwhile the navigation algorithm should be optimized to achieve high efficiency, real-time response, and low power consumption. In general dynamic navigation field, the most popular and effective method is integrated navigation system combining global navigation satellite system (GNSS) and inertial navigation system (INS) [1]. GNSS has advantages of all-weather work, high precision, and low cost but is easy to be interfered by environment and external signals [2]. INS is a self-contained navigation system with fine short-term accuracy, but its navigation accuracy will degrade with time due to the errors produced by the sensors [3]. GNSS and INS have strong complementarity with each other in many aspects like error characteristics. GNSS/INS draws the world’s attention as the “gold combination,” which has broad application prospects in both military and civilian fields. The fusion of the navigation information from INS and GNSS is usually made by Kalman filter (KF).
Kalman filter is a linear minimum variance estimation proposed for the first time in 1960 [4]. With the knowledge of the system state model, measurement state model, and the state noise and the measurement noise, Kalman filter can estimate dynamic system state from a series of noisy sensor data. Kalman filter is the most popular method of all the present filtering methods, which has been used in many fields of communication, navigation, guidance, and control. One of the most successful applications is the integrated navigation system.
In the early age of computer technology, the algorithm complexity is strictly restricted by the computer’s memory and operation speed. Algorithm simplification of the navigation system, including the Kalman filter for integrated navigation, caught the attention of the researchers as a necessary job. Then, as computer hardware advanced with Moore’s law, the algorithm simplification became unnecessary for the new processors with more and more power and memory. Until microelectromechanical system (MEMS) inertial measurement unit (IMU) is widely used recently in consumer electronics products, like cellphone, wearable devices, and so on, the algorithm simplification arouses researcher’s interests again because inertial navigation algorithm has to keep running at backstage all the time in principle so as to provide seamless location data. For such consumer portable applications, it has certain requirements to the power consumption and the real-time performance of the GNSS/INS yet is not severe on the navigation accuracy. In order to meet the needs of low power consumption and real-time response, the improvement work based on the standard integrated navigation algorithm is particularly important and necessary.
GNSS/INS algorithm is mainly composed of two parts: INS mechanization and Kalman filter. INS algorithm improvement has been well studied to minimize the computation load. Zhang et al. modified position, velocity, and attitude update by omitting rotation correction and sculling correction as minor terms and assuming the angular rate as a constant in a short sampling interval [5]. Shin and Bortz et al. made intensive discourses on the INS error equations and presented new simplified INS mechanization [6, 7]. However, the INS computations are not the major part of the computational load of a GNSS/INS algorithm, so the simplified analysis to the INS mechanization did not fulfill the demands of improving the computational efficiency well.
As is known to all, Kalman filter occupies a large amount of calculation as a result of its enormous matrix operations. The theoretical analysis and application in practice indicate that, in Kalman filter, the most time consuming process is the prediction of the state covariance matrix ( matrix) by approximate 70% of the whole Kalman filter operations. Meanwhile, matrix outer product method was proposed to decrease the KF prediction load by Xurong and Daxin in 1997 [8]. Based on this method, the system can achieve a speedup of 8 over the standard Kalman filter. As we all know, the state space model of GNSS/INS has some special characteristics, including sparse system matrix and symmetric state covariance matrix. Based on the above characteristics, Zhu et al. proposed a rapid computation method, which makes the prediction of state covariance matrix to be expanded rather than computing by a generic matrix function, to further reduce calculation load [9]. As early as in 1979, Maybeck proposed a set of means to reduce the complexity of the navigation Kalman filter to meet performance specifications and the practical constraints imposed by the computer at that time [10]. However, no theoretical derivation or result analysis was made to provide an intuitive cognition of the improvement of the algorithm complexity. Though the computational efficiency of Kalman filter has been studied by some people, few researches have achieved the demands of reducing computation load and took actual applications and implementation into analysis. Based on one of the methods proposed in Maybeck’s book, which is to predict the matrix between measurement update epochs instead of INS update epochs, this paper further deduces its rationality by equation derivation, proves its applicability by the error loss analysis, and shows its contribution by algorithm complexity analysis. The method will be named as “one-step prediction of matrix” in Kalman filter for GNSS/INS integration in this paper.
This paper is arranged as follows: Section 2 firstly derives the improved Kalman filter algorithm “one-step prediction of matrix” proposed in this paper, gives the Kalman filter design applied for GNSS/INS briefly, and then investigates the improved computation efficiency through counting arithmetic operands. In Section 3, a field test data processing is presented to verify the performance of the proposed improved algorithm through the analysis of the navigation accuracy degradation and the percentage of computation saving. Section 4 is the summary and conclusion.
2. Methodology
2.1. Kalman Filtering Algorithm Improvement
Kalman described the Kalman filter algorithm in detail [4]. And the five basic equations of GNSS/INS integrated navigation system (loosely coupled as example) are expressed as follows [4, 11].
Prediction:
Update:where is the state vector; stands for the transition matrix; is the system noise covariance matrix; is the state covariance matrix as mentioned before; is the design matrix; is the observation noise covariance matrix; is the Kalman gain matrix which determines the weights of GPS information when status is updated; is the observation vector; means the time interval from epoch to epoch (). All the subscripts indicate the state change; for example, represents the state vector from epoch to epoch .
In standard integrated navigation algorithm of GNSS/INS, the prediction is conducted for each INS epoch (such as 200 Hz); however, the update is carried out only when the GNSS information is available (e.g., 1 Hz). And, from the analysis of (1) to (6), we can find that the function of only determines the weighting and provides the accuracy index of the estimation results but is not so critical to the estimation values. Thus, the times of calculating matrix prediction can be reduced to save computation load. For example, only one matrix prediction step is taken in one update cycle, which makes the prediction frequency the same as the measurement update so as to save significant computation under the condition of acceptable accuracy. The proposed method is named as “one-step prediction of matrix,” which is a kind of simplified GNSS/INS navigation algorithm that can improve the efficiency. The flow charts of the standard and the improved integrated navigation algorithms of GNSS/INS are shown in Figure 1.
(a) Standard algorithm
(b) Improved algorithm
In Figure 1(b), the prediction of state vector is omitted due to its resetting to zero after Kalman filter update and feedback each time, which makes it have no sense to be predicted. From comprehensive analysis of (1) to (6), the update accuracy is determined by the state transition matrix . Therefore, the solution of is put forward for consideration.
The state transition matrix is determined by corresponding time navigation state and time interval. Based on the navigation state, three different simplified methods were proposed to calculate :
(1) The median: is taken into calculation with the use of the median navigation state between two measurement updates.
(2) The mean: is taken into calculation with the use of the mean navigation state from the previous measurement update to the current measurement update.
(3) The multiplication: is taken into calculation by each epoch, and then all the between the adjacent epochs are multiplied one by one to get the total state transition matrix. Before Kalman filter update, take the multiplicative value of into (2) and (3). The theoretical derivation process is given in detail as follows.
In standard Kalman filter, prediction is taken at each epoch. Taking two adjacent epochs as example, matrices predicted from epoch to epoch have two steps as follows. Here we assume that the measurement update of Kalman filter in the th epoch is unavailable.
At epoch , defineand then
At epoch , defineand thenwhere is the spectral density matrix which is constant and is the noise-input mapping matrix which is related to the attitudes [12].
Apply the proposed one-step matrix prediction to the above example. Prediction from epoch to epoch is skipped; thus the expression of predicted matrix should be changed as
In (11), the second and third terms of the right side (named as and hereafter) are calculated as follows:
Relative to the standard Kalman filter, the algorithm accuracy of (11) has no loss so far. But the third term of the right side of the equation shows that matrix in the th epoch needs to be used in the th epoch. When the predicted frequency is far higher than the update frequency, the storage of matrix will be increased greatly, which improves the complexity of the algorithm. So, to keep the same shape with the first term of the right side in (11) and simplify the computation load simultaneously, (13) are put forward to replace (12). Consider
To a stable navigation system, the sampling interval is normally equal and minor (e.g., 0.005 s for INS):
Besides, the system noise modeled as white noise is time-independent, and (15) is acceptable. Consider
Based on the previous conditions, the difference between and is 2nd-order small quantity of and can be approximated to zero matrix (see Appendix A for detailed derivation):
Finally, the expression of matrix predicted from epoch directly to epoch turns to where
Similarly, extend the above derivation to the one-step prediction of epochs, that is, prediction from epoch and to epoch ; the matrix prediction has the following form:where
To the general dynamic navigation with GNSS/INS, one-step prediction of matrix may save lots of calculations; and the consequent navigation error will be investigated in Section 3.
2.2. Kalman Filter for GNSS/INS Integration
Kalman filter is the most popular technique for GNSS/INS integration. The linear continuous-time system of augmentation of an INS error model with the sensor error models can be expressed as follows [12]:where is the dynamics matrix; is the noise-input mapping matrix; is the system noise vector; is the 21-state vector of the following form:where , are the position and velocity error vector; is the attitude error; and are the gyros and accelerometers biases; and are the gyros and accelerometers scale factor errors.
The observation equation is designed as follows using the loosely coupled implementation of GNSS/INS integration as example [12]:where is the design matrix; is the measurement noise; is the measurement vector which is the difference between GNSS and INS navigation state. At last the estimated state vector is fed back as compensation to the INS.
2.3. Analysis on Computation Load
The efficiencies of the four algorithms (i.e., the standard and the three simplified) can be compared in arithmetic operands (e.g., adds and subtracts (A&S), multiples (M), divisions (D), square roots (SR), and trigonometric (T)) [10]. Concretely, only Kalman filter was investigated in detail in one GNSS measurement update cycle (1 s), because Kalman filter occupies the major proportion in the whole GNSS/INS integrated navigation algorithm.
Table 1 presents the number of operations required for one complete Kalman filter cycle (1 s). From this table, it is clear that the simplified algorithms have much higher computation efficiency than the standard one, which reduced the arithmetic operands by about 98%, 98%, and 74%, respectively. Obviously, the simplified algorithm will have significant contribution in terms of power saving and real-time response.
3. Results and Analysis
The detailed introduction of the improved algorithm is given in previous description; meanwhile the efficiency improvement has been shown through arithmetic operands statistics. To verify the impact on navigation accuracy and actual computation time saving of the proposed simplified algorithms, we made evaluations through processing field test datasets.
3.1. Test Description
Field test was conducted at Wuhan city in 2013, covering both urban and suburban areas. The test trajectory is shown in Figure 2, composed of straight, turns, and the other typical land-vehicle motions. Road scenes are rich, containing open sky and Global Positioning System (GPS) signal block sections.
The test takes X-sens MTi-G module as a typical low-end MEMS IMU and SPAN-FSAS as a middle-grade IMU. Meanwhile, a high accuracy navigation system combining GPS RTK with navigation grade IMU is included to provide the navigation reference [13, 14]. Table 2 gives the specifications of MTi-G and FSAS based on lab testing after calibration.
It is known that the statistic summary of the navigation errors, such as RMS, max, and mean values, is used as metric for evaluating the navigation performance of GNSS/INS integration systems, when GNSS information is available [15]. But for INS, the error drifting with time can be well mitigated by the continuous GNSS update. Hence the estimation to the simplified algorithm should be taken when INS works alone, like during simulated GNSS signal outages. In another word, the position drift error under the condition of GPS signal outages (e.g., 60 seconds each) is used to evaluate whether the GNSS/INS is working properly [16]. Therefore, the navigation errors obtained for each processing scenario during such GPS blockage were then used to evaluate the performance of the navigation system in all cases.
Three sets of data were collected through the test, including low-end MTi-G and middle-grade FSAS. By the analysis of the postprocessing results of these data, we have found that navigation accuracy degradation and efficiency improvement have no relation with the IMU grade. Thus, to make the conciseness of this paper, only one MTi-G dataset’s processing result and analysis are given in detail, along with the brief summary results of the other two datasets.
3.2. Navigation Accuracy Evaluation
Navigation accuracy evaluation is to analyze the navigation errors of position, velocity, and attitude. However, only the horizontal error (position drift error in GPS signal outages) is considered in our paper since the vertical errors are not the major concern in general dynamic navigation. Meanwhile, the corresponding standard deviations (STD) of the estimation error from the Kalman filter are compared to the actual error level to verify whether the filter is working correctly.
However, the median method and the mean method all result in unacceptable degradation of yaw error over 40% compared to the standard algorithm. Thus, only the results of the multiplication method will be given in this section. The comprehensive analysis including navigation accuracy evaluation and time efficiency verification using the multiplication method is given with one set of data. (See Appendix B for the results of the other two simplified methods.)
Firstly, we will compare the discrepancy of navigation errors with standard and simplified GPS/INS algorithms.
Figures 3 and 4 show the navigation errors when using the standard and the simplified GPS/INS integrated navigation algorithms, respectively. From the comparison of Figures 3 and 4, the navigation errors of these two algorithms are at the same grade.
(a)
(b)
(c)
(a)
(b)
(c)
To get a more specific comparison, quantitative analysis to the navigation errors was made as shown in Table 3. The degradation in Table 3 means the error degradation percent with respect to the standard algorithm results of navigation errors. Table 3 shows that the degradations of position, velocity, and attitude errors are all less than 2%. Here please note that only one corresponding standard deviation is calculated after the prediction and update in one single filter period (i.e., P+) for the simplified algorithm since it has only one-step prediction of matrix, while, for the standard algorithm, the corresponding standard deviation is calculated in every epoch after the filter prediction (i.e., P−) or update (i.e., P+). So the statistic value of the corresponding standard deviations of the simplified algorithm is slightly smaller than that of the standard algorithm, which is reasonable in theory. According to Table 3, for both the standard algorithm and the simplified algorithm, the corresponding standard deviations and the actual navigation errors level (RMS values) are consistent in general. This implies that the Kalman filter works in a healthy status in this case.
According to Figures 3 and 4 and Table 3, it can be concluded that the navigation accuracy can be guaranteed by the simplified GPS/INS algorithm when GPS update is available.
As mentioned at the beginning of this section, the best way to verify the performance of GNSS/INS is to simulate GPS signal outages and then analyze the drift errors of INS. Totally 19 GPS signal outages (60 s each) were simulated during the postprocessing of the field test data to evaluate the performance of the simplified GPS/INS integrated navigation algorithm.
Figures 5 and 6 are the navigation errors of the standard and the simplified GPS/INS algorithms with GPS outages. Most position drift errors are less than 100 meters after losing GPS update for 60 s, which fit the performance of low-end MEMS INS. Figures 5 and 6 show great similarity again. In fact, the INS drift errors are almost the same case by case.
(a)
(b)
(c)
(a)
(b)
(c)
The statistic summary of the position drift error during GPS outages is given in Table 4. According to Table 4, the difference of the standard and the simplified algorithms are almost the same (less than 1%). The position drift errors of the corresponding standard deviations match the actual position drift errors well. Furthermore, the position drift errors of corresponding standard deviations between standard and simplified algorithms are close enough to each other.
According to the results analysis above, it is clear that the simplified algorithm does not degrade the navigation accuracy of low-end GNSS/INS system, and the filter provides consistent standard deviation of the estimation errors. In order to verify the generality of this conclusion, the results of the other two datasets in the field test were also compared. Here only the statistic summary of the navigation accuracy results during GPS outages is given as representative results.
Table 5 gives the RMS of position drift errors and the corresponding estimated standard deviations for another MTi-G dataset and FSAS dataset during GPS outages. From this table, the simplified algorithm has almost no degradation, not only for low-end MEMS system but also for middle-grade system; meanwhile the RMS of position drift errors is coincident with the estimated standard deviations from Kalman filter.
In summary, the proposed simplified GPS/INS algorithm does not cause degradation of navigation accuracy. In other words, the algorithm simplification does not sacrifice accuracy.
3.3. Time Efficiency Verification
Arithmetic operands do not have a one-to-one relationship with the executing time of the program. In order to have a more intuitive evaluation to the efficiency of the proposed improved algorithm, the program running time is counted by the timing function. However, the program running time is associated with not only the algorithm complexity, but also the current running computer software and hardware performance, such as computer configuration and running applications, making it impossible to calculate absolute algorithm running time. Thus the approximate time acquired in the same environment is only used to make relative comparison.
Table 6 presents the time consumption of the standard algorithm and the improved algorithm that process three GPS/INS road test datasets. The durations of these datasets are 1.8 hours, 2.5 hours, and 2.6 hours, respectively, while FSAS has double sampling rate of MTi-G. Similar to Table 1, the simplified algorithm takes much less time in processing the datasets. All results of the three datasets show that the time consumption decreases by 54% approximately. This value indicates that the time saving by the simplified algorithm is not related to the data length to be processed. However, it is not completely consistent with the result of Table 1 because of incomplete statistics of arithmetic operands, different CPU occupancy time of different operation types, and the other background running threats of the computer as mentioned above. For the GNSS/INS integrated navigation algorithm, it has made a great reduction in time consumption, which can promote the real-time implementation of GNSS/INS navigation algorithm effectively.
4. Conclusions
In this paper, a GNSS/INS navigation algorithm improvement, that is, one-step prediction of P matrix in Kalman filter, is proposed and evaluated. By reducing the prediction rate to be the same as the GNSS update rate, the one-step prediction algorithm saves dramatic computation load. Field test data processing results were used to evaluate the new algorithm in terms of accuracy degradation and computation efficiency. Compared to the standard algorithm, the navigation result degradations of the new algorithm are all less than 3%, which is utterly acceptable. The field test data processing also proved the expected computation efficiency. Compared to the standard algorithm, one-step prediction algorithm reduces the time consumption for over 50%, which is meaningful saving to the real-time response and power consumption of navigation system. In general, the negligible accuracy loss, effective computation time saving, and low power consumption brought by the improved algorithm can all make great contribution to the real-time realization of navigation algorithm and promote the use of low-cost MEMS IMUs.
The future work is to apply the proposed one-step prediction of matrix algorithm to the real applications that request strict real-time response, such as deeply coupled GNSS/INS systems.
Appendices
A. Detailed Deviation of (16)
Proof (16) is approximate zero matrix:
It is known that if is very small, the state transition matrix can be calculated by the following numerical approximation:
Then suppose that
Considering (14) and (15), (A.1) can be derived as follows:
In (A.4), all terms are high-order small quantity of . So it is acceptable to approximate (A.4) to zero matrix.
B. Navigation Accuracy Evaluations for the Other Two Simplified Methods
In this appendix, the navigation accuracy evaluations based on the median method and the mean method will be done.
Figures 7 and 8 present the navigation errors of the simplified GPS/INS algorithms based on the median and the mean methods, respectively. Compared with Figure 3, not much discrepancy can be found as the scale of the vertical axis. Comparisons with navigation errors of the standard algorithm will be made with the quantitative data analysis as shown in Tables 7 and 8.
(a)
(b)
(c)
(a)
(b)
(c)
From Table 7, some more distinct differences have existed by about 10.8% and 6.5% in north and east position degradation, and the heading degradation is even up to 40%. Table 8 shows similar result to Table 7, but its heading degradation is more severe (52%). This tremendous accuracy loss is not acceptable.
The principle of algorithm simplification is under the premise of guarantee accuracy. The median and mean methods have broken this bottom line, which makes it meaningless to take further time efficiency verification.
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 of China (NSFC, 41174028, 41304004, and 41404029), the Fundamental Research Funds for Central Universities (2042014kf0258), and the National High Technology Research and Development Program of China (no. 2015AA124002). Dr. Zhiqin Zhu from the School of Geodesy and Geomatics at Wuhan University is thanked for providing SPAN-FSAS. Tisheng Zhang, Xin Yang, Jian Kuang, and Zhongping Guo from the GNSS Research Center at Wuhan University are acknowledged for collecting the field test datasets used in this paper.