# Efficiency Improvement of Kalman Filter for GNSS/INS through One-Step Prediction ofP Matrix

Authors: Qingli Li; Yalong Ban; Xiaoji Niu; Quan Zhang; Linlin Gong; Jingnan Liu
Journal: Mathematical Problems in Engineering
 (2015)

---

## 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 ofP 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.


---

## Body

## 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 (P 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 F 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 P 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 P matrix” in Kalman filter for GNSS/INS integration in this paper.This paper is arranged as follows: Section2 firstly derives the improved Kalman filter algorithm “one-step prediction of P 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:(1)xk,k-1=Φk,k-1xk-1,(2)Qk-1=12Φk,k-1Q-+Q-Φk,k-1TΔtk,(3)Pk,k-1=Φk,k-1Pk-1Φk,k-1T+Qk-1.Update:(4)Kk=Pk,k-1HkTHkPk,k-1HkT+Rk-1,(5)xk=xk,k-1+Kkzk-Hkxk,k-1,(6)Pk=I-KkHkPk,k-1I-KkHkT+KkRkKkT,where x is the state vector; Φ stands for the transition matrix; Q is the system noise covariance matrix; P is the state covariance matrix as mentioned before; H is the design matrix; R is the observation noise covariance matrix; K is the Kalman gain matrix which determines the weights of GPS information when status is updated; z is the observation vector; Δtk means the time interval from epoch k-1 to epoch k (Δtk=tk-tk-1). All the subscripts indicate the state change; for example, xk,k-1 represents the state vector from epoch k-1 to epoch k.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 P 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 P matrix prediction can be reduced to save computation load. For example, only one P 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 P 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.Figure 1
The flow charts of GNSS/INS integrated navigation algorithms.
(a)
Standard algorithm
(b)
Improved algorithmIn Figure1(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,P matrices predicted from epoch k to epoch k+2 have two steps as follows. Here we assume that the measurement update of Kalman filter in the (k+1)th epoch is unavailable.At epochk, define(7a)Q-k=GktQtGktTand then(7b)Qk=12Φk+1,kQ-k+Q-kΦk+1,kTΔtk+1,(8)Pk+1,k=Φk+1,kPkΦk+1,kT+Qk.At epochk+1, define(9a)Q-k+1=Gk+1tQtGk+1tTand then(9b)Qk+1=12Φk+2,k+1Q-k+1+Q-k+1Φk+2,k+1TΔtk+2,(10)Pk+2,k+1=Φk+2,k+1Pk+1Φk+2,k+1T+Qk+1,where Q(t) is the spectral density matrix which is constant and G(t) is the noise-input mapping matrix which is related to the attitudes [12].Apply the proposed one-stepP matrix prediction to the above example. Prediction from epoch k to epoch k+1 is skipped; thus the expression of predicted Pk+2,k+1 matrix should be changed as (11)Pk+2,k+1=Φk+2,k+1Pk+1Φk+2,k+1T+Qk+1=Φk+2,k+1Φk+1,kPkΦk+1,kT+QkΦk+2,k+1T+Qk+1=Φk+2,k+1Φk+1,kPkΦk+2,k+1Φk+1,kT+Qk+1+Φk+2,k+1QkΦk+2,k+1T.In (11), the second and third terms of the right side (named as s1 and s2 hereafter) are calculated as follows:(12)
s
1
=
Q
k
+
1
=
1
2
Φ
k
+
2
,
k
+
1
Q
-
k
+
1
+
Q
-
k
+
1
Φ
k
+
2
,
k
+
1
T
Δ
t
k
+
2
,
s
2
=
Φ
k
+
2
,
k
+
1
Q
k
Φ
k
+
2
,
k
+
1
T
=
Φ
k
+
2
,
k
+
1
1
2
Φ
k
+
1
,
k
Q
-
k
+
Q
-
k
Φ
k
+
1
,
k
T
Δ
t
k
+
1
·
Φ
k
+
2
,
k
+
1
T
=
1
2
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
Q
-
k
+
Q
-
k
Φ
k
+
1
,
k
T
·
Φ
k
+
2
,
k
+
1
T
Δ
t
k
+
1
=
1
2
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
Q
-
k
Φ
k
+
2
,
k
+
1
T
+
Φ
k
+
2
,
k
+
1
Q
-
k
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
T
Δ
t
k
+
1
.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 Q matrix in the kth epoch needs to be used in the (k+1)th epoch. When the predicted frequency is far higher than the update frequency, the storage of Q 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(13)
s
3
=
1
2
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
Q
-
k
+
1
+
Q
-
k
+
1
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
T
Δ
t
k
+
2
,
s
4
=
1
2
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
Q
-
k
+
1
+
Q
-
k
+
1
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
T
Δ
t
k
+
1
.To a stable navigation system, the sampling interval is normally equal and minor (e.g., 0.005 s for INS):(14)Δtk+2=Δtk+1=Δt.Besides, the system noise modeled as white noise is time-independent, and (15) is acceptable. Consider(15)Q-k≈Q-k+1.Based on the previous conditions, the difference betweens1+s2 and s3+s4 is 2nd-order small quantity of Δt and can be approximated to zero matrix (see Appendix A for detailed derivation):(16)v=s3+s4-s1+s2≈0.Finally, the expression ofP matrix predicted from epoch k directly to epoch k+2 turns to (17)Pk+2,k=Φk+2,k+1Φk+1,kPkΦk+2,k+1Φk+1,kT+Qk+1,2Δt,where(18)
Q
k
+
1,2
Δ
t
=
1
2
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
Q
-
k
+
1
+
Q
-
k
+
1
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
T
·
2
Δ
t
.Similarly, extend the above derivation to the one-step prediction ofn epochs, that is, prediction from epoch k and to epoch k+n; the P matrix prediction has the following form:(19)Pk+n,k=Φk+n,kPkΦk+n,kT+12Φk+n,kQ-k+n-1+Q-k+n-1Φk+n,kT·nΔt,where(20)Φk+n,k=Φk+n,k+n-1Φk+n-1,k+n-2⋯Φk+2,k+1Φk+1,k.To the general dynamic navigation with GNSS/INS, one-step prediction ofP 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]:(21)x˙=Fx+Gw,where F is the dynamics matrix; G is the noise-input mapping matrix; w is the system noise vector; x is the 21-state vector of the following form:(22)x=δrnδvnψnbgbasgsaT,where δrn, δvn are the position and velocity error vector; ψn is the attitude error; bg and ba are the gyros and accelerometers biases; sg and sa 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]:(23)z=Hx+v,where H is the design matrix; v is the measurement noise; z 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.Table1 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.Table 1
Comparisons on arithmetic operand number (in 1 s cycle) and improved efficiency between standard and simplified GNSS/INS algorithm.
A&S
M
D
SR
T
Standard algorithm
3701430
3847308
1852
303
717
Median  algorithm
64716 (98.3%)
67560 (98.2%)
70 (96.2%)
6 (98.0%)
24 (96.7%)
Mean algorithm
66326 (98.2%)
67810 (98.2%)
183 (90.1%)
107 (64.7%)
343 (52.2%)
Multiplication algorithm
953226 (74.2%)
1008366 (73.8%)
169 (90.8%)
105 (65.3%)
24 (96.7%)


## 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:(1)xk,k-1=Φk,k-1xk-1,(2)Qk-1=12Φk,k-1Q-+Q-Φk,k-1TΔtk,(3)Pk,k-1=Φk,k-1Pk-1Φk,k-1T+Qk-1.Update:(4)Kk=Pk,k-1HkTHkPk,k-1HkT+Rk-1,(5)xk=xk,k-1+Kkzk-Hkxk,k-1,(6)Pk=I-KkHkPk,k-1I-KkHkT+KkRkKkT,where x is the state vector; Φ stands for the transition matrix; Q is the system noise covariance matrix; P is the state covariance matrix as mentioned before; H is the design matrix; R is the observation noise covariance matrix; K is the Kalman gain matrix which determines the weights of GPS information when status is updated; z is the observation vector; Δtk means the time interval from epoch k-1 to epoch k (Δtk=tk-tk-1). All the subscripts indicate the state change; for example, xk,k-1 represents the state vector from epoch k-1 to epoch k.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 P 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 P matrix prediction can be reduced to save computation load. For example, only one P 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 P 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.Figure 1
The flow charts of GNSS/INS integrated navigation algorithms.
(a)
Standard algorithm
(b)
Improved algorithmIn Figure1(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,P matrices predicted from epoch k to epoch k+2 have two steps as follows. Here we assume that the measurement update of Kalman filter in the (k+1)th epoch is unavailable.At epochk, define(7a)Q-k=GktQtGktTand then(7b)Qk=12Φk+1,kQ-k+Q-kΦk+1,kTΔtk+1,(8)Pk+1,k=Φk+1,kPkΦk+1,kT+Qk.At epochk+1, define(9a)Q-k+1=Gk+1tQtGk+1tTand then(9b)Qk+1=12Φk+2,k+1Q-k+1+Q-k+1Φk+2,k+1TΔtk+2,(10)Pk+2,k+1=Φk+2,k+1Pk+1Φk+2,k+1T+Qk+1,where Q(t) is the spectral density matrix which is constant and G(t) is the noise-input mapping matrix which is related to the attitudes [12].Apply the proposed one-stepP matrix prediction to the above example. Prediction from epoch k to epoch k+1 is skipped; thus the expression of predicted Pk+2,k+1 matrix should be changed as (11)Pk+2,k+1=Φk+2,k+1Pk+1Φk+2,k+1T+Qk+1=Φk+2,k+1Φk+1,kPkΦk+1,kT+QkΦk+2,k+1T+Qk+1=Φk+2,k+1Φk+1,kPkΦk+2,k+1Φk+1,kT+Qk+1+Φk+2,k+1QkΦk+2,k+1T.In (11), the second and third terms of the right side (named as s1 and s2 hereafter) are calculated as follows:(12)
s
1
=
Q
k
+
1
=
1
2
Φ
k
+
2
,
k
+
1
Q
-
k
+
1
+
Q
-
k
+
1
Φ
k
+
2
,
k
+
1
T
Δ
t
k
+
2
,
s
2
=
Φ
k
+
2
,
k
+
1
Q
k
Φ
k
+
2
,
k
+
1
T
=
Φ
k
+
2
,
k
+
1
1
2
Φ
k
+
1
,
k
Q
-
k
+
Q
-
k
Φ
k
+
1
,
k
T
Δ
t
k
+
1
·
Φ
k
+
2
,
k
+
1
T
=
1
2
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
Q
-
k
+
Q
-
k
Φ
k
+
1
,
k
T
·
Φ
k
+
2
,
k
+
1
T
Δ
t
k
+
1
=
1
2
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
Q
-
k
Φ
k
+
2
,
k
+
1
T
+
Φ
k
+
2
,
k
+
1
Q
-
k
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
T
Δ
t
k
+
1
.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 Q matrix in the kth epoch needs to be used in the (k+1)th epoch. When the predicted frequency is far higher than the update frequency, the storage of Q 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(13)
s
3
=
1
2
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
Q
-
k
+
1
+
Q
-
k
+
1
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
T
Δ
t
k
+
2
,
s
4
=
1
2
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
Q
-
k
+
1
+
Q
-
k
+
1
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
T
Δ
t
k
+
1
.To a stable navigation system, the sampling interval is normally equal and minor (e.g., 0.005 s for INS):(14)Δtk+2=Δtk+1=Δt.Besides, the system noise modeled as white noise is time-independent, and (15) is acceptable. Consider(15)Q-k≈Q-k+1.Based on the previous conditions, the difference betweens1+s2 and s3+s4 is 2nd-order small quantity of Δt and can be approximated to zero matrix (see Appendix A for detailed derivation):(16)v=s3+s4-s1+s2≈0.Finally, the expression ofP matrix predicted from epoch k directly to epoch k+2 turns to (17)Pk+2,k=Φk+2,k+1Φk+1,kPkΦk+2,k+1Φk+1,kT+Qk+1,2Δt,where(18)
Q
k
+
1,2
Δ
t
=
1
2
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
Q
-
k
+
1
+
Q
-
k
+
1
Φ
k
+
2
,
k
+
1
Φ
k
+
1
,
k
T
·
2
Δ
t
.Similarly, extend the above derivation to the one-step prediction ofn epochs, that is, prediction from epoch k and to epoch k+n; the P matrix prediction has the following form:(19)Pk+n,k=Φk+n,kPkΦk+n,kT+12Φk+n,kQ-k+n-1+Q-k+n-1Φk+n,kT·nΔt,where(20)Φk+n,k=Φk+n,k+n-1Φk+n-1,k+n-2⋯Φk+2,k+1Φk+1,k.To the general dynamic navigation with GNSS/INS, one-step prediction ofP 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]:(21)x˙=Fx+Gw,where F is the dynamics matrix; G is the noise-input mapping matrix; w is the system noise vector; x is the 21-state vector of the following form:(22)x=δrnδvnψnbgbasgsaT,where δrn, δvn are the position and velocity error vector; ψn is the attitude error; bg and ba are the gyros and accelerometers biases; sg and sa 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]:(23)z=Hx+v,where H is the design matrix; v is the measurement noise; z 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.Table1 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.Table 1
Comparisons on arithmetic operand number (in 1 s cycle) and improved efficiency between standard and simplified GNSS/INS algorithm.
A&S
M
D
SR
T
Standard algorithm
3701430
3847308
1852
303
717
Median  algorithm
64716 (98.3%)
67560 (98.2%)
70 (96.2%)
6 (98.0%)
24 (96.7%)
Mean algorithm
66326 (98.2%)
67810 (98.2%)
183 (90.1%)
107 (64.7%)
343 (52.2%)
Multiplication algorithm
953226 (74.2%)
1008366 (73.8%)
169 (90.8%)
105 (65.3%)
24 (96.7%)


## 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 Figure2, 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.Figure 2
Trajectory of the GPS/INS field test.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.Table 2
IMU characteristics after lab calibration.
Measurement
MTi-G
FSAS
Gyros
Bias (°/h)
36
0.75
ARW (°/h)
3
0.1
Scale factor (ppm)
300
300
Accelerometers
Bias (mGal)
2000
1000
VRW (m/s/h)
0.12
0.03
Scale factor (ppm)
300
300It 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 AppendixB 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.Figures3 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.Figure 3
Navigation errors ofstandard GPS/INS algorithm (#MTi-G).
(a)
(b)
(c)Figure 4
Navigation errors ofsimplified GPS/INS algorithm (#MTi-G).
(a)
(b)
(c)To get a more specific comparison, quantitative analysis to the navigation errors was made as shown in Table3. 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 P 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.Table 3
Statistic values of the navigation errors and the estimated standard deviations of the standard and the simplified GPS/INS algorithms (#MTi-G).
Mean
RMS
Max
STD (sqrt ofP matrix)
Position
North (m)
Standard
0.022
0.030
0.186
0.033
Simplified
0.022
0.030
0.188
0.025
Degradation (%)
0.00
0.00
1.08
—
East (m)
Standard
0.020
0.026
0.194
0.031
Simplified
0.020
0.026
0.194
0.023
Degradation (%)
0.00
0.00
0.00
—
Velocity
North (m/s)
Standard
0.014
0.019
0.167
0.026
Simplified
0.014
0.019
0.169
0.019
Degradation (%)
0.00
0.00
1.20
—
East (m/s)
Standard
0.013
0.017
0.086
0.026
Simplified
0.013
0.017
0.086
0.019
Degradation (%)
0.00
0.00
0.00
—
Attitude
Roll (°)
Standard
0.073
0.091
0.433
0.094
Simplified
0.074
0.092
0.436
0.085
Degradation (%)
1.37
1.10
0.69
—
Pitch (°)
Standard
0.105
0.125
0.506
0.093
Simplified
0.105
0.125
0.507
0.084
Degradation (%)
0.00
0.00
0.20
—
Heading (°)
Standard
0.698
0.861
2.596
0.719
Simplified
0.677
0.838
2.549
0.705
Degradation (%)
−3.01
−2.67
−1.81
—According to Figures3 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.Figures5 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.Figure 5
Navigation errors ofstandard GPS/INS algorithm with simulating GPS outages (#MTi-G).
(a)
(b)
(c)Figure 6
Navigation errors ofsimplified GPS/INS algorithm with simulated GPS outages (#MTi-G).
(a)
(b)
(c)The statistic summary of the position drift error during GPS outages is given in Table4. 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.Table 4
Statistic values of the position drift error and estimated position standard deviation of standard and simplified GPS/INS algorithms during GPS outages (#MTi-G).
Position drift error (m)
Position STD
Mean
Standard
67.556
—
Simplified
67.306
—
Degradation (%)
−0.37
—
RMS
Standard
81.364
87.289
Simplified
81.108
86.856
Degradation (%)
−0.31
—
Max
Standard
183.590
—
Simplified
183.423
—
Degradation (%)
−0.09
—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.Table5 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.Table 5
Statistic values (RMS) of position drift error and estimated position standard deviation of standard and simplified GPS/INS algorithms during GPS outages (2nd MEMS dataset and a middle grade dataset).
Horizontal position (m)
Position STD
RMS
MTi-G (#2)
Standard
88.053
87.553
Simplified
87.550
83.565
Degradation (%)
−0.57
—
FSAS
Standard
3.540
3.854
Simplified
3.540
3.708
Degradation (%)
0
—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.Table6 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.Table 6
Time consumption comparison of standard and simplified GPS/INS algorithms.
Standard algorithm (s)
Simplified algorithm (s)
Increased efficiency (%)
MTi-G (#1)
108.01
48.91
54.7
MTi-G (#2)
199.24
91.15
54.3
FSAS
395.26
183.86
53.5


## 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 Figure2, 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.Figure 2
Trajectory of the GPS/INS field test.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.Table 2
IMU characteristics after lab calibration.
Measurement
MTi-G
FSAS
Gyros
Bias (°/h)
36
0.75
ARW (°/h)
3
0.1
Scale factor (ppm)
300
300
Accelerometers
Bias (mGal)
2000
1000
VRW (m/s/h)
0.12
0.03
Scale factor (ppm)
300
300It 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 AppendixB 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.Figures3 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.Figure 3
Navigation errors ofstandard GPS/INS algorithm (#MTi-G).
(a)
(b)
(c)Figure 4
Navigation errors ofsimplified GPS/INS algorithm (#MTi-G).
(a)
(b)
(c)To get a more specific comparison, quantitative analysis to the navigation errors was made as shown in Table3. 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 P 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.Table 3
Statistic values of the navigation errors and the estimated standard deviations of the standard and the simplified GPS/INS algorithms (#MTi-G).
Mean
RMS
Max
STD (sqrt ofP matrix)
Position
North (m)
Standard
0.022
0.030
0.186
0.033
Simplified
0.022
0.030
0.188
0.025
Degradation (%)
0.00
0.00
1.08
—
East (m)
Standard
0.020
0.026
0.194
0.031
Simplified
0.020
0.026
0.194
0.023
Degradation (%)
0.00
0.00
0.00
—
Velocity
North (m/s)
Standard
0.014
0.019
0.167
0.026
Simplified
0.014
0.019
0.169
0.019
Degradation (%)
0.00
0.00
1.20
—
East (m/s)
Standard
0.013
0.017
0.086
0.026
Simplified
0.013
0.017
0.086
0.019
Degradation (%)
0.00
0.00
0.00
—
Attitude
Roll (°)
Standard
0.073
0.091
0.433
0.094
Simplified
0.074
0.092
0.436
0.085
Degradation (%)
1.37
1.10
0.69
—
Pitch (°)
Standard
0.105
0.125
0.506
0.093
Simplified
0.105
0.125
0.507
0.084
Degradation (%)
0.00
0.00
0.20
—
Heading (°)
Standard
0.698
0.861
2.596
0.719
Simplified
0.677
0.838
2.549
0.705
Degradation (%)
−3.01
−2.67
−1.81
—According to Figures3 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.Figures5 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.Figure 5
Navigation errors ofstandard GPS/INS algorithm with simulating GPS outages (#MTi-G).
(a)
(b)
(c)Figure 6
Navigation errors ofsimplified GPS/INS algorithm with simulated GPS outages (#MTi-G).
(a)
(b)
(c)The statistic summary of the position drift error during GPS outages is given in Table4. 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.Table 4
Statistic values of the position drift error and estimated position standard deviation of standard and simplified GPS/INS algorithms during GPS outages (#MTi-G).
Position drift error (m)
Position STD
Mean
Standard
67.556
—
Simplified
67.306
—
Degradation (%)
−0.37
—
RMS
Standard
81.364
87.289
Simplified
81.108
86.856
Degradation (%)
−0.31
—
Max
Standard
183.590
—
Simplified
183.423
—
Degradation (%)
−0.09
—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.Table5 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.Table 5
Statistic values (RMS) of position drift error and estimated position standard deviation of standard and simplified GPS/INS algorithms during GPS outages (2nd MEMS dataset and a middle grade dataset).
Horizontal position (m)
Position STD
RMS
MTi-G (#2)
Standard
88.053
87.553
Simplified
87.550
83.565
Degradation (%)
−0.57
—
FSAS
Standard
3.540
3.854
Simplified
3.540
3.708
Degradation (%)
0
—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.Table6 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.Table 6
Time consumption comparison of standard and simplified GPS/INS algorithms.
Standard algorithm (s)
Simplified algorithm (s)
Increased efficiency (%)
MTi-G (#1)
108.01
48.91
54.7
MTi-G (#2)
199.24
91.15
54.3
FSAS
395.26
183.86
53.5


## 4. Conclusions

In this paper, a GNSS/INS navigation algorithm improvement, that is, one-step prediction ofP 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 ofP matrix algorithm to the real applications that request strict real-time response, such as deeply coupled GNSS/INS systems.



---

*Source: 109267-2015-06-28.xml*