Application of Adaptive Extended Kalman Smoothing on INS/WSN Integration System for Mobile Robot Indoors

Abstract

The inertial navigation systems (INS)/wireless sensor network (WSN) integration system for mobile robot is proposed for navigation information indoors accurately and continuously. The Kalman filter (KF) is widely used for real-time applications with the aim of gaining optimal data fusion. In order to improve the accuracy of the navigation information, this work proposed an adaptive extended Kalman smoothing (AEKS) which utilizes inertial measuring units (IMUs) and ultrasonic positioning system. In this mode, the adaptive extended Kalman filter (AEKF) is used to improve the accuracy of forward Kalman filtering (FKF) and backward Kalman filtering (BKF), and then the AEKS and the average filter are used between two output timings for the online smoothing. Several real indoor tests are done to assess the performance of the proposed method. The results show that the proposed method can reduce the error compared with the INS-only, least squares (LS) solution, and AEKF.

1. Introduction

Automation indoor mobile robots have increasingly been used in a wide range of applications [1]. The ability to obtain their navigation information (such as position and velocity) has become one key issue. Although the global positioning system (GPS) is widely used for navigation applications, it is essential for outdoor navigation, and there is also a growing need for accurate navigation information indoors [2]. Consequently, this topic has received significant scientific research attention over the past few decades.

In order to achieve accurate navigation information indoors, a number of methods for localization with various sensors and precision were proposed [1–3]. For instance, in [4], an RFID-based position and orientation measurement system for mobile objects was proposed by Shirehjini et al.; in [5] Park proposed an indoor location system using ZigBee; in [6], Saad et al. proposed high-accuracy reference-free ultrasonic location estimation. All the above-mentioned attempts employ reference node (RN) with known location to complete the localization of blind node (BN). Its principle is similar to global positioning systems (GPS), but the communication technology used by Beacon-based solutions is short-range radio, such as WiFi, UWB, RFID, ZigBee, and ultrasound. The shortcoming of the above-mentioned attempts is that the localization accuracy has to maintain a high density of RNs, which is not useful for large localization area.

To outdoor navigation, in order to achieve continuous navigation, inertial navigation systems (INS) have been used for the compensation to the GPS outage since it is capable of providing positioning information independently [7]. For example, a novel hybrid of least squares support vector machine (LS-SVM) and Kalman filter for GPS/INS integration was proposed by Xu et al. in [8]. To indoor navigation, Ruiz et al. employed inertial measuring units (IMUs)/radio frequency identification (RFID) integration navigation for pedestrian indoor navigation in [2]. However, INS solution is poor in long-term self-contained navigation since the accuracy deteriorates with time [9, 10].

In the integrated system, the integration filter should be carefully designed since it is the core of system. As one of the most popular information fusion algorithms, Kalman filter (KF) is widely used in integrated system. However, although it is able to achieve the optimal estimation of states in multiinput and multioutput (MIMO) systems [11], KF is not suitable for nonlinear systems since the noises of system and measurement should be corrupted by white noise and the state estimation is approached with the minimization of the covariance of the estimation error. Then, the extended KF (EKF) is proposed to overcome this problem by using Taylor series expansion [10]. However, the EKF is difficult to track the accurate state during the target’s fast movement since it employs a fixed priori estimates for the process and measurement noise covariances during the whole estimation process [12]. Thus, the AEKF is proposed to update the covariance of process noise and measurement noise in current. In order to obtain high accuracy of information fusion, smoothing algorithms have been widely used in integrated navigation systems [13]. Rauch-Tung-Strieble smoothing (RTSS) is widely used in navigation applications due to its robustness and effectiveness [14]. Liu et al. proposed two-filter smoothing (TFS) and applied it in INS/GPS integration for postprocessing applications in [15]. Meanwhile, the results proposed in [15] show that the TFS has the advantage to be applicable in cases of nonlinear dynamics that may occur in some land-vehicle navigation (LVN) applications. TFS is performed by combining the results of forward Kalman filtering (FKF) and backward Kalman filtering (BKF) by minimizing the smoother error covariance.

This work proposed the design and implementation of adaptive extended Kalman smoothing (AEKS) on INS/WSN integration system for mobile robot indoors. In this mode, the adaptive extended Kalman filter (AEKF) is employed to improve the forward filtering output accuracy, and the back filter is used to smooth the forward filtering output. In order to achieve online smoothing, the AEKS and the average filter are used between two output periods. The remainder of the paper is organized as follows: Section 2 gives the adaptive extended Kalman smoothing for integration system. The real indoor tests and performance are illustrated in Section 3. Finally, the conclusions are given.

2. Adaptive Extended Kalman Smoothing for Integration System

2.1. Integration Model

Figure 1 displays the configuration of the integrated system. In this mode, the integrated model which is proposed in [16] is employed for the integrated system in this work. The continuous-time process model of the system is illustrated as follows: 𝛿𝑃_𝐸,𝑘𝛿𝑉_𝐸,𝑘𝛿𝐴𝑐𝑐_𝐸,𝑘𝛿𝑃_𝑁,𝑘𝛿𝑉_𝑁,𝑘𝛿𝐴𝑐𝑐_{𝑁,𝑘𝐗𝑘}=𝑇1𝑇²2𝑇00001𝑇0000010000001𝑇²200001𝑇000001_𝐀𝛿𝑃_{𝐸,𝑘−1}𝛿𝑉_{𝐸,𝑘−1}𝛿𝐴𝑐𝑐_{𝐸,𝑘−1}𝛿𝑃_{𝑁,𝑘−1}𝛿𝑉_{𝑁,𝑘−1}𝛿𝐴𝑐𝑐_{𝑁,𝑘−1𝐗𝑘−1}+𝐖_𝑘, (1) where  (𝛿𝑃_𝐸,𝑘,𝛿𝑃_𝑁,𝑘), (𝛿𝑉_𝐸,𝑘,𝛿𝑉_𝑁,𝑘),  and (𝛿𝐴𝑐𝑐_𝐸,𝑘,𝛿𝐴𝑐𝑐_𝑁,𝑘)  are the errors of position, velocity and accelerometer measured by INS in east and north direction. 𝑇  is sample time; 𝐖_𝑘  is the Gaussian process noise.

Figure 1 

Configuration of the hybrid system.

The observation vectors of the filter are formed by differencing the WSN and INS velocities  (𝑉^INS,𝑉^WSN)  and the distances between the robot and the 𝑖th RN  (𝑑^INS_𝑖,𝑑^WSN_𝑖). The measurement equation at time 𝑘 is illustrated as follows: Δ𝑉_𝐸,𝑘Δ𝑉_𝑁,𝑘Δ𝑑²_1,𝑘Δ𝑑²_2,𝑘⋮Δ𝑑²_{𝑚,𝑘𝐲𝑘}=𝛿𝑉_𝐸,𝑘𝛿𝑉_𝑁,𝑘ℎ_𝑑1𝛿𝑃_𝐸,𝑘,𝛿𝑃_𝑁,𝑘ℎ_𝑑2𝛿𝑃_𝐸,𝑘,𝛿𝑃_𝑁,𝑘⋮ℎ_𝑑𝑚𝛿𝑃_𝐸,𝑘,𝛿𝑃_{𝑁,𝑘𝐡𝐗𝑘}+𝝊_𝑘. (2)

Here, the difference between  (𝑑^INS_𝑖)²  and  (𝑑^WSN_𝑖)² is denoted as  Δ𝑑²_𝑖, and it is expressed as follows: Δ𝑑²_𝑖=𝑑^INS_𝑖²−𝑑^WSN_𝑖²=2𝑃^INS_𝐸−𝑥_𝑖𝛿𝑃_𝐸+2𝑃^INS_𝑁−𝑦_𝑖𝛿𝑃_𝑁−𝛿𝑃²_𝐸+𝛿𝑃²_𝑁,𝑖=1,2,…,𝑚. (3)

And  (Δ𝑉_𝐸,Δ𝑉_𝑁)  is the difference of the WSN and INS velocities in east and north direction, respectively, and 𝝊_𝑘  is the Gaussian process noise. It is assumed that  𝝎_𝑘  and  𝝊_𝑘  are independent zero-mean white Gaussian sequences with covariances  𝐐  and  𝐑.

2.2. Adaptive Extended Kalman Filter

Consider the nonlinear system given by (1) and (2); the AEKF used in this paper involves the following recursive relations: 𝐗_{𝑘∣𝑘−1}=𝐀𝐗_{𝑘−1∣𝑘−1}𝐏_{𝑘∣𝑘−1}=𝐀𝐏_𝑘−1𝐀^𝑇𝐊+𝐐_𝑘=𝐏_{𝑘|𝑘−1}𝐇_𝑘^𝑇𝐒_𝑘⁻¹𝐗_𝑘∣𝑘=𝐗_{𝑘|𝑘−1}+𝐊_𝑘𝐫𝐫_𝑘𝐏_𝑘∣𝑘=𝐼−𝐊_𝑘𝐇_𝑘𝐏_{𝑘∣𝑘−1}𝐫𝐫_𝑘=𝐳_𝑘−𝐡𝐗_{𝑘∣𝑘−1}−𝐫̂_𝑘𝐒_𝑘=𝐇_𝑘𝐏_{𝑘∣𝑘−1}𝐇_𝑘^𝑇+𝐑_𝑘. (4)

Here,  𝑘=1,2,…,𝑁, 𝐇(𝐗_𝑘∣𝑘)=𝜕𝐡(𝐗_𝑘∣𝑘)/𝜕𝐗_𝑘∣𝑘, 𝐫̂_𝑘  and 𝐑_𝑘  are computed by the time-varying noise statistics estimators with the following equations: 𝐫̂_𝑘=1−𝑑_𝑘−1𝐫̂_𝑘−1+𝑑_𝑘−1𝐳_𝑘−𝐡𝐗_{𝑘∣𝑘−1}𝐑_𝑘=1−𝑑_𝑘−1𝐑_𝑘−1+𝑑_𝑘−1𝝊_𝑘𝝊_𝑘^𝑇−𝐇_𝑘𝐏_𝑘𝐇_𝑘^𝑇 (5)

Here,  𝑑_𝑘−1=(1−𝑏)/(1−𝑏^𝑘),  0<𝑏<1.

2.3. Online Adaptive Extended Kalman Smoother

In this work, in order to achieve high accuracy, adaptive two-extended-filter smoothing (ATEFS) is proposed. Consider the nonlinear system given by (1) and (2); the FKF employs the AEKF mentioned in Section 2.2, and the BKF is utilizing a set of equations as follows: 𝐊^𝐵_𝑘=𝐏^𝐵_{𝑘∣𝑘+1}𝐇_𝑘^𝑇𝐒^𝐵_𝑘⁻¹𝐗^𝐵_𝑘∣𝑘=𝐗^𝐵_{𝑘∣𝑘+1}+𝐊^𝐵_𝑘𝐫𝐫^𝐵_𝑘𝐏^𝐵_𝑘∣𝑘=𝐼−𝐊^𝐵_𝑘𝐇_𝑘𝐏^𝐵_{𝑘∣𝑘+1}𝐫𝐫^𝐵_𝑘=𝐳_𝑘−𝐡𝐗_{𝑘∣𝑘+1}−𝐫̂^𝐵_𝑘𝐒_𝑘=𝐇_𝑘𝐏_{𝑘∣𝑘+1}𝐇_𝑘^𝑇+𝐑_𝑘𝐗^𝐵_{𝑘−1∣𝑘}=𝐀⁻¹𝐗^𝐵_𝑘∣𝑘𝐏^𝐵_{𝑘−1∣𝑘}=𝐀⁻¹𝐏^𝐵_𝑘∣𝑘𝐀^−1𝑇+𝐐. (6)

Here,  𝑘=𝑁−1,𝑁−2,…,1,  𝐇(𝐗_𝑘∣𝑘)=𝜕𝐡(𝐗_𝑘∣𝑘)/𝜕𝐗_𝑘∣𝑘, 𝐫̂^𝐵_𝑘  and  𝐑^𝐵_𝑘  are computed by the time-varying noise statistics estimators with the following equations: 𝐫̂_𝑘=1−𝑑_𝑘−1𝐫̂_𝑘−1+𝑑_𝑘−1𝐳_𝑘−𝐡𝐗_{𝑘∣𝑘+1}𝐑_𝑘=1−𝑑_𝑘−1𝐑_𝑘−1+𝑑_𝑘−1𝝊_𝑘𝝊_𝑘^𝑇−𝐇_𝑘𝐏_𝑘𝐇_𝑘^𝑇, (7) where  𝑑_𝑘−1=(1−𝑏)/(1−𝑏^𝑘),  0<𝑏<1. Then, the smoothing estimate, that is, the combination of the FKF update and the BKF prediction, will be fixed as 𝐊^𝑆_𝑘=𝐏^𝐵_𝑘|𝑘+1𝐏_𝑘|𝑘+𝐏^𝐵_𝑘|𝑘+1⁻¹𝐗^𝑆_𝑘=𝐊^𝑆_𝑘𝐗_𝑘|𝑘+𝐈−𝐊^𝑆_𝑘𝐗^𝐵_𝑘|𝑘+1𝐏^𝑆_𝑘=𝐏_𝑘|𝑘+𝐏^𝐵_𝑘|𝑘+1⁻¹. (8)

Here,  𝑘=1,2,…,𝑁. Order for online smoothing, the AEKS and the average filter are used between two output moments. When the integration filter needs to output the navigation solution, BKF is used to smooth the FKF output. The output of the BKF is used to compute the INS output estimation, and then the average of the INS output estimation between two output moments is sent as navigation solution. The process of online AEKS is shown in Figure 2.

Figure 2 

The process of online AEKS.

3. Indoor Localization Tests and Performance

3.1. Real Indoor Test Environment

In order to assess the performance of the proposed method, two real indoor tests were done. The real indoor test environment is shown in Figure 3. In this work, one robot and 6 RNs are employed for the test. Both the robot and the RN are marked in Figure 3. As shown in Figure 4, the robot is the carrier of the IMU and the ultrasonic sender. It is able to collect the data of IMU and the distances between the robot and the RNs by using the PC fixed on the robot. In this work, the RN is used to receive the signal of the ultrasonic ranging sent by the ultrasonic sender and calculate the distance between the RNs and robot. It is also able to send the sensor data to the ultrasonic sender when it gets the command. The sample time used in this work is 0.02 s.

Figure 3 

Real indoor test environment.

Figure 4 

The prototype of the robot.

Figure 5 displays the trajectories of the real tests. The robot runs from the beginning point (denoted by a black square) to the end point (denoted by a black circle) with 0.33 m/s. Meanwhile, the RNs are denoted by yellow circles in Figure 5.

(a)

(b)

Figure 5 

The trajectory of the real test.

3.2. The Position Errors of the Proposed Method

In this section, the position errors are discussed. The position errors for the AEKF, average filter of AEKF (average AEKF), and the proposed method are shown in Figure 6. In the figures, the AEKF solution is depicted in blue, the green line represents the average AEKF solution, and the proposed method employs the green line.

(a)

(b)

(c)

(d)

Figure 6 

The position errors for the AEKF, average AEKF, and the proposed method.

The east and north position errors of the first trajectory are shown in Figures 6(a) and 6(b), respectively. In Figure 6(a)(A), it can be seen that the AEKF is able to keep the east position error about 0.0400 m, and it decreases the mean east position errors by about 58.30% and 82.63% compared with least squares (LS) solution and the INS-only solution, respectively. The average AEKF outputs the average value of the AEKF solution between two output moments, and the results show that it is able to reduce the mean east position errors to 0.0389 m. Figure 6(a)(B) shows the east position errors for the average AEKF solution and the proposed method. It is easy to see that the proposed method is effective to reduce the east position error, and the results show that the mean east position of the proposed method is 0.0370 m. To the north position errors of the first trajectory, from Figure 6(b), it is easy to see that the proposed method solution also has the lowest error. The mean north position of the proposed method is 0.0262 m, and it reduces the mean north position errors by about 5.2% and 1% compared with the AEKF solution and the average AEKF solution.

Figures 6(c) and 6(d) show the east and north position errors of the second trajectory, respectively. In Figure 6(c), it is easy to see that the proposed method solution also has the lowest error. The mean east position of the proposed method is 0.0253 m, and it reduces the mean north position errors by about 25.56% and 14.08% compared with the AEKF solution and the average AEKF solution. The north position errors of the second trajectory are shown in Figure 6(d), and similar to the first trajectory, the average AEKF solution reduces the position error from 0.0310 m measured by AEKF to 0.0253 m; then the proposed method improves the accuracy to 0.0218, and it reduce the mean north position errors by about 13.87%. The proposed method used in the second trajectory is significantly effective than that used in the first trajectory. The performance for the INS-only, LS, AEKF, offline AEKS, average AEKF, and the proposed method is shown in Table 1.

Table 1 

The position errors of the trajectories.

3.3. The Velocity Errors of the Proposed Method

The velocity errors are discussed in this section. Figure 7 displays the position errors for the AEKF, average filter of AEKF (average AEKF), and the proposed method. Similar to Figure 6, the AEKF solution is depicted in blue, the green line represents the average AEKF solution, and the proposed method employs the green line.

(a)

(b)

(c)

(d)

Figure 7 

The velocity errors for the AEKF, average AEKF, and the proposed method.

Figures 7(a) and 7(b) display the east and north velocity errors of the first trajectory, respectively. In Figure 7(a), it can be seen that the proposed method has the lowest error. The results show that it decreases the mean east position errors by about 27.74% and 62.06% compared with LS solution and the INS-only solution, respectively. To the north velocity errors of the first trajectory, from the Figure 7(b)(A), it can be seen that the average AEKF is more effective than the AEKF, and the results show that the mean north velocity of the average AEKF is 0.0436 m/s. Figure 7(b)(B) shows the north velocity errors for the average AEKF and the proposed method, it can be seen that the proposed method decreases the mean north position errors from 0.0436 m/s to 0.0361 m/s. The east and north velocity errors of the second trajectory are shown in Figures 7(c) and 7(d), respectively. From the figures, it can be seen that the proposed method has the lowest error, and the mean velocity errors of the second trajectory in east direction and north direction is 0.0274 m/s and 0.0267 m/s. The performance for the INS-only, LS, AEKF, offline AEKS, average AEKF, and the proposed method is shown in Table 2.

Table 2 

The velocity errors of the trajectories.

4. Conclusions

This work proposed the design and implementation of AEKS on INS/WSN integration system for mobile robot indoors. In this mode, the AEKF is employed to improve the forward filtering output accuracy, and the back filter is used to smooth the forward filtering output. In order to achieve online smoothing, the AEKS and the average filter are used between two output period. Two real indoor tests have been done to assess the performance of the proposed method, and the experimental results show that proposed method is the most effective method to estimate the navigation information and give the optimal state estimation.

Acknowledgments

This work was supported in part by the National Natural Science Foundation of China (nos. 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).