Next Article in Journal
Quality Evaluation of Agricultural Distillates Using an Electronic Nose
Previous Article in Journal
GPR-Based Water Leak Models in Water Distribution Systems

Sensors 2013, 13(12), 15937-15953; doi:10.3390/s131215937

Article
Autonomous Integrated Navigation for Indoor Robots Utilizing On-Line Iterated Extended Rauch-Tung-Striebel Smoothing
Yuan Xu , Xiyuan Chen * and Qinghua Li
Key Laboratory of Micro-Inertial Instrument and Advanced Navigation Technology, Ministry of Education, School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China; E-Mails: xy_abric@126.com (Y.X.); liqinghua1977@163.com (Q.L.)
*
Author to whom correspondence should be addressed; E-Mail: chxiyuan@seu.edu.cn; Tel./Fax: +86-25-8379-2010.
Received: 24 September 2013; in revised form: 7 November 2013 / Accepted: 11 November 2013 /
Published: 25 November 2013

Abstract

: In order to reduce the estimated errors of the inertial navigation system (INS)/Wireless sensor network (WSN)-integrated navigation for mobile robots indoors, this work proposes an on-line iterated extended Rauch-Tung-Striebel smoothing (IERTSS) utilizing inertial measuring units (IMUs) and an ultrasonic positioning system. In this mode, an iterated Extended Kalman filter (IEKF) is used in forward data processing of the Extended Rauch-Tung-Striebel smoothing (ERTSS) to improve the accuracy of the filtering output for the smoother. Furthermore, in order to achieve the on-line smoothing, IERTSS is embedded into the average filter. For verification, a real indoor test has been done to assess the performance of the proposed method. The results show that the proposed method is effective in reducing the errors compared with the conventional schemes.
Keywords:
inertial navigation systems (INS); integrated navigation; iterated extended Kalman filter (IEKF); extended Rauch-Tung-Striebel smoother (ERTSS); average filtering

1. Introduction

Autonomous mobile robots have increasingly been used in a wide range of applications [1]. One key issue for mobile robots is the ability to obtain their navigation information (such as position, velocity and so on). In order to obtain accurate navigation information indoors, a number of methods for localization with various sensors and different degrees of precision were proposed over the past few decades [13].

The main research approaches for indoor localization include beacon-based solutions and beacon-free solutions [4]. Beacon-based solutions employ reference nodes (RNs) with known location to complete the localization of blind nodes (BNs). Its principle is similar to that of global positioning systems (GPS), but the communication technology used is short-range radio, such as WiFi, UWB, RFID, ZigBee, ultrasound, etc. Many researchers have attempted to employ beacon-based solutions for indoor localization. For instance, Shirehjini et al. proposed an RFID-based position and orientation measurement system for mobile objects in [5]; Park et al. employed ZigBee for an indoor location system [6]. Most of the abovementioned attempts employ the measurement of one or several physical parameters of the radio signal transmitted between the RNs and BNs to complete the wireless localization. Due to the influence of the building structures in indoor environments, the accuracy is about one meter. In order to obtain higher accuracy, some researchers employ ultrasonic waves, and the time of arrival (TOA) mode to complete the distance measurement. For instance, Minami et al. proposed a fully distributed localization system based on ultrasound, where the localization accuracy was about 20 cm with 24 devices [7], and Saad et al. proposed high-accuracy reference-free ultrasonic location estimation in [2]. Ultrasonic sensors have been shown to be a simple but powerful system for this mode, however, it needs has a high density of RNs to maintain the localization accuracy, which is not practical for large localization areas. Differing from beacon-based solutions relying on RNs, beacon-free ones are a self-contained system capable of providing positioning information independently [8]. Some attempts using inertial navigation system (INS)-based beacon-free solutions have been used in integrated outdoor navigation [9]. For example, a GPS/INS navigation system for launchers and re-entry vehicles was described by Boulade et al. in [10], and Xu et al. proposed a novel hybrid of least squares support vector machine (LS-SVM) and Kalman filter for GPS/INS integration in [11]. Like the integration navigation mentioned above, several approaches also employ INS-based beacon-free solutions for indoor navigation. For example, Ruiz et al. employed inertial measuring units (IMUs)/radio frequency identification (RFID) integrated navigation for pedestrian indoor navigation in [4]. Evennou et al. proposed a WiFi/INS integration navigation system for indoor mobile positioning in [12]. However, it should be pointed out that beacon-free solutions are poor in long-term self-contained navigation since the accuracy deteriorates with time [13,14], thus it is just a short-term compensation and therefore, it is not suitable for the precision control of indoor mobile robots.

In the data fusion for the integrated navigation system, the integration filter plays an important role in the navigation accuracy. One of the most popular information fusion algorithms is the Kalman filter (KF). However, its optimality heavily depends on linearity [15]. In order to overcome this problem, the extended KF (EKF) is used [14], but the linearization of a nonlinear system by Taylor series expansion and neglection of the truncated high-order terms will introduce a truncation error [15]. Then, the unscented KF (UKF) and iterated Extended Kalman filter (IEKF) are proposed. Although the UKF is able to overcome the shortcomings of the EKF, it needs more time to compute large numbers of samples [16]. The IEKF is able to reduce the bias and the estimation errors by adding only a few simple iterative operations. In order to obtain high accuracy, smoothing algorithms have been effectively applied for integrated navigation systems [17]. The Rauch-Tung-Striebel smoothing (RTSS) is one of the most popular methods [18]. RTSS was first proposed in [19], and it includes one forward data processing and one backward data processing step. Due to its robustness and effectiveness, it is widely used in navigation applications. However, it is only suitable for linear systems. In order to overcome this problem, some researchers employ the so-called Extended RTSS (ERTSS). In this mode, the forward data processing of ERTSS is implemented by EKF instead of KF, such as in [20]. In order to reduce the estimated error of the INS/WSN navigation indoors for mobile robots, this work proposes the design and implementation of an on-line iterated ERTSS (IERTSS). The IEKF is used to improve the filtering output accuracy of the ERTSS algorithm. Then, in order to achieve the on-line smoothing, the IERTSS is used in an average filter to smooth the errors of the INS during the output period. A real indoor test is used to evaluate the performance of the proposed method. The remainder of the paper is organized as follows: Section 2 gives the IERTSS embedded average filter design. The unbiased tightly-coupled integrated model for mobile robot navigation indoors is illustrated in Section 3. Section 4 gives the real indoor tests and performance. Finally, conclusions are given in Section 5.

2. On-Line Iterated Extended Rauch-Tung-Striebel Smoothing

In this section, a brief introduction to the IEKF and ERTSS will be given, and then, an on-line IERTSS will be proposed.

2.1. Iterated Extended Kalman Filter

It is assumed that a discrete-time model of a nonlinear system is given by Equations (1) and (2):

X k = f ( X k - 1 ) + B k - 1 ω k - 1
y k = h ( X k ) + υ k
where Xk is the state vector at time k, f(Xk) is the system nonlinear function, Bk is the process noise input matrix, yk is the observation vector, and h(Xk) is the observation nonlinear function. ωk is the process noise, and υk is the observation noise. It is assumed that ωk and υk are independent zero-mean white Gaussian sequences with covariance Q and R, respectively. The IEKF used in this paper involves the following recursive relations [17,19]:
X ^ k k - 1 = A k - 1 X ^ k - 1 k - 1
P k k - 1 = A k - 1 P k - 1 A k - 1 T + Q
where A k = f ( X ^ k k ) X ^ k k. Compared with the EKF, the IEKF employs a few simple iterative operations to reduce the bias and the estimation error after getting X̂k|k-1 in Equation (3) and Pk|k-1 in Equation (4). The corresponding recursive relations are:
X ^ k k 1 = X ^ k k - 1
P k k 1 = P k k - 1
K k n = P k k - 1 H n ( X ^ k k n ) T [ H n ( X ^ k k n ) P k k - 1 H n ( X ^ k k n ) T + R ] - 1
X ^ k k n + 1 = X ^ k k n + K k n [ y k - h n ( X ^ k k n ) - H n ( X ^ k k n ) × ( X ^ k k - 1 - X ^ k k n ) ]
P k k n = [ I - K k n H n ( X ^ k k n ) ] P k k - 1 [ I - K k n H n ( X ^ k k n ) ] T + K k n R ( K k n ) T
here, H n ( X ^ k k n ) = h ( X ^ k k n ) X ^ k k n, n is the number of iteration and n=1,2,…,j. Then:
X ^ k k = X ^ k k j
P k k = P k k j

2.2. Extended Rauch-Tung-Striebel Smoothing

Consider the nonlinear system given by Equations (1) and (2), the forward data processing of ERTSS is utilizing a set of equations as follows:

X ^ k k - 1 = A k - 1 X ^ k - 1 k - 1
P k k - 1 = A k - 1 P k - 1 A k - 1 T + Q
K k = P k k - 1 H k T [ H k P k k - 1 H k T + R ] - 1
X ^ k k = X ^ k k - 1 + K k [ z k - h ( X ^ k k - 1 ) ]
P k k = [ I - K k H k ] P k k - 1
where A k = f ( X ^ k k ) X ^ k k, H k = h ( X ^ k k ) X ^ k k. The backward data processing propagates the filtering outputs and achieves the smoothing results by using the R-T-S formulation. It is computed with the following equations:
K k S = P k k A k T ( P k + 1 k ) - 1
X ^ k k S = X ^ k k + K k S [ X ^ k + 1 k + 1 S - X ^ k k - 1 ]
P k S = P k k + K k S ( P k + 1 S - P k k - 1 ) ( K k S ) T
where the superscript S denotes the smoothing, and the recursion [Equations (17)(19)] is started from the filtering output at the final time.

2.3. On-Line Iterated Extended Rauch-Tung-Striebel Smoothing

As mentioned above, it can be seen that the filtering output accuracy of ERTSS is dependent on the EKF, however, the EKF will generate truncated errors since it employs Taylor series expansion to linearize the nonlinear system. In this work, in order to obtain a higher accuracy solution, the IEKF mentioned above is used in the forward data processing part of ERTSS (called IERTSS). Moreover, in order to achieve on-line smoothing, this work proposes an on-line IERTSS. The flow chart of this on-line IERTSS is shown in Figure 1. In this mode, the IEKF is used for optimal state estimation. When the output periods are coming, firstly, the IERTSS is employed to smooth the filtering output of IEKF between two data output periods. Then, the average value of the INS state estimation is computed with the INS solution and IERTSS solution.

3. Unbiased Tightly-Coupled Integrated Model for Mobile Robot Navigation Indoors

The unbiased tightly-coupled integrated model proposed in [21] is employed in this work. The continuous-time state equation of the filter is illustrated in Equation (20):

[ δ P ˙ E , k δ V ˙ E , k δ A c ˙ c E , k δ P ˙ N , k δ V ˙ N , k δ A c ˙ c N , k ] X ˙ k = [ 0 1 T 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 T 0 0 0 0 0 1 0 0 0 0 0 0 ] A * [ δ P E , k - 1 δ V E , k - 1 δ A c c E , k - 1 δ P N , k - 1 δ V N , k - 1 δ A c c N , k - 1 ] X k - 1 + W k *
where (δPE,K, δPN,K), (δVE,K, δVN,K) and (δAccE,K, δAccN,K) are the errors of position, velocity and acceleration measured by the INS in east and north direction at moment k. T is the sample time; W k *is the Gaussian process noise. Equation (20) can be transferred into a discrete-time state equation:
[ δ P E , k δ V E , k δ A c c E , k δ P N , k δ V N , k δ A c c N , k ] X k = [ 1 T T 2 / 2 0 0 0 0 1 T 0 0 0 0 0 1 0 0 0 0 0 0 1 T T 2 / 2 0 0 0 0 1 T 0 0 0 0 0 1 ] A [ δ P E , k - 1 δ V E , k - 1 δ A c c E , k - 1 δ P N , k - 1 δ V N , k - 1 δ A c c N , k - 1 ] X k - 1 + W k
where Wk is the Gaussian process noise. Here, the position of the robot measured by the INS is denoted as ( P E INS , P N INS ), and the position of the RN is denoted as(x,y). Thus, the distance between the robot and the RN measured by the INS can be expressed as Equation (22):
d i INS = ( P E INS - x i ) 2 + ( P N INS - y i ) 2 , i = 1 , 2 , , m
where m is the number of the RN. Theoretically, the real distance between the robot and the RN is expressed as Equation (23):
d i Real = ( P E Re a l - x i ) 2 + ( P N Re a l - y i ) 2 , i = 1 , 2 , , m
where d i Real is the real distance between the robot and the RN, ( P E Re a l , P N Re a l ) is the real position of the robot. The difference between ( d i INS ) 2 and ( d i Re a l ) 2 is denoted as Δ d i 2, and it is expressed as Equation (24):
Δ d i 2 = ( d i INS ) 2 - ( d i Re a l ) 2 = ( P E INS - x i ) 2 + ( P N INS - y i ) 2 - [ ( P E Re a l - x i ) 2 + ( P N Re a l - y i ) 2 ] , i = 1 , 2 , , m

The real robot position can be computed by Equation (25):

P E Re a l = P E INS - δ P E , P N Re a l = P N INS - δ P N

Thus, the Equation (24) can be transferred to Equation (26):

h d i ( δ P E , δ P N ) = Δ d i 2 = 2 ( P E INS - x i ) δ P E + 2 ( P N INS - y i ) δ P N - ( δ P E 2 + δ P N 2 ) , i = 1 , 2 , , m

The final matrix of the measurement equation at k moment is illustrated in Equation (27):

[ Δ V E , k Δ V N , k Δ d 1 , k 2 Δ d 2 , k 2 Δ d m , k 2 ] y k = [ δ V E , k δ V N , k h d 1 ( δ P E , k , δ P N , k ) h d 2 ( δ P E , k , δ P N , k ) h d m ( δ P E , k , δ P N , k ) ] h ( X k ) + υ k
where υk is the Gaussian process noise,(ΔVE, ΔVN) are the differences between INS velocity and WSN velocity in the east and north direction respectively. It is assumed that ωk and υk are independent zero-mean white Gaussian sequences with covariance Q and R, respectively.

The configuration of the data fusion for the integrated navigation in this work is shown in Figure 2.

4. Indoor localization Tests and Performance

4.1. Real Indoor Test Environment

In this work, two real indoor tests were done to assess the performance of the proposed method. The testbed composes of one robot and six RNs. The prototype of the robot used in this work is shown in Figure 3. The robot is composed of an IMU, an ultrasonic sender and a wireless communication board. Its size is 380 mm × 380 mm × 400 mm (length × width × height). The performance characteristics of the IMU used in this work are listed in Table 1. The robot is the carrier of the IMU and the ultrasonic sender. It is able to collect the IMU datum and the distances between the robot and the RNs by using the PC fixed on the robot. Figure 4 shows the implemented prototype of the RN. Its size is 120 mm × 60 mm × 80 mm (length × width × height). Here, the RN is used to receive the ultrasonic ranging signal sent by the ultrasonic sender and the distance between the RNs and robot can be calculated. It is also able to send the sensor data to the ultrasonic sender when it gets the command. The real indoor test environment is shown in Figure 5, and the positions of the RNs are also marked in Figure 5.

Figure 6 displays the trajectory of the real test. The robot runs from the beginning point (denoted by a black square) to the end point (denoted by a black circle) at a speed of 0.33 m/s. Meanwhile, the RNs are denoted by yellow circles in Figure 6.

4.2. Algorithm Implementation

The pseudocode of the main program is presented in Table 2. In the pseudocode, the percentage symbol, “%,” is used to mark the comments. The software methodology is implemented in the Matlab programming language. The data refresh rate of the netbook computer is 50 Hz. Sensor data can be stored at the end of each test for subsequent analysis.

4.3. The Performance of the Off-Line IERTSS

In this section, the experimental results when the off-line IERTSS works are discussed. In Figure 7, the position errors for INS-only and WSN in the east direction and north direction are shown in Figure 7a,c, respectively. The position errors for the WSN, EKF, ERTSS and off-line IERTSS in the east direction and north direction are shown in Figure 7b,d, respectively. Furthermore, the root mean square errors (RMSE) of position and velocity for the INS-only, WSN, EKF, ERTSS and off-line IERTSS are shown in Tables 3 and 4, respectively.

Figure 7a shows the east position error of the INS-only and WSN. From the figure, one can easily see that though the INS-only solution is continuous, and the east position INS error is accumulated since the DR-based current position has to depend on the previous moment. In Figure 7a, the east position INS error increases to about 190 m in 65 s without any correction. The RMSE of the INS-only solution is 85.89 m. Thus, it is necessary to correct the INS solution. Moreover, we can see that the WSN is able to maintain the east position error compared with the INS-only solution. Table 3 shows that its RMSE is 11.78 cm, which is lower than the INS solution. The east position errors for the WSN, EKF, ERTSS and off-line IERTSS are shown in Figure 7b. From the figure, it can be seen that the estimation accuracy in terms of east position for EKF is superior to that for WSN. The EKF reduces the RMSE in the east direction by about 41.17% compared with the WSN solution. Regarding the smoothing methods, it is evident that both the ERTSS and the off-line IERTSS are effective to reduce the RMSE. Table 3 shows that the position RMSE for the off-line IERTSS is lower than that for ERTSS. The off-line IERTSS reduces the position RMSE by about 38.22% compared with ERTSS.

The north position errors for the INS-only, WSN, EKF, ERTSS and off-line IERTSS are shown in Figure 7c,d, respectively. The trend in this figure is similar to that in Figure 7a,b. The north position error of INS is also accumulated. The WSN solution is also able to maintain the accuracy of the position. From Table 3, we can see that the RMSE of the north position for WSN remains at about 7.71 cm since the WSN solution just depends on the current measurement. Like Figure 7b, the off-line IERTSS solution also has the lowest error. The RMSE of the north position for the off-line IERTSS is 3.46 cm. The improvement in RMSE is about 43.37% and 37.66% compared with the EKF and ERTSS, respectively. In summary, the off-line IERTSS is the most effective method to reduce the position error compared with the INS-only, WSN, EKF and ERTSS.

The RMSE results of velocity for the INS-only, WSN, EKF, ERTSS and off-line IERTSS are shown in Table 4. From the table, it can be seen that the off-line smoothing-based methods are able to effectively reduce the velocity error of the filter. However, the ERTSS and off-line IERTSS solution are almost the same both in the east and north direction, respectively.

4.4. The Relationship between Smoothing Window Size and Accuracy for the On-Line IERTSS

In this section, the relationship between smoothing window size and accuracy for the on-line IERTSS is discussed. The relation between the smoothing window size and the filtering period is shown in Figure 8. From the figure, we can see that the relation can be expressed by the following equation:

( Smoothing window size ) = n ( Filtering period ) = n T

Tables 5 and 6 and Figure 9 show the testing results. From the results, we can see that both the RMSE of position and that of velocity reduce as the smoothing window size increases on the beginning, then the RMSE increases rapidly.

From Table 5, we can see that RMSE of position for the on-line IERTSS is lowest when the smoothing window size is 6, so for the velocity, the solution is 6. Table 5 displays the RMSE of position in the east and north direction with the different smoothing window sizes, while, the average position RMSE in the east and north direction is also shown in the last line. The position RMSE in the east and north direction with the different smoothing window size is shown in Table 6, and the average position RMSE in the east and north direction is also shown in the last line. In order to obtain a high accuracy navigation solution, we take the average of the average RMSE for position and velocity, and the result is shown in Table 7. From that table, we can see when the smoothing window size is 6, the average value is lowest.

4.5. Comparison of On-Line and Off-Line IERTSS

In this section, the comparison of on-line and off-line IERTSS is discussed. The position errors for the off-line and on-line IERTSS are shown in Figure 10. The comparison of on-line and off-line mode in terms of position error is shown in Table 8. From the figures, one can see that both in the east direction and north direction, the position error of the on-line and the off-line IERTSS solutions are almost the same, and the off-line mode is a little better than the on-line mode. Table 9 displays the comparison of on-line and off-line mode in terms of velocity error. Like the position error, the on-line and the off-line IERTSS solutions are also almost the same, and the off-line mode is better than the on-line mode. However, the performance of on-line IERTSS is little better than the ERTSS.

5. Conclusions

This work proposed an on-line IERTSS for tightly integrated INS/WSN mobile robot navigation indoors. In this mode, IEKF is employed instead of the EKF in forward data processing of the ERTSS. Then, IERTSS is embedded into average filter for on-line smoothing. The experimental results show that the proposed on-line smoothing outperforms the ERTSS. The performance of on-line smoothing is also comparable to that of off-line smoothing. The results show that the performance of the on-line and off-line mode is almost the same, and the off-line mode is a little better than the on-line mode.

This work was supported in part by National Natural Science Foundation of China (No. 51375087, 41204025, 50975049), Ocean Special Funds for Scientific Research on Public Causes (No. 201205035-09), Specialized Research Fund for the Doctoral Program of Higher Education (No. 20110092110039), the 52th China Postdoctoral Science Foundation (No. 2012M520967), and the Program Sponsored for Scientific Innovation Research of College Graduate in Jiangsu Province, China (No.CXLX_0101).

Conflicts of Interest

The authors declare no conflict of interest.

References

  1. Kim, S.J.; Kim, B.K. Accurate hybrid global self-localization algorithm for indoor mobile robots with two-dimensional isotropic ultrasonic receivers. IEEE Trans. Instrum. Meas. 2011, 10, 3391–3404. [Google Scholar]
  2. Saad, M.M.; Bleakley, C.J.; Ballal, T.; Dobson, S. High-accuracy reference-free ultrasonic location estimation. IEEE Trans. Instrum. Meas. 2012, 6, 1561–1570. [Google Scholar]
  3. Zhang, J.; Song, G.-M.; Qiao, G.-F.; Meng, T.-H.; Sun, H.-T. An indoor security system with a jumping robot as the surveillance terminal. IEEE Trans. Consum. Electron. 2011, 4, 1774–1781. [Google Scholar]
  4. Ruiz, A.R.J.; Granja, F.S.; Honorato, J.C.P.; Rosas, J.I.G. Accurate pedestrian indoor navigation by tightly coupling foot-mounted IMU and RFID measurements. IEEE Trans. Instrum. Meas. 2012, 1, 178–189. [Google Scholar]
  5. Shirehjini, A.A.N.; Yassine, A.; Shirmohammadi, S. An RFID-based position and orientation measurement system for mobile objects in Intelligent Environments. IEEE Trans. Instrum. Meas. 2012, 6, 1664–1675. [Google Scholar]
  6. Park, W.-C.; Yoon, M.-H. The Implementation of Indoor Location System to Control ZigBee Home Network. Proceedings of the 2006 SICE-ICASE International Joint Conference, Busan, Korea, 18–21 October 2006.
  7. Minami, M.; Morikawa, H.; Aoyama, T. Design and implementation of a fully distributed ultrasonic positioning system. Electron. Commun. Jpn. Part III Fundam. Electron. Sci. 2007, 6, 17–26. [Google Scholar]
  8. Fang, L.; Antsaklis, P.; Montestruque, L.; McMickell, M.; Lemmon, M.; Sun, Y.; Fang, H.; Koutroulis, I.; Haenggi, M.; Xie, M.; et al. Design of a wireless assisted pedestrian dead reckoning system—The NavMote experience. IEEE Trans. Instrum. Meas. 2005, 6, 2342–2358. [Google Scholar]
  9. Feng, G.-H.; Wu, W.-Q.; Wang, G.-L. Observability analysis of a matrix Kalman filter-based navigation system using visual/inertial/magnetic sensors. Sensors 2012, 12, 8877–8894. [Google Scholar]
  10. Boulade, S.; Frapard, B.; Biard, A. GPS/INS Navigation System for Launchers and Re-Entry Vehicles-Developments and Adaptation to Ariane 5. Proceedings of the 5th ESA International Conference on Spacecraft Guidance, Navigation and Control Systems, Frascati, Italy, 22–25 October 2002.
  11. Xu, Z.-K.; Li, Y.; Rizos, C.; Xu, X.-S. Novel hybrid of LS-SVM and Kalman Filter for GPS/INS integration. In J. Navig.; 2010; Volume 2, pp. 289–299. [Google Scholar]
  12. Evennou, F.; Marx, F. Advanced integration of WiFi and Inertial Navigation Systems for indoor mobile positioning. EURASIP J. Appl. Signal Process 2006, 2006, 1–11. [Google Scholar]
  13. Lobo, J.; Lucas, P.; Dias, J.; Traca de Almeida, A. Inertial Navigation System for Mobile Land Vehicles. Proceedings of the IEEE International Symposium on Industrial Electronics ISIE'95, Athens, Greece, 10–14 July 1995.
  14. El-Sheimy, N.; Schwarz, K.P. Integrating Differential GPS Receivers with an Inertial Navigation System (INS) and CCD Cameras for a Mobile GIS Data Collection System. Proceedings of the International Society for Photogrammetry and Remote Sensing Conference, Ottawa, Canada, 6–10 June 1994.
  15. Fang, J.-C.; Gong, X.-L. Predictive iterated Kalman filter for INS/GPS integration and its application to SAR motion compensation. IEEE Trans. Instrum. Meas. 2010, 4, 909–915. [Google Scholar]
  16. Shademan, A.; Sharifi, F.J. Sensitivity Analysis of EKF and Iterated EKF Pose Estimation for Position-Based Visual Servoing. Proceedings of the IEEE Conference Control Application, Toronto, Canada, 28–31 August 2005.
  17. Lefebvre, T.; Bruyninckx, H.; Schutter, J.D. Kalman filters for nonlinear systems: A comparison of performance. Int. J. Control 2004, 7, 639–653. [Google Scholar]
  18. Chiang, K.-W.; Duong, T.T.; Liao, J.-K.; Lai, Y.-C.; Chang, C.-C.; Cai, J.-M.; Huang, S.-C. On-line smoothing for an integrated navigation system with low-cost MEMS inertial sensors. Sensors 2012, 12, 17372–17389. [Google Scholar]
  19. Rauch, H.; Tung, F.; Striebel, C. Maximum likelihood estimates of linear dynamic systems. AIAA J. 1965, 8, 1445–1450. [Google Scholar]
  20. Gong, X.-L.; Zang, R.; Fang, J.-C. Application of unscented R-T-S smoothing on INS/GPS integration system post processing for airborne earth observation. In Measurement; 2013; Volume 3, pp. 1074–1083. [Google Scholar]
  21. Xu, Y.; Chen, X.-Y.; Li, Q.-H. Unbiased tightly-coupled INS/WSN integrated navigation based on extended Kalman filter. J. Chin. Inert. Technol. 2012, 3, 292–299. [Google Scholar]
Sensors 13 15937f1 1024
Figure 1. The flow chart of the on-line IERTSS.

Click here to enlarge figure

Figure 1. The flow chart of the on-line IERTSS.
Sensors 13 15937f1 1024
Sensors 13 15937f2 1024
Figure 2. The configuration of the data fusion for the integrated navigation.

Click here to enlarge figure

Figure 2. The configuration of the data fusion for the integrated navigation.
Sensors 13 15937f2 1024
Sensors 13 15937f3 1024
Figure 3. The prototype of the robot.

Click here to enlarge figure

Figure 3. The prototype of the robot.
Sensors 13 15937f3 1024
Sensors 13 15937f4 1024
Figure 4. The prototype of the RN.

Click here to enlarge figure

Figure 4. The prototype of the RN.
Sensors 13 15937f4 1024
Sensors 13 15937f5 1024
Figure 5. Real indoor test environment.

Click here to enlarge figure

Figure 5. Real indoor test environment.
Sensors 13 15937f5 1024
Sensors 13 15937f6 1024
Figure 6. The trajectory of the real test.

Click here to enlarge figure

Figure 6. The trajectory of the real test.
Sensors 13 15937f6 1024
Sensors 13 15937f7 1024
Figure 7. The position error for the INS-only, WSN, EKF, ERTSS and off-line IERTSS. (a) and (b) East direction; (c) and (d) North direction.

Click here to enlarge figure

Figure 7. The position error for the INS-only, WSN, EKF, ERTSS and off-line IERTSS. (a) and (b) East direction; (c) and (d) North direction.
Sensors 13 15937f7 1024
Sensors 13 15937f8 1024
Figure 8. The relation between the smoothing window size and the filtering period.

Click here to enlarge figure

Figure 8. The relation between the smoothing window size and the filtering period.
Sensors 13 15937f8 1024
Sensors 13 15937f9 1024
Figure 9. Relationship between smoothing window size and RMSE. (a) Position and (b) Velocity.

Click here to enlarge figure

Figure 9. Relationship between smoothing window size and RMSE. (a) Position and (b) Velocity.
Sensors 13 15937f9 1024
Sensors 13 15937f10 1024
Figure 10. The position error for the off-line IERTSS and on-line IERTSS. (a) East direction and (b) North direction.

Click here to enlarge figure

Figure 10. The position error for the off-line IERTSS and on-line IERTSS. (a) East direction and (b) North direction.
Sensors 13 15937f10 1024
Table Table 1. The performance parameters for the IMU used in this work.

Click here to display table

Table 1. The performance parameters for the IMU used in this work.
ParametersDescriptionData
Angular RateInput Range: Yaw, Pitch, Roll±300 deg/s
Bias0.02 deg/s RMS
Scale Factor Accuracy0.2%
Non-Linearity0.1% FS
Random Walk6 deg/sqrt (h)
Linear accelerationInput Range: X/Y/Z±2 g
Bias0.3 mg RMS
Scale Factor Accuracy<0.1%
Non-Linearity0.2% FS
Random Walk0.06 deg/sqrt (h)
Table Table 2. The pseudocode of the proposed algorithm.

Click here to display table

Table 2. The pseudocode of the proposed algorithm.
State (s), Covariance (Cov), Position (Po), Velocity (Ve), Probability (PP)
1.procedure MAIN % Main program
2. [S, Cov, Po, Ve, PP] ← Initialize();
3. Start IMU & ultrasonic measurements ();
4.loop % 50 Hz rate
5.  WaitNextIMUSample;
6.  [ωb,fb,Yaω] ← GetIMUData();
7.  [ V N INS, ] ← IMUattitudesolution(ωb,fb);
8.  [ V N INS, ] ← DeadReckoning( , );
9.  DataUltrasonic ← GetUltrasonicData();
10. T O A ( 1 , , n ) Ultrasonic ← MeanTOA(DataUltrasonic);
11. T O A ( 1 , , n ) Ultrasonic ← TOAtoDistanceModel( );
12. P N INS ← INSPositionToDistanceModel( , );
13. V e WSN ← GetVelFromCodeWheel();
14.  [ V e WSN, ] ←GetNorth&EastVel( , Yaω);
15.z← [δVe, δd2]; % Measurements
16.Covz ← GetCovZ(z);
17.S← [ V e i - 1 +, ];
18.Cov ← [ P P i , , Covz];
19.  [S,Cov] ← IEKF(z,S,Cov);
21.  FilterData ← StoreFilterData(S,Cov);
22.  if (OutputData = = 1)
23.     [δPE,δPN,δPE,δPN]IERTSS ← IERTSS(S,Cov);
24.      [PE,PN,VE,VN]IERTSS ←[ V N INS, , , ] –[δPE, δPN,δPE,δPN]IERTSS
25.      [PE,PN,VE,VN]Avg. IERTSS ←GetAverageValue([PE,PN,VE,VN]IERTSS);
26.    OutputData();
27.  endif
28.end loop
29. Stop IMU & ultrasonic measurements ();
30. StoreSession(All variables); % For ananlysis
31.end procedure
Table Table 3. Comparison of five estimation strategies in terms of position error.

Click here to display table

Table 3. Comparison of five estimation strategies in terms of position error.
MethodRMSE (cm)

EastNorthMean
INS-only8589.143625.926107.53
WSN11.787.719.74
EKF6.936.116.52
ERTSS5.735.555.64
Off-line IERTSS3.543.463.50
Table Table 4. Comparison of five estimation strategies in terms of velocity error.

Click here to display table

Table 4. Comparison of five estimation strategies in terms of velocity error.
MethodRMSE (cm/s)

EastNorthMean
INS-only343.49131.48237.48
WSN5.738.797.26
EKF4.076.125.10
ERTSS3.154.733.94
Off-line IERTSS2.772.792.78
Table Table 5. Relationship between smoothing window size and position RMSE (cm).

Click here to display table

Table 5. Relationship between smoothing window size and position RMSE (cm).
SizeEastNorthAverage
1 T4.374.164.27
2 T4.223.954.09
3 T4.073.843.96
4 T3.943.733.84
5 T3.833.753.79
6 T3.723.733.73
7 T3.713.773.74
8 T3.693.833.76
9 T3.653.973.81
10 T3.604.063.83
20 T4.305.925.11
40 T7.0210.598.80
60 T15.5610.0112.78
80 T13.0220.2116.61
100 T15.0625.4720.26
Table Table 6. Relationship between smoothing step and velocity RMSE (cm/s).

Click here to display table

Table 6. Relationship between smoothing step and velocity RMSE (cm/s).
SizeEastNorthAverage
1 T3.944.214.07
2 T3.724.073.89
3 T3.783.923.85
4 T3.713.903.81
5 T3.613.983.79
6 T3.683.803.74
7 T3.933.813.87
8 T4.023.943.98
9 T4.523.984.25
10 T3.903.973.93
20 T4.595.244.92
40 T6.527.276.90
60 T8.107.117.61
80 T9.989.769.87
100 T12.359.3010.83
Table Table 7. The average of average RMSEs for position and velocity.

Click here to display table

Table 7. The average of average RMSEs for position and velocity.
SizePosition (cm)Velocity (cm/s)Average
1 T4.274.074.17
2 T4.093.893.99
3 T3.963.853.90
4 T3.843.813.82
5 T3.793.793.79
6 T3.733.743.73
7 T3.743.873.81
8 T3.763.983.87
9 T3.814.254.03
10 T3.833.933.88
20 T5.114.925.01
40 T8.806.907.85
60 T12.787.6110.19
80 T16.619.8713.24
100 T20.2610.8315.55
Table Table 8. Comparison of on-line and off-line mode in terms of position error.

Click here to display table

Table 8. Comparison of on-line and off-line mode in terms of position error.
MethodRMSE (cm)

EastNorthMean
Off-line IERTSS3.543.463.50
On-line IERTSS3.723.733.73
Table Table 9. Comparison of on-line and off-line mode in terms of velocity error.

Click here to display table

Table 9. Comparison of on-line and off-line mode in terms of velocity error.
MethodRMSE (cm/s)

EastNorthMean
Off-line IERTSS2.772.792.78
On-line IERTSS3.683.803.74
Sensors EISSN 1424-8220 Published by MDPI AG, Basel, Switzerland RSS E-Mail Table of Contents Alert