- freely available
- re-usable

*Sensors*
**2012**,
*12*(12),
17372-17389;
doi:10.3390/s121217372

^{1}

^{2}

## Abstract

**:**The integration of the Inertial Navigation System (INS) and the Global Positioning System (GPS) is widely applied to seamlessly determine the time-variable position and orientation parameters of a system for navigation and mobile mapping applications. For optimal data fusion, the Kalman filter (KF) is often used for real-time applications. Backward smoothing is considered an optimal post-processing procedure. However, in current INS/GPS integration schemes, the KF and smoothing techniques still have some limitations. This article reviews the principles and analyzes the limitations of these estimators. In addition, an on-line smoothing method that overcomes the limitations of previous algorithms is proposed. For verification, an INS/GPS integrated architecture is implemented using a low-cost micro-electro-mechanical systems inertial measurement unit and a single-frequency GPS receiver. GPS signal outages are included in the testing trajectories to evaluate the effectiveness of the proposed method in comparison to conventional schemes.

## 1. Introduction

For navigation applications and the Mobile Mapping System (MMS), the integration of the Inertial Navigation System (INS) using an Inertial Measurement Unit (IMU) and the Global Positioning System (GPS) is widely applied for determining state vectors, which include the position, velocity, and orientation of the mobile platform. The advantages of INS are autonomous operation, high measurement sampling rate, and short-term accuracy. However, its navigation accuracy degrades rapidly with time if no external aiding source is available. This is particularly true when a low-cost IMU is applied. In contrast, GPS is able to provide long-term position and velocity accurately. However, a low sampling rate, environmental dependence, and the lack in orientation determination with single antenna are the primary limitations for navigation oriented-applications with GPS alone. The integration of INS and GPS is an optimal solution that utilizes the advantages of each system and overcome in limitations.

The Kalman filter (KF) [1] is commonly applied for multi-sensor data fusion. The KF aims to find the optimal estimates of the system states based on the minimization of covariance. There are two main steps in the KF computation cycle. In the first step, the prediction primarily relies on the information of the system output. In the second step, whenever aiding measurements are available, the estimates are updated using this information. However, besides the limitations reported in [2–5], most filtering techniques including KF can only be used for optimal estimation when aiding measurements are available. Otherwise, navigation states rely on predicted results from the INS mechanization. This significantly increases positional drifts in the system when a low-cost micro electro mechanical system (MEMS) IMU is applied in GPS-denied environments. In addition, the system and the measurement noise must be carefully pre-modeled for the filtering process. This procedure is costly and impossible to implement in certain cases [2].

To overcome the limitations of filtering techniques, smoothing algorithms have been effectively applied for integrated navigation systems when post-processing is permitted. In principle, smoothing estimates the states at time k given the measurements at a time greater than k. Most smoothing algorithms utilize forward and backward passes to find the estimates of the states at every epoch of the system output. In the popular Rauch-Tung-Strieble (RTS) smoother [6], the forward estimation is obtained using standard KF and the estimation of the backward pass is based on the maximum likelihood estimates. The main advantages of this algorithm are high reliability and simple implementation. Liu et al.[7] developed Two-Filter Smoothing (TFS) and applied it in INS/GPS integration for post-processing applications. The estimation accuracies of TFS and RTS smoother are comparable. The computational times are similar as well. In comparison to forward KF, the improvement of smoothing in positioning error ranges from 35% to 95% depending on the length of GPS signal outages. Chiang [8] proposed a combination of RTS smoothing and artificial neural networks (ANN) for accurate INS/GPS integrated position and orientation determination. The research illustrated that the improvement of ANN-RTS algorithm compared to RTS is about 70%. However, the extra computational time for ANN-RTS algorithm is significant due to the training process.

In general, the estimation accuracy of smoothing is superior to that of filtering. However, most smoothing techniques have been applied for post-processing applications since the backward process always starts from the end of the forward filtering mission. This limits smoothing in real-time applications. The present study utilizes smoothing to on-line update the states of the system for near-real-time applications.

## 2. Optimal Estimations and Problem Statements

#### 2.1. Kalman Filter

The KF is considered as a special form of Bayesian estimation [9,10], in which the system and measurement models are originally linear or linearized into linear functions as shown:

_{k}∈ R

^{nx}is the state vector at time k, Φ

_{k}

_{−1;}

_{k}is the state transition matrix from epoch k − 1 to k, w

_{k}∈ R

^{nx}is the system noise, z

_{k}∈ R

^{nz}is the aiding measurement, H

_{k}is the measurement mapping matrix, and v

_{k}∈ R

^{nv}is measurement noise.

In the KF, Gaussian distribution is assumed for the system and measurement noise with zero mean and covariances Q_{k} and R_{k}, respectively:

With this assumption, the prior and posterior probability density function (PDF) of state vector given aiding measurements, p(x_{k} | z_{k}_{−1}), p(x_{k} | z_{k}) are normal distribution functions.

_{k}; x̂

_{k|k}, P

_{k|k}) denotes a normal distribution of x

_{k}with mean x̂

_{k|k}and covariance P

_{k|k}.

Derived in terms of the minimum mean square error of the state vector, KF calculation steps are:

Prediction:

Updating:

_{k|k}

_{−1}, P

_{k|k}

_{−1}are the predicted states and covariance at time k given information at time k − 1, x̂

_{k}

_{−1}, P

_{k}

_{−1}are the estimated states and covariance at time k −1, and x̂

_{k}, P

_{k}are the estimated states and covariance at time k.

Although well known as an optimal linear estimator, the KF has some limitations for INS/GPS integration. First, KF can only be applied with linear models and the assumption of Gaussian-distribution noise. However, in INS/GPS integration applications, the system and measurement models are originally non-linear and the noise during operation may be non-Gaussian. This is particular true when a low-cost MEMS IMU is used with highly dynamic movement. These restrictions were investigated in [2–5].

Second, in a generic KF, covariance matrices Q and R, which represent the behavior of the system and measurement noise, respectively, must be carefully pre-modeled. Insufficiently known or wrong a priori statistics about the system and measurement noise result in poor performance or divergence of the filter [11]. To estimate the a priori statistics of noise, intensive calibration or reliable information about the sensors of the system is usually required. This increases the overall cost of the system. This problem is mentioned in [8,11–13].

In addition, since the sampling rate of the INS is higher than that of GPS, the states of the system are updated for the estimates only when GPS measurements are available. Otherwise, the predicted estimates of KF are used. This situation leads to large positional and attitude errors when using a MEMS-based INS/GPS system during long periods of GPS signal outages, as described in [8].

#### 2.2. Rauch-Tung-Striebel Off-Line Smoothing

According to [6], the purpose of smoothing is to estimate the PDF of the states at time k with all given measurements up to and at time N, where k ≤ N:

The RTS smoother [6] applies the maximum likelihood of the state vectors given aiding measurements vectors as the criteria for finding optimal estimates:

_{k}, x

_{k}

_{+1}| z

_{N}) is the likelihood of x

_{k}, x

_{k}

_{+ 1}given z

_{N}

By resolving the criteria in Equation (13), the estimates and covariance of the states are obtained:

_{k|N}, P

_{k|N}are the smoothed states and covariance at time k given information up to N (k ≤ N), x̂

_{k}, P

_{k}are the states and corresponding covariance estimated by KF at time k, C

_{k}is the cross covariance, determined as:

The implementation of the RTS smoother includes two main stages of estimation: the forward direction estimation using standard KF and the backward direction using Equations (14) to (16). First, the prediction is implemented based on the output of the system model. The predicted states x̂^{−} and covariance P^{−} are stored in temporary files. Whenever an aiding measurement is available, the KF is activated. The updated states x̂ and covariance P are calculated accordingly. These parameters are also stored in temporary files for later smoothing. This recursive process continues until the end of the mission, for a forward pass. After the forward pass, the RTS smoothing begins from the end of the mission and moves back to the starting point of the data set. The predicted and updated information are used in this process. This implementation of the RTS smoother has some limitations. First, it is commonly applied for post processing and can be considered as off-line smoothing. Second, it is time-consuming to store and retrieve the predicted and updated information from the stored files. A lot of storage is also required. Third, since the smoothing is activated from the end to the beginning of the mission, the output solution is always in reverse sequence of time. This is inconvenient when the output data is used for other tasks. Figure 1 describes the process and the performance of RTS off-line smoothing and Figure 2 shows the architecture design of INS/GPS integration with off-line smoothing.

## 3. On-Line Smoothing

#### 3.1. Principle of On-Line Smoothing

The proposed on-line smoothing is implemented during operation time. The algorithm overcomes the disadvantages of off-line smoothing. The proposed on-line smoothing is derived from the RTS smoother algorithm. Instead of waiting until the end of the data set, the smoothing process is activated whenever updating measurements are found. In the KF, the execution time for prediction and filtering is shorter than the time of each epoch (the time between two consecutive IMU samples), meaning that in each epoch, the processing unit waits for the incoming data of the next epoch from an IMU after implementing the KF as shown in Figure 3. The on-line smoothing aims to utilize the time remaining in each epoch to implement smoothing to improve the overall accuracy of the system during operation. Figure 4 illustrates the principle of the proposed on-line smoothing. The architecture design of INS/GPS integration with on-line smoothing is shown in Figure 5.

As shown in Figures 4 and 5, during estimation, the KF is applied for forward estimation; the predicted information is temporally stored in dynamic arrays. When the updated measurement from aiding sensors is available, updating is activated. The RTS smoother performs smoothing from the current time to the previous updating time using the latest updated estimates and predicted information in dynamic arrays. After smoothing process in the current epoch has finished, the smoothed solution is stored or given in output streams with the sequence from the previous updating time. The data in the dynamic arrays is replaced by the predicted information of the next epoch. With this implementation, the smoothing process is nearly parallel to the filtering process.

In real-time applications, the time required for on-line smoothing must be considered to keep track of the continuity of the incoming data from the sensor. The time required for on-line smoothing is closely related to the number of smoothing steps. It thus depends on the sampling rate of the system sensor and the length of time between consecutive measurement updates.

#### 3.2. Output Rate of On-Line Smoothing for Real-Time Application

Let d_{s} be the frequency of the system sensor (INS sensor). The time for each epoch during system operating is:

Let t_{f} be the time for each filtering step, T is the time for each smoothing step, d_{out} is output data rate (number of output data packages per second) of on-line smoothing, and n_{up} is number of updating steps during on-line smoothing or window size of on-line smoothing. Time for smoothing, t_{sm} in each on-line smoothing window will be:

To implement on-line smoothing without missing incoming data from the system sensor of the next epoch, the following criteria must be kept:

Substituting Equations (17) and (18) into (19) yields:

From Equation (20), the output data rate in on-line smoothing is determined as:

In practice, the time for each estimation step depend on the configuration of the processor, the applied programming language and the optimization of algorithms. In a test with a program written in C++ language, tested on a Core 2 duo 1.86 GHz CPU, the time for each filtering step is 0.3 ms and for each smoothing step is 0.5 ms. If an IMU with 50 Hz of output rate is applied, the output rate of on-line smoothing will be about 40 Hz. In practical, 25 Hz of output rate and window size n_{up} = 1 is applied for this case.

A discontinuity between estimating epochs may arise since the output rate is not the same as the sampling rare. In this case, the prediction states between discontinuous epochs k − 1 and k + N cannot be calculated based on Equation (7). They are instead calculated as:

## 4. System and Software Design

#### 4.1. Equipment

Two INS/GNSS integrated navigation systems were set up to conduct a field test in this research. The reference system comprised a high-end tactical-grade IMU, SPAN-LCI (NovAtel). A dual-frequency geodetic-grade GNSS receiver, ProPak V3 (NovAtel). A distance measurement instrument (DMI). The specifications of SPAN-LCI IMU are shown in Table 1.

The testing system comprised a low-cost MEMS IMU, MIDGII (Microbotics), with an integrated single-frequency GPS receiver. The specifications of the testing system are shown in Table 2. Both systems were mounted on a mobile mapping van for data collection to validate the performance of the proposed algorithms, as shown in Figure 8.

#### 4.2. Software Design

A tightly coupled (TC) INS/GPS integration scheme with KF-based estimation was implemented in this study. As shown in Figure 9, the input data sets for the integrated system were raw IMU and GPS measurements. The raw measurements provided by IMU, including incremental angles and velocities sensed by the gyroscope and accelerometers, after compensation for system errors, are processed by an INS mechanization for a navigation solution in a local-level frame. Pseudo-range and Doppler signal measurements, the raw measurements from GPS, are firstly pre-processed for eliminating system errors such as clock, troposphere, and ionosphere errors. All the measurements are then blended and estimated with the KF and smoothing for the optimal navigation solution, including position, velocity, and orientation. Compared to a loosely coupled (LC) integration scheme, the TC scheme has major advantage of aiding measurements being supported by GPS even when fewer four GPS satellites are available. For a LC scheme, at least four visible GPS satellites are required to derive GPS-aided solutions. Therefore, the TC scheme is particularly suitable for land-based mobile mapping where the system is often operated in GPS-hostile environments such as urban canyons or under dense canopy.

Based on the designated scheme, software for processing raw measurements from GPS and the IMU was developed in the C++ programming language. The graphical user interface (GUI) design is illustrated in Figure 10. The input for the software includes IMU and GPS raw measurements. The software can also process dual-frequency GPS carrier phase measurements in differential mode for accurate GPS solutions.

## 5. Performance Validation of Proposed Algorithms

In the first test, the testing data sets were collected under various environment scenarios in urban and suburban areas in Kaohsiung, Taiwan. The testing trajectory is shown in Figure 11. The reference trajectory was generated with the reference system with its IMU raw measurements and raw GPS carrier phase measurements processed in differential mode with commercial software, Inertial Explorer (NovAtel), performing sensor fusion in TC smoothing mode with aid from DMI. In general, the kinematic positioning accuracy of the applied reference system was less than 10 centimeters, which is considered sufficient.

For the testing scenario, three algorithms, namely the KF, off-line RTS smoother and, on-line smoothing based on a forward KF and backward RTS, were implemented. The estimated results of the algorithms, including position and orientation, were compared to the reference data for analysis.

Table 3 and Figure 12 illustrate numerical and histogram statistics of positional root mean square errors (RMSE) and Figure 13 illustrates the performance of the three algorithms. The analysis indicates that in general, the estimation accuracy in terms of position for smoothing is superior to that for KF. The improvement in RMSE is about 85% with RTS smoother and 60% with on-line smoothing. The efficiency can be seen clearly when GPS signals are weak or blocked. Overall, the off-line RTS smoother produced the best estimates. During GPS signal outages, the performances of on-line smoothing and conventional RTS smoother were comparable.

For orientation accuracy, the improvement of smoothing techniques is not as large as that for positional estimates. The estimated orientation of the off-line RTS smoother is the best, but only slightly. The performances of on-line smoothing and the KF are similar overall. The only difference is that during GPS outages, the estimated results of on-line smoothing are slightly better than those of KF. Table 4, Figures 14 and 15 show these results.

The processing time of the three algorithms was also analyzed. Table 5 shows the numerical analysis. Figure 16 shows a comparison of time for KF, off-line RTS smoother, and on-line smoothing. The statistic data is surveyed from the processing time for a data set of 1 h on-field operation time, a 50-Hz IMU sampling rate, and processed on a computer with a Core 2 duo 1.86 GHz CPU. The analysis illustrates that although off-line smoothing produced the best estimates, it required the longest processing time. The time increment of off-line RTS smoothing is about 250% compared to that of KF. The time increment of on-line smoothing compared to that for KF is only about 40%, for an improvement in positional accuracy of about 60%. In addition, the estimated output of on-line smoothing is near real-time and the data is in a forward sequence of time, which is convenient for data usage.

The second test was conducted to analyze the relationship between output rates, window size and estimation accuracy in on-line smoothing. The data was collected with the same systems applied in the previous test in Tainan, Taiwan. The testing trajectory is shown on Figure 17. Updating rate from GPS is reduced to 0.05 Hz (20 s for each GPS update). Three cases of online-smoothing output rate with 50 Hz, 25 Hz and 10 Hz are tested, two types of output solutions including smoothed only solutions and mixed solutions (mixes of smoothed and filtered solutions) are considered. The processing time tracked on two versions of software is tested, C++ and Matlab, the operation time for testing was 34 min. Table 6 and Figure 18 show the testing results. Four options of on-line smoothing window size with 50 Hz output are also considered and illustrated in Table 7 and Figure 19.

The second test results indicate that if the smoothed solutions are considered as the output solutions, there are only small variations in estimation accuracy with different smoothing output rates. On the other hand, the accuracy of mixed solutions decreases dramatically if output rates decrease. The processing time changes proportionally with the change of smoothing output rates. On the other hand, the estimation accuracy improves when the smoothing window size increases. However, long processing time causes the latency in the case of large window size. Table 7 and Figure 19 show that the improvement in terms of positioning accuracy with different window size is not significant while the processing time increases dramatically. Tables 6 and 7 also illustrate that by improving the algorithms and programming environment, the processing time decreases significantly.

## 6. Conclusions

This study developed an on-line smoothing algorithm to improve the estimation accuracy of positional and orientation parameters of integrated navigation systems utilizing low-cost MEMS inertial sensor in near-real-time.

The results indicate that the proposed on-line smoothing outperforms the KF. In terms of positional RMSE, the improvement of on-line smoothing is about 60% compared to KF. The performance of on-line smoothing is comparable to that of off-line smoothing.

The estimation accuracy and processing time in on-line smoothing depend on the output data rate and window size accordingly. The optimal parameters for on-line smoothing are application dependent. For the future works, parallel computing technique should be investigated and applied on on-line smoothing to improve both output rate and estimation accuracy.

## Acknowledgments

The authors acknowledge the financial support by Taiwan’s Industrial Technology Research Institute and the Research & Development Foundation of National Cheng Kung University.

## References

- Kalman, R.E. A new research approach to Linear Filtering and Prediction Problem. J. Basic Eng
**1960**, 82, 35–45. [Google Scholar] - Chiang, K.W.; Noureldin, A.; El-Sheimy, N. Multi-sensors integration using neuron computing for land vehicle navigation. GPS Solut
**2003**, 6, 209–218. [Google Scholar] - Julier, S.J.; Uhlmann, J.K. Unscented Filtering and Nonlinear Estimation. Proc. IEEE
**2004**, 92, 401–422. [Google Scholar] - Duong, T.T.; Chiang, K.W. Non-Linear, Non-Gaussian estimation for INS/GPS integration. Proceedings of the 24th International Technical Meeting of the Satellite Division of the Institute of Navigation (ION GNSS), Portland, OR, USA, 19–23 September 2011; pp. 946–954.
- Zhou, J.; Knedlik, S.; Loffeld, O. INS/GPS Tightly-coupled Integration using Adaptive Unscented Particle Filter. J. Navig
**2010**, 63, 491–511. [Google Scholar] - Rauch, H.; Tung, F.; Striebel, C. Maximum likelihood estimates of linear dynamic systems. AIAA J
**1965**, 3, 1445–1450. [Google Scholar] - Liu, H.; Nassar, S.; El-Sheimy, N. Two-Filter smoothing for accurate INS/GPS Land-Vehicle Navigation in Urban Center. IEEE Trans. Veh. Technol
**2010**, 59, 4256–4276. [Google Scholar] - Chiang, K.W.; Chang, H.W.; Li, C.Y.; Huang, Y.W. An Artificial Neural Network Embedded Position and Orientation Determination Algorithm for Low Cost MEMS INS/GPS Integrated Sensors. Sensors
**2009**, 9, 2586–2610. [Google Scholar] - Jekeli, C. Inertial Navigation System with Geodetic Application; De Gruyter: Berlin, Germany, New York, NY, USA, 2001. [Google Scholar]
- John, L.C.; John, L.J. Optimal Estimation of Dynamic System; Chapman&HALL/CRC: Washington, DC, USA, 2004. [Google Scholar]
- Mohamed, A.H.; Schwarz, K.P. Adaptive Kalman Filtering for INS/GPS integration. J. Geod
**1997**, 73, 193–203. [Google Scholar] - Almagbile, A.; Wang, J.; Ding, W. Evaluating the performance of adaptive Kalman filter method in GPS/INS integration. J. Glob. Position. Syst
**2010**, 9, 33–40. [Google Scholar] - Mayers, K.A.; Tapley, B.D. Apdatetive Sequential Estimation with Unknown Noise Statistic. IEEE Trans. Autom. Control
**1976**, 21, 520–523. [Google Scholar]

**Figure 3.**Proportion of time used for filtering in integrated navigation system. t

_{e}is the time of each epoch k, and t

_{f}is time required for filtering.

Physical characteristics | IMU performance | Measurement accuracy | ||
---|---|---|---|---|

Output rate (Hz) | 200 | Heading (degree) | 0.02 | |

Gyro bias (degree/h) | 1.0 | Attitude (degree) | 0.4 | |

Gyro scale factor (ppm) | 100 | Position (m) | 0.1–1.5 | |

Accelerometer bias (mg) | 1.0 | Velocity (m/s) | 0.02 | |

Accelerometer scale factor (ppm) | 250 | Altitude (m) | 0.08 |

Physical characteristics | IMU performance | Measurement accuracy | ||
---|---|---|---|---|

Output rate (Hz) | 50 | Heading (degree) | 2 | |

Gyro bias (degree/h) | 4.7 | Attitude (degree) | 0.4 | |

Gyro scale factor (ppm) | 5,000 | Position (m) | 2–3 | |

Accelerometer bias (mg) | 6.0 | Velocity (m/s) | 0.2 | |

Accelerometer scale factor (ppm) | 19,700 | Altitude (m) | 3–5 |

Method | RMSE (m) | Improvement (%) | |||
---|---|---|---|---|---|

East | North | Up | 3D | ||

KF | 17.423 | 3.437 | 2.462 | 17.93 | |

Off-line smoothing | 2.449 | 0.688 | 0.946 | 2.71 | 85 |

On-line smoothing | 7.196 | 2.511 | 2.063 | 7.9 | 56 |

Method | RMSE (degree) | ||
---|---|---|---|

Roll | Pitch | Heading | |

KF | 0.71 | 0.77 | 1.90 |

Off-line smoothing | 0.25 | 0.34 | 1.21 |

On-line smoothing | 0.68 | 0.73 | 1.82 |

Method | Processing time (s) | Increment (%) |
---|---|---|

KF | 75 | |

On-line smoothing | 105 | 40 |

Off-line smoothing | 265 | 253 |

Smoothing rate (Hz) | Positional error (m) | Processing time (s) | ||
---|---|---|---|---|

Smoothed solution | Mixed solution | Matlab version | C++ version | |

10 | 4.64 | 10.51 | 240 | 29 |

25 | 4.42 | 7.22 | 270 | 33 |

50 | 4.33 | 4.33 | 360 | 42 |

KF | 16.19 | 16.19 | 215 | 20 |

RTS smoother | 1.81 | 1.81 | 420 | 50 |

Smoothing window size | Positional error (m) | Processing time (s) | |
---|---|---|---|

Matlab version | C++ version | ||

1 | 4.33 | 360 | 42 |

3 | 3.80 | 430 | 52.5 |

5 | 3.35 | 490 | 63.7 |

7 | 3.17 | 700 | 91 |

KF | 16.19 | 215 | 20 |

RTS smoother | 1.81 | 420 | 50 |

© 2012 by the authors; licensee MDPI, Basel, Switzerland This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).