Integrated SINS / WSN Positioning System for Indoor Mobile Target Using Novel Asynchronous Data Fusion Method

According to the asynchronous transmission of data for the SINS/WSN integrated positioning system, this paper proposes a novel asynchronous data fusion method using Unscented Kalman Filter for SINS/WSN integrated positioning system based on indoor mobile target. The state equation of the integrated system is built with the motion characteristic of mobile target. The pseudo measurement equation is built based on the time sequence of SINS/WSN measured value through detecting the measurement of WSN in every fusion period. Considering that the improved state-space model, comprised of the state equation and pseudo measurement equation, is the nonlinear equations, the Unscented Kalman Filter is applied to estimate the state value of the statespacemodel.Hence the asynchronous data fusionmethod for SINS/WSN integrated positioning system can be achieved. Simulation results and experimental tests show that the positioning system with proposed asynchronous data fusion algorithm performs feasibility and stability under circumstances of the asynchronous time, and it is superior to the traditional asynchronous data fusion and synchronous data fusion methods.


Introduction
Indoor localization of mobile target is playing an increasingly important role on home intelligence, factory controllability, and shopping malls automation [1].Indoor positioning technology is a critical part in the location based services for indoor mobile target.High-precision positioning system can improve the automation and intelligence of mobile targets [2].Strap-down Inertial Navigation System (SINS), entirely self-contained within the mobile target, neither sends signal to external nor depends on external signal [3].The SINS can continuously supply the overall motion parameters and the short-term high performance for indoor positioning; however the SINS are known for their drift with time [4].In order to achieve long-term stability, other technologies will be used to support the SINS.It is well known that the Global Positioning System (GPS) can supply the high-precision position and velocity of the mobile target [5].According to the cumulative error of SINS, the GPS can be applied to correct the SINS; then the integrated positioning system will be established based on SINS and GPS [6].However, the signal of GPS is obstructed by the building; a non-GPS localization system need be aided by the SINS for indoor mobile target.
A Wireless Sensor Network (WSN) has enormous potential for the short-range indoor localization with intelligent and distributed network [7].WSN is composed of some mobile nodes and a large number of anchor nodes through the self-organization and multihop methods [8].In order to achieve continuous indoor positioning, some scholars put forward the INS/WSN integrated positioning system [9].Hur and Ahn [10] propose a localization technique for mobile target using INS/WSN based on an intelligent filter with low complexity.Chen et al. [11] propose an INS/WSN integration system of mobile target with adaptive extended Kalman Filter.Because the WSN and SINS are two separate (self-contained) subsystems, the data alignment discrepancies between SINS and WSN would appear with the clock difference and data transmission latency.The time synchronization between WSN and SINS becomes a matter of great public interest before the integrated positioning system is implemented [12].The data alignment discrepancies could lead the suboptimal fusion algorithm and low-precision for the integrated system.This is due to the reason that the majority of the data fusion theories need to work in ideal condition without system time bias.However, the actual positioning system with multisensor cannot meet the condition of synchronous data fusion model [13].In order to research this problem, Hu et al. [14] propose a batch asynchronous data fusion algorithm which can strictly synchronize asynchronous measurement in the time domain.Skog and Handel [15] and Yang and Shim [16] analyze the effects of time synchronization errors in a GPS-aided INS integrated positioning system and proposed a software-based time synchronization method using a data integration filter.Gao [17] proposes an asynchronous data fusion algorithm to solve problems about asynchronous fusion of multisensors and achieved good results for the SINS/GPS/CNS integrated navigation system.However, the data type of GPS is different from the WSN.The time synchronization method of GPS/INS integrated positioning system cannot be applied to the WSN/SINS positioning system.At the present, the proposed fusion method iteratively calculates the state equation for whole asynchronous data in every update period, which is described as the traditional asynchronous fusion algorithm.Furthermore, the traditional asynchronous fusion algorithms require large calculation burden and cost a lot of time, which may cause large position error occurring in a high-dynamic system.
Given all that, nowadays the research for time synchronization algorithms of integrated positioning system is mainly used for the GPS/INS.The traditional asynchronous data fusion algorithms have some adverse conditions which are the large calculation burden, being time-consuming, and large position error for high-dynamic system.To solve these problems, this paper proposed an asynchronous data fusion algorithm with Unscented Kalman Filter (UKF) to apply the SINS/WSN integrated positioning system.The state equation of state-space model is built with the motion characteristic of mobile target.The measurement equations of two subsystems are established based on the measurements of SINS and WSN, respectively.The time duration which is comprised of some sample periods of SINS is used for time unit of the fusion center.If the measurement of WSN existed in a time unit, the pseudo measurement equation of SINS/WSN will be built based on the asynchronous data fusion method.On the contrary, the measurement equation of pure SINS will be used.Hence the state-space model for asynchronous data fusion method is established.Owing to the nonlinear improved state-space model, the UKF is applied to estimate the state value.Then the novel asynchronous fusion with UKF for SINS/WSN integrated positioning system is achieved.
The rest of the paper is organized as follows.The proposed state-space model established with asynchronous data fusion theory is presented in Section 2. Section 3 describes Unscented Kalman Filter algorithm, while Section 4 evaluates the asynchronous data fusion method for SINS/WSN integrated positioning system in simulation.In Section 5, we examine the performance of the proposed method.Finally, Section 6 concludes the paper.

State-Space Model for Positioning System
This section describes the process of establishment for the state-space model.This model is built with state equation and pseudo measurement equation of SINS/WSN integrated positioning system through the asynchronous data fusion method.

State Equation.
The state vector of integrated system can be expressed as x = [p   v  a  ]  .p and v  are the position and velocity of mobile target in the navigation frame (-frame), respectively. is the triaxis attitude angle of mobile target and a  is the acceleration of mobile target in the body frame (-frame).
According to the movement characteristics of mobile target, the motion equations are illustrated as follows: where C   is the --frame transformation matrix. is the sample time of system.Then the state equation of mobile target is shown as where W  represents system noise vector, W  = [  ∇  ]  .  and ∇  are gyros drift and accelerometer bias, respectively.f(⋅) represents the -dimension nonlinear vector function.G  is the model input matrix of system.
where  ,SINS , the Gaussian process noise, is the measurement noise of SINS as  SINS = [ ,SINS  V,INS ]  .The measurement transformation matrix H ,SINS is denoted as The measurement vector of WSN can be defined as z WSN = (p  WSN )  , where p  WSN is the position of WSN.Then the measurement equation of WSN can be expressed as where is the time interval between   and the moment for th measurement of SINS ( = 1, 2, . . .,  SINS

𝑘
).    is the time interval between   and the moment for ith measurement of WSN ( = 1, 2, . . .,  WSN  ).According to the time stamps of data, all measurements are sorted with the order of measured time in fusion center, after all measurements have reached the fusion center at ( −1 ,   ].Because the update frequency of SINS is considerably greater than that of WSN, it may lead to nonmeasurement of WSN in some fusion periods when the fusion period is smaller than the sampling period of WSN.Considering the high accuracy clock frequency of SINS, the time range with a fixed number of sampling periods of SINS is set as the data fusion period.Figure 1 displays the timeline of measured value.
According to ( 4) and ( 6), the measured expressions of SINS and WSN can be obtained at ( −1 ,   ] as where

Pseudo Measurement Equation of Integrated System.
According to data transmission characteristics of SINS/WSN, the pseudo measurement equation of asynchronous statespace model can be divided into two kinds of fusion types.When the measurements of SINS existed only in one update period, the state equation used by SINS is updated with SINS data, instead of asynchronous fusion state equation updating.Otherwise the pseudo measurement equation would be built with SINS and WSN, and then the asynchronous fusion algorithm will be calculated.
(a) If there has been no measurement of WSN at ( −1 ,   ], the discussion is illustrated as follows: if there has been no measurement of WSN at ( −1 ,   ], the data fusion center will implement the pure SINS update in the fusion center.The state equation and measurement equation of SINS in a sample time (denoted as ( − −1  ,  −   ]) can be obtained as Every SINS measured value would be used to update the state-space equation at ( −1 ,   ], so the fusion algorithm of positioning system in whole fusion period can be obtained. (b) If there have been some measurements of WSN at ( −1 ,   ], the discussion is illustrated as follows: according to (2), at the moment of  −   , the state vector is expressed as From (10), then Put it into (8) and the measurement vector of SINS can be obtained as According to Taylor's formula, ( 12) can be further developed as So we can define Then ( 13) is Similarly, the measurement vector of WSN can be obtained as The schematic for the timeline of measured value.
According to the order of measured value from WSN and SINS, the parameters of pseudo measurement equation are defined as So the pseudo measurement equation can be obtained as Then the state-space model of asynchronous data fusion algorithm for the integrated positioning system can be achieved.

Unscented Kalman Filter for Positioning System
The state-space model based on SINS/WSN integrated positioning system which is reflected in (3) and ( 18) is a nonlinear model, so the Unscented Kalman Filter (UKF) would be applied to estimate the state vector [18].We define the following: the initial state x 0 is independent from all noise.Initialized state vector and the state covariance matrix are (x 0 ) = x0 and cov(x 0 ) = P 0 .
where  =  2 ( + ) − ; the coefficient  determines the diffused degree of the sigma points; here  = 0.01 and  = 0. (√( + )P −1 )  expresses the elements of matrix square root in the ith row.n is the number of dimensions for vector x  .From (3), we can get  = 12.

(b) Sigma Points Transformation through the State Equation
where (21)

𝑖
is the weight coefficient for the mean associated with the ith point and  ()   is the weight coefficient for the covariance associated with the ith point.The tuning parameter is  = 2. (23)

Performing Update.
According to the new measurement z  , the filter of the system is updated, and then the state estimates can be obtained: , where K  is the gain matrix of filter.
Finally, the novel asynchronous data fusion algorithm has been built according to the state-space model and UKF. Figure 2 shows the flow chart for the novel asynchronous data fusion method using state-space model and UKF based on the SINS/WSN positioning system.The traditional asynchronous fusion algorithm cannot judge whether the WSN measurement exists in this fusion period, so it must calculate the asynchronous state-space equation in every fusion period.It will consume more calculation and time than proposed asynchronous fusion algorithm; then the positioning error of traditional asynchronous fusion algorithm will be increased.

Simulation Results
Simulation experiments have been carried out to evaluate the performance of the proposed approach in comparison with the traditional fusion methods and the other filter algorithms based on SINS/WSN integrated positioning processing.Program codes are constructed with the Matlab 2010 version software.We can define the initial position and attitude angle as zero on the 3D Cartesian coordinate system.The vector of initial velocity is (10, 10, 0) m/s in three directions.The initial acceleration is zero in three directions, respectively.According to the performance parameters of SINS and WSN, the simulation initial conditions are expressed as Q = diag(0.002,0.002, 0.002, 0.002, 0.002, 0.002), R SINS = diag(30, 30, 30, 2, 2, 2), R WSN = diag(50, 50, 50), and P = diag(10, 10, 10, 1, 1, 1, 20, 10, 10, 10, 10, 10).The sampling periods of SINS and WSN are  SINS = 0.2 s and  WSN = 1 s, respectively.The fusion period is selected at 0.8 s.The simulation lasts for 60 s.We can simulate the above process with UKF, Extended Kalman Filter (EKF), and Particle Filter (PF); the simulation results are shown in Figures 3, 4, and 5.The detailed derivation and algorithm procedures of EKF and PF can be found in [19,20].
Figure 3 shows the positioning trajectory of proposed asynchronous data fusion method with UKF, EKF, and PF based on the SINS/WSN integrated positioning system.The reference trajectory is expressed as a black line with dot and that with EKF algorithm is expressed as red dash with triangle; in addition that with UKF algorithm is expressed as a blue line with dot and that with PF algorithm is expressed as a green line.Obviously the positioning system with UKF can track the reference trajectory effectively.Additionally the positioning system with PF can track the reference trajectory at the start of simulation, while that with PF has engendered the cumulative error over time.However the positioning system with EKF causes the serious divergence at the end of simulation.Because the PF algorithm needs a large amount of calculation, the position error of positioning system with PF has been accumulated over time.The EKF algorithm needs the first order linearization of the nonlinear function to be done; furthermore the EKF has to calculate the Jacobi matrix of the nonlinear function.Moreover the estimation accuracy of EKF will be seriously decreased and even diverged, when the model of positioning system has strong nonlinearity.
In Figure 4, the position errors of proposed asynchronous data fusion method with UKF, EKF, and PF algorithms are displayed.The UKF method is depicted in blue dash dotted line and the EKF method is in red dotted line and the PF method is in green real line.The maximum of position error with EKF is 38.1 m and the maximum of that with PF is 15.8 m; in addition the maximum of position error with UKF is 7.2 m.The UKF method reduces the position errors about 81.1% and 54.4% compared with EKF and PF methods.The UKF is a feasible filtering method for the improved asynchronous data  fusion method based on SINS/WSN integrated positioning system.
Figure 5 shows the attitude errors of proposed asynchronous data fusion method with UKF, EKF, and PF algorithms.It is obvious that positioning systems with UKF and PF have small attitude error, while the attitude error of positioning system with EKF has engendered the divergence at the end of simulation.As a consequence, the maximum attitude errors with UKF and PF are 1.1 deg and 2.8 deg, respectively.Positioning accuracy of attitude with UKF meets the acquirement of indoor navigation.
In order to verify the effect of the SINS/WSN integrated positioning system, we design the simulation experiment with different fusion algorithms.Firstly, the time difference of data arrival between SINS and WSN always has not been considered with the simple synchronous data fusion methods, so this method can be described as the synchronous data fusion method.Moreover the SINS/WSN integrated positioning system with UKF is carried out based on three fusion methods which include the asynchronous data fusion method proposed by this paper and traditional asynchronous data fusion and synchronous data fusion methods.Finally, the results are shown in Figures 6 and 7.
Figure 6 shows the position errors for integrated positioning system based on the proposed method and traditional asynchronous data fusion (denoted as UKF-asyn) and synchronous data fusion (denoted as UKF-syn) methods.The arrival time of SINS data is over half second faster than that of WSN data.Position errors of synchronous data fusion are larger than that of traditional asynchronous data fusion and proposed method, because the synchronous data fusion cannot consider the time difference between the SINS and WSN.The maximum position error with synchronous data fusion algorithm is 33.3 m, while the maximum position errors with traditional asynchronous data fusion algorithm and the proposed method are 10.8 m and 5.3 m, respectively.The traditional asynchronous data fusion algorithm cannot discuss whether the measurement of WSN exists in the fusion period, so it consumes more calculation time.
Figure 7 shows the average position errors for many time differences between SINS and WSN.We can define the SINS data as half or one second faster than the WSN data; in addition SINS data are half or one second slower than the WSN data.So the horizontal ordinate can be expressed as −1, −0.5, 0, 0.5, and 1.From Figure 7, we can see that the positioning error based on synchronous data fusion is  the minimum without time difference between SINS and WSN.Similarly, the minimum position errors based on both the traditional asynchronous data fusion and proposed method appeared at the zero time difference.The position error with synchronous data fusion is the smallest of three fusion methods at none time difference, because the synchronous data fusion algorithm needs less calculation.The position error with synchronous data fusion has been increased quickly, when the SINS data is over a half second faster than WSN data.However, the position errors with traditional asynchronous data fusion and proposed method are hardly increased with increase of the time difference.The traditional asynchronous data fusion algorithm and the proposed method perform effectively stability for the varied time difference between SINS and WSN, and note that the  accuracy of proposed method is higher than traditional asynchronous data fusion algorithm.

Experimental Results
In this section, we perform several tests to evaluate the proposed asynchronous data fusion method based on SINS/ WSN integrated navigation system in the room.An electric vehicle is used as the mobile target in the tests.The initial parameters of the positioning system are given as follows: (1) The WSN consisted of four anchor nodes and a mobile node.The mobile node is attached on the mobile  target and the anchor nodes are deployed in the four corners of the room.A rectangle location area is constituted by four anchor nodes.Time synchronization for TOA approach among anchor nodes can be accomplished through the Ethernet cable.The power is supplied for anchor nodes through the twisted pair and the mobile node is operated through batteries.The sampling period of WSN is 1 s.
(2) The used CleverNavi type SINS includes six-degreeof-freedom IMU ADIS16350 which consisted of a triaxial accelerometer and a triaxial gyroscope.The RS232 serial communication is used only for data transmission between SINS and computer.The baud rate is 115200 bit/s, and the sampling period is 0.01 s.Attitude reference precision of SINS is 10  when it is towards the fixed north.The latitude accuracy is not less than 1  , and the accuracy of gravitational acceleration is not less than 2 × 10 −5 g.The power is supplied for the SINS through a storage battery in the electric vehicle.
(3) The SINS is installed on the mobile target and the inertial data is transported with two Bluetooth models.One Bluetooth model is connected to the SINS with ribbon cable; the other one is connected to the computer by a wired USB-serial connection (Bluetooth 1.1 and USB 2.0).The maximum received distance between two Bluetooth models is up to 60 m in ideal conditions (free space).The wireless signal is broadcasted by the mobile node of WSN real time and received by anchor nodes.The distance values among the mobile node and anchor nodes are firstly collected by anchor nodes and then forwarded to the router which is connected with a computer through the Ethernet cable.Meanwhile inertial data of SINS and the measured distance value of WSN are used to be computed in fusion center.As a consequence, the position and attitude of integrated positioning system can be obtained.Moreover the electric vehicle is controlled by an operator through a remote controller; the electric vehicle can move along a predesigned trajectory.Meanwhile the velocity of electric vehicle can be detected by a speed-sensor on a wheel.As a consequence the reference trajectory of mobile target can be obtained.The structure block for the experimental system is presented in Figure 8. Figure 9 shows the experimental diagram of integrated positioning system with SINS/WSN.
The frame of nodes deployment and a predetermined trajectory are shown in Figure 10.The positions of anchor nodes are (0.6, 7.4) m, (5.4,7.4) m, (5.4,0.6) m, and (0.6, 0.6) m, respectively.The starting point of mobile target is (4.75, 1.00) m.In order to control the dimension of matrix and calculation burden of fusion center, the sampling period time of SINS can be set as 0.1 s.The fusion period is selected as 0.8 s.The experiment lasts for 41 s.
Figure 11 shows the experimental performance of the SINS/WSN integrated positioning system with different data fusion algorithms.The real trajectory is shown with a black line.The trajectories with traditional asynchronous data fusion algorithm (denoted as UKF-asyn) and synchronous data fusion algorithm (denoted as UKF-syn) are expressed with the green dash line and red dash line, respectively.The trajectory with proposed asynchronous data fusion method is shown with a blue dash dotted line.Note that the trajectories with three algorithms can track the real trajectory accurately at the beginning of motion process.The trajectory with synchronous data fusion algorithm begins to produce the position error at the first corner of the trajectory.Furthermore, the position error with synchronous data fusion algorithm has deviated from the real trajectory with time.The maximum position error with synchronous data fusion algorithm is 1.11 m.However the trajectory of positioning system based on traditional asynchronous data fusion algorithm can track the real trajectory with small position error.The position error with the proposed method is smaller than that with traditional asynchronous data fusion algorithm.Because of the accumulative error of SINS, the positioning system with the proposed method still has a small position error for real trajectory.The average position error with the proposed method and traditional asynchronous data fusion algorithm are 0.28 m and 0.70 m, respectively.The tracking accuracy of the positioning system with the proposed method surpasses obviously that with synchronous data fusion and traditional asynchronous data fusion algorithms, 68.8% and 36.9%improvements, respectively.The position errors of integrated positioning system with different fusion methods are shown in Figure 12.Table 1 shows performance comparison for different fusion algorithms based on the SINS/WSN integrated positioning system.
In order to evaluate the real-time performance of the proposed method and traditional asynchronous data fusion algorithm, the integrated positioning system is used to test with  synchronous data fusion algorithm will bring the large positioning error.The traditional asynchronous data fusion algorithm needs large calculation burden and causes the low positioning accuracy for high-dynamic system.This paper proposes a novel asynchronous data fusion algorithm with UKF for the SINS/WSN integrated positioning.Asynchronous data fusion algorithm based on positioning system is analyzed in simulation with variety of conditions.Simulation results indicated that the maximum position error with UKF is 7.2 m when the mobile target moves about 600 meters; meanwhile the position error with UKF is reduced by 81.1% and 54.4% compared with EKF and PF methods, respectively.Additionally the position error with the proposed method is the smallest compared with the synchronous data fusion and traditional asynchronous data fusion algorithms.Note that the position error with synchronous data fusion is grown seriously; the proposed method and traditional asynchronous data fusion algorithm performed stability as the time difference increased.The experimental results present the fact that the positioning system with the proposed method can track the real trajectory effectively.The position error with the proposed method is 0.28 m and surpasses that with synchronous data fusion and traditional asynchronous data fusion algorithms, 68.8% and 36.9%improvements.Moreover the proposed method needs less calculation and has better real-time performance in comparison with the traditional asynchronous data fusion algorithm.As a consequence, the asynchronous data fusion method with improved state-space model and UKF is a feasible positioning method for the data alignment discrepancies.

𝑘
matrixes of SINS and WSN, respectively, which apply ith measured value. SINS −   and  WSN −   are the zero-mean Gaussian white noise vector.Their covariance matrixes are R SINS −   and R WSN −   , respectively.We can define z  = {z −   }   =1 and Z  = {z  }  =1 .The state estimation value (denoted as x−1|−1 =  * [x −1 |z −1 ]) and covariance matrix of estimation error (denoted as P −1|−1 = cov[x −1|−1 |z −1 ]) last time have been obtained in fusion center at the moment of  −1 .Then the asynchronous fusion method can be described simply as follows: the expressions (denoted as x| =  * [x  |z  ] and P | = cov[x | |z −1 ]) would be calculated after a new measurement z  is obtained in fusion center at the moment of   .

Figure 2 :
Figure 2: Flow chart for the asynchronous data fusion method based on the SINS/WSN positioning system.

Figure 3 :
Figure 3: Positioning performance of improved asynchronous data fusion method with UKF, EKF, and PF algorithms.

Figure 4 :
Figure 4: Position errors of improved asynchronous data fusion method with UKF, EKF, and PF algorithms.

Figure 5 :
Figure 5: Attitude errors of improved asynchronous data fusion method with UKF, EKF, and PF algorithms.

Figure 6 :
Figure 6: Position errors for integrated positioning system based on different fusion methods.

Figure 7 :
Figure 7: Average position errors of positioning system for various time differences between SINS and WSN.

Figure 11 :
Figure 11: Experimental performance of the SINS/WSN integrated positioning system with fusion algorithm.

Figure 12 :
Figure 12: The position error of integrated positioning system with different fusion methods.

Figure 13 :
Figure 13: The consumption time for operation with different fusion periods.
,WSN , the Gaussian process noise, is the measurement noise of WSN as  ,WSN = ( ,WSN )  . SINS and  WSN , and  WSN > 10 SINS .We can define   as the number of measurements for all sensors at the interval ( −1 ,   ].Then note that one or more measured values might be supplied by a sensor, or none of measured values might be supplied by this sensor. SINS