Adaptive Iterated Extended Kalman Filter and Its Application to Autonomous Integrated Navigation for Indoor Robot

As the core of the integrated navigation system, the data fusion algorithm should be designed seriously. In order to improve the accuracy of data fusion, this work proposed an adaptive iterated extended Kalman (AIEKF) which used the noise statistics estimator in the iterated extended Kalman (IEKF), and then AIEKF is used to deal with the nonlinear problem in the inertial navigation systems (INS)/wireless sensors networks (WSNs)-integrated navigation system. Practical test has been done to evaluate the performance of the proposed method. The results show that the proposed method is effective to reduce the mean root-mean-square error (RMSE) of position by about 92.53%, 67.93%, 55.97%, and 30.09% compared with the INS only, WSN, EKF, and IEKF.


Introduction
As the development of automation indoor mobile robots, how to obtain accurate navigation information of indoor mobile robots has received great attention over the past few decades.
To the integrated system, the global positioning systems (GPS)/inertial navigation systems (INS) integrated system is one of the most used methods for the outdoor navigation. Many attempts try to improve the accuracy of the GPS/INS integration. For example, Quinchia et al. compared different error modeling of MEMS applied to GPS/INS integrated systems in [1], Jwo et al. proposed a fuzzy adaptive strong tracking unscented Kalman filter for ultratight GPS/INS integrated systems [2], Chen et al. proposed a GPS/INS system using novel filtering methods for vessel attitude determination [3], and Jwo et al. proposed a nonlinear filtering with IMM algorithm for ultratight GPS/INS integration [4]. Meanwhile, in order to overcome the GPS outage, some attempts try to design bridge methods by using the artificial intelligence algorithms [5] such as Neural Networks (NN) [6][7][8] and least squares support vector machine (LS-SVM) [9][10][11]. However, since the accuracy of the integrated system is depending on the GPS, it has poor performance in the indoor environment.
In order to get higher positioning accuracy indoor, some literatures try to employ wireless localization to replace the GPS in the integrated system. For instance, S. J. Kim and B. K. Kim proposed an accurate hybrid global self-localization algorithm for indoor mobile robots with two-dimensional isotropic ultrasonic receivers [12], and an accurate pedestrian indoor navigation by tightly coupling foot-mounted IMU and RFID measurements was proposed in [13]. On the basis of the navigation strategy, the data fusion algorithm should also be designed seriously. In this field, Kalman filter (KF) is able to achieve the optimal state estimation [14]. However, it is not suitable for nonlinear systems. Thus, the extended KF (EKF) is proposed to overcome this problem by Taylor series expansion, which may introduce a truncated error [15]. In order to overcome this problem, the iterated EKF (IEKF) is proposed. However, the data fusion algorithms mentioned above are 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 [16].
In order to overcome these problems, we employed the noise statistics estimator in the IEKF, which combines the advantages of the AEKF and the IEKF. The remainder of 2 The Scientific World Journal the paper is organized as follows. Sections 2 and 3 give the introduction for AIEKF and its application to INS/WSN integrated system. The tests and discussion are illustrated in Section 4. Finally, the conclusions are given.

Adaptive Iterated Extended Kalman Filter
In this section, a brief introduction to the EKF and IEKF will be given, and then AIEKF will be proposed. It is assumed that a discrete-time model of a nonlinear system is given by where x and y are the state vector and the measurement vector for the filter, (⋅) and ℎ(⋅) are the dynamic model function and the measurement function, respectively, and and are the process noise vector and measurement noise vector, respectively. It is assumed that and are independent zero-mean white Gaussian sequences with covariance Q and R, respectively.

Extended Kalman Filter.
The traditional EKF algorithm is utilizing a set of equations as follows [17]: where

Iterated Extended Kalman Filter.
Compared with the EKF, the IEKF employs a few simple iterative operations to reduce the bias and the estimation error after getting X in (2) and P in (3). The corresponding recursive relations arê where H ( ) = h(X | )/X | and the superscript ( = 1, 2, . . . , ) is the number of iteration steps, And then, 2.3. Adaptive Iterated Extended Kalman Filter. The EKF overcomes the nonlinear problem by ignoring the higher order terms, which may introduce a truncated error. Thus, the IEKF overcomes this problem. However, it is evident that both the Q and R for EKF and those for IEKF are prior estimates.
In practice, there are uncertainties in the noise description, and the assumptions on the statistics of disturbances are violated since the availability of precisely known model is unrealistic practical situations. In order to overcome these problems, we employed the noise statistics estimator into the IEKF. Meanwhile, when the system noise is stable and the error variance is small, it is able to employ observation noise estimator only. The corresponding recursive relations arê whereR ( ) is computed by the time-varying noise statistics estimators with the following equations: here, −1 = (1 − )/(1 − ), 0 < < 1. And then,

Adaptive Iterated Extended Kalman Filter for Integrated Navigation
In this work, we just consider the navigation information for mobile robot in the relative coordinate. The INS error is the accumulation of errors in each time. In order to achieve   ] ⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟⏟ where is sample time and W is the process noise vector. The measurement equation for the filter at time is illustrated in.
Here, Δ 2 is the difference between the distance form reference node (RN) to the mobile robot measured by the INS and WSN, respectively, at time , and it is expressed as follows: where INS  is the difference of the WSN and INS velocities in east and north direction, respectively, and̃is measurement noise vector. The derivation of (15) has a very detailed description in [18]. The configuration of the hybrid system is shown in Figure 1

The Architecture of the Integrated Navigation System.
In this work, a real testbed is built to evaluate the performance of the proposed method. Figure 2 displays the architecture of the testbed. In this work, the mobile robot (shown in Figure 3) is used to run along the preset trajectory. The IMU fixed to the robot are used to provide INS solution for the position, velocity, and the attitude of the mobile robot. The mobile robot position measured by the WSN is used as ultrasonic sender and the receiver. And the computer is used for saving sensor data.
The sample time used in the test is 0.02 s, and the mobile robot runs along the trajectories shown in Figure 4 Table 2 shows the comparison of five estimation strategies in terms of velocity error. It can be seen that the EKF, IEKF, and the proposed method are able to reduce the velocity error compared with the IN S and the WSN, respectively. The result shows that the mean RMSE of velocity for the proposed method is 0.0468 m/s. However, the velocity estimation accuracy for the EKF, IEKF, and the proposed method is close.

Conclusions
In this work, the noise statistics estimator is employed into the IEKF to combine the advantages of the AEKF and the IEKF. Then, the AIEKF is used in INS/WSN integrated system. The experimental results show that the proposed method is effective in reducing the position error compared with the INS only, WSN, EKF, and IEKF; however, the velocity estimation accuracy for the EKF, IEKF, and the proposed method is close.