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/).
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.
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 [1–3].
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.
In this section, a brief introduction to the IEKF and ERTSS will be given, and then, an on-line IERTSS will be proposed.
Iterated Extended Kalman Filter
It is assumed that a discrete-time model of a nonlinear system is given by Equations (1) and (2):
Xk=f(Xk-1)+Bk-1ωk-1yk=h(Xk)+υkwhere X_{k} is the state vector at time k, f(X_{k}) is the system nonlinear function, B_{k} is the process noise input matrix, y_{k} is the observation vector, and h(X_{k}) 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=Ak-1X^k-1∣k-1Pk∣k-1=Ak-1Pk-1Ak-1T+Qwhere
Ak=∂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 P_{k|k}_{-1} in Equation (4). The corresponding recursive relations are:
X^k∣k1=X^k∣k-1Pk∣k1=Pk∣k-1Kkn=Pk∣k-1Hn(X^k∣kn)T[Hn(X^k∣kn)Pk∣k-1Hn(X^k∣kn)T+R]-1X^k∣kn+1=X^k∣kn+Kkn[yk-hn(X^k∣kn)-Hn(X^k∣kn)×(X^k∣k-1-X^k∣kn)]Pk∣kn=[I-KknHn(X^k∣kn)]Pk∣k-1[I-KknHn(X^k∣kn)]T+KknR(Kkn)There,
Hn(X^k∣kn)=∂h(X^k∣kn)∂X^k∣kn, n is the number of iteration and n=1,2,…,j. Then:
X^k∣k=X^k∣kjPk∣k=Pk∣kj
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=Ak-1X^k-1∣k-1Pk∣k-1=Ak-1Pk-1Ak-1T+QKk=Pk∣k-1HkT[HkPk∣k-1HkT+R]-1X^k∣k=X^k∣k-1+Kk[zk-h(X^k∣k-1)]Pk∣k=[I-KkHk]Pk∣k-1where
Ak=∂f(X^k∣k)∂X^k∣k,
Hk=∂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:
KkS=Pk∣kAkT(Pk+1∣k)-1X^k∣kS=X^k∣k+KkS[X^k+1∣k+1S-X^k∣k-1]PkS=Pk∣k+KkS(Pk+1S-Pk∣k-1)(KkS)Twhere the superscript S denotes the smoothing, and the recursion [Equations (17)–(19)] is started from the filtering output at the final time.
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.
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δAc˙cE,kδP˙N,kδV˙N,kδAc˙cN,k]︸X˙k=[01T00000100000000000001T000001000000]︸A*[δPE,k-1δVE,k-1δAccE,k-1δPN,k-1δVN,k-1δAccN,k-1]︸Xk-1+Wk*where (δP_{E,K}, δP_{N,K}), (δV_{E,K}, δV_{N,K}) and (δAcc_{E,K}, δAcc_{N,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;
Wk*is the Gaussian process noise. Equation (20) can be transferred into a discrete-time state equation:
[δPE,kδVE,kδAccE,kδPN,kδVN,kδAccN,k]︸Xk=[1TT2/200001T0000010000001TT2/200001T000001]︸A[δPE,k-1δVE,k-1δAccE,k-1δPN,k-1δVN,k-1δAccN,k-1]︸Xk-1+Wkwhere W_{k} is the Gaussian process noise. Here, the position of the robot measured by the INS is denoted as
(PEINS,PNINS), 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):
diINS=(PEINS-xi)2+(PNINS-yi)2,i=1,2,⋯,mwhere m is the number of the RN. Theoretically, the real distance between the robot and the RN is expressed as Equation (23):
diReal=(PEReal-xi)2+(PNReal-yi)2,i=1,2,⋯,mwhere
diReal is the real distance between the robot and the RN,
(PEReal,PNReal) is the real position of the robot. The difference between
(diINS)2 and
(diReal)2 is denoted as
Δdi2, and it is expressed as Equation (24):
Δdi2=(diINS)2-(diReal)2=(PEINS-xi)2+(PNINS-yi)2-[(PEReal-xi)2+(PNReal-yi)2],i=1,2,⋯,m
The real robot position can be computed by Equation (25):
PEReal=PEINS-δPE,PNReal=PNINS-δPN
Thus, the Equation (24) can be transferred to Equation (26):
hdi(δPE,δPN)=Δdi2=2(PEINS-xi)δPE+2(PNINS-yi)δPN-(δPE2+δPN2),i=1,2,⋯,m
The final matrix of the measurement equation at k moment is illustrated in Equation (27):
[ΔVE,kΔVN,kΔd1,k2Δd2,k2⋮Δdm,k2]︸yk=[δVE,kδVN,khd1(δPE,k,δPN,k)hd2(δPE,k,δPN,k)⋮hdm(δPE,k,δPN,k)]︸h(Xk)+υkwhere υ_{k} is the Gaussian process noise,(ΔV_{E}, ΔV_{N}) 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.
Indoor localization Tests and PerformanceReal 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.
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.
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.
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.
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.
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.
ReferencesKimS.J.KimB.K.Accurate hybrid global self-localization algorithm for indoor mobile robots with two-dimensional isotropic ultrasonic receiversSaadM.M.BleakleyC.J.BallalT.DobsonS.High-accuracy reference-free ultrasonic location estimationZhangJ.SongG.-M.QiaoG.-F.MengT.-H.SunH.-T.An indoor security system with a jumping robot as the surveillance terminalRuizA.R.J.GranjaF.S.HonoratoJ.C.P.RosasJ.I.G.Accurate pedestrian indoor navigation by tightly coupling foot-mounted IMU and RFID measurementsShirehjiniA.A.N.YassineA.ShirmohammadiS.An RFID-based position and orientation measurement system for mobile objects in Intelligent EnvironmentsParkW.-C.YoonM.-H.The Implementation of Indoor Location System to Control ZigBee Home NetworkProceedings of the 2006 SICE-ICASE International Joint ConferenceBusan, Korea18–21 October 2006MinamiM.MorikawaH.AoyamaT.Design and implementation of a fully distributed ultrasonic positioning systemFangL.AntsaklisP.MontestruqueL.McMickellM.LemmonM.SunY.FangH.KoutroulisI.HaenggiM.XieM.Design of a wireless assisted pedestrian dead reckoning system—The NavMote experienceFengG.-H.WuW.-Q.WangG.-L.Observability analysis of a matrix Kalman filter-based navigation system using visual/inertial/magnetic sensorsBouladeS.FrapardB.BiardA.GPS/INS Navigation System for Launchers and Re-Entry Vehicles-Developments and Adaptation to Ariane 5Proceedings of the 5th ESA International Conference on Spacecraft Guidance, Navigation and Control SystemsFrascati, Italy22–25 October 2002XuZ.-K.LiY.RizosC.XuX.-S.Novel hybrid of LS-SVM and Kalman Filter for GPS/INS integrationEvennouF.MarxF.Advanced integration of WiFi and Inertial Navigation Systems for indoor mobile positioningLoboJ.LucasP.DiasJ.Traca de AlmeidaA.Inertial Navigation System for Mobile Land VehiclesProceedings of the IEEE International Symposium on Industrial Electronics ISIE'95Athens, Greece10–14 July 1995El-SheimyN.SchwarzK.P.Integrating Differential GPS Receivers with an Inertial Navigation System (INS) and CCD Cameras for a Mobile GIS Data Collection SystemProceedings of the International Society for Photogrammetry and Remote Sensing ConferenceOttawa, Canada6–10 June 1994FangJ.-C.GongX.-L.Predictive iterated Kalman filter for INS/GPS integration and its application to SAR motion compensationShademanA.SharifiF.J.Sensitivity Analysis of EKF and Iterated EKF Pose Estimation for Position-Based Visual ServoingProceedings of the IEEE Conference Control ApplicationToronto, Canada28–31 August 2005LefebvreT.BruyninckxH.SchutterJ.D.Kalman filters for nonlinear systems: A comparison of performanceChiangK.-W.DuongT.T.LiaoJ.-K.LaiY.-C.ChangC.-C.CaiJ.-M.HuangS.-C.On-line smoothing for an integrated navigation system with low-cost MEMS inertial sensorsRauchH.TungF.StriebelC.Maximum likelihood estimates of linear dynamic systemsGongX.-L.ZangR.FangJ.-C.Application of unscented R-T-S smoothing on INS/GPS integration system post processing for airborne earth observationXuY.ChenX.-Y.LiQ.-H.Unbiased tightly-coupled INS/WSN integrated navigation based on extended Kalman filterFigures and Tables
The flow chart of the on-line IERTSS.
The configuration of the data fusion for the integrated navigation.
The prototype of the robot.
The prototype of the RN.
Real indoor test environment.
The trajectory of the real test.
The position error for the INS-only, WSN, EKF, ERTSS and off-line IERTSS. (a) and (b) East direction; (c) and (d) North direction.
The relation between the smoothing window size and the filtering period.
Relationship between smoothing window size and RMSE. (a) Position and (b) Velocity.
The position error for the off-line IERTSS and on-line IERTSS. (a) East direction and (b) North direction.
The performance parameters for the IMU used in this work.
Parameters
Description
Data
Angular Rate
Input Range: Yaw, Pitch, Roll
±300 deg/s
Bias
0.02 deg/s RMS
Scale Factor Accuracy
0.2%
Non-Linearity
0.1% FS
Random Walk
6 deg/sqrt (h)
Linear acceleration
Input Range: X/Y/Z
±2 g
Bias
0.3 mg RMS
Scale Factor Accuracy
<0.1%
Non-Linearity
0.2% FS
Random Walk
0.06 deg/sqrt (h)
The pseudocode of the proposed algorithm.
State (s), Covariance (Cov), Position (Po), Velocity (Ve), Probability (PP)