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

The inertial navigation systems (INS)/wireless sensor network (WSN) integration system formobile 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 extendedKalman filter (AEKF) is used to improve the accuracy of forwardKalman 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.


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][2][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 referencefree 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.

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: ).The measurement equation at time  is illustrated as follows: Here, the difference between denoted as Δ 2  , and it is expressed as follows: 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 Q and R.

Adaptive Extended Kalman
Filter.Consider the nonlinear system given by ( 1) and ( 2); the AEKF used in this paper involves the following recursive relations: . .Here,  = 1, 2, . . ., , H( X| ) = h( X| )/ X| , r and R are computed by the time-varying noise statistics estimators with the following equations: Here,

Online Adaptive Extended Kalman Smoother.
In this work, in order to achieve high accuracy, adaptive twoextended-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: Here,  =  − 1,  − 2, . . ., 1, H( X| ) = h( X| )/ X| , r  and R  are computed by the time-varying noise statistics estimators with the following equations: 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 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.

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

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

The Velocity
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 Mathematical Problems in Engineering 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.

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.

Figure 1 :
Figure 1: Configuration of the hybrid system.

Figure 4 :
Figure 4: The prototype of the robot.

Figure 5 :
Figure 5: The trajectory of the real test.

Figure 6 :
Figure 6: The position errors for the AEKF, average AEKF, and the proposed method.
where ( , ,  , ), ( , ,  , ), and ( , ,  , ) are the errors of position, velocity and accelerometer measured by INS in east and north direction. is sample time; W  is the Gaussian process noise.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

Table 1 :
The position errors of the trajectories.

Table 2 :
The velocity errors of the trajectories.