Indoor Localization of a Quadrotor Based on WSN: A Real-Time Application

A real-time localization algorithm is presented in this paper. The algorithm presented here uses an extended Kalman filter and is based on Time Difference Of Arrivals (TDOA) measurements of radio signal. The position and velocity of an Unmanned Aerial Vehicle (UAV) are successfully estimated in closed-loop in real-time, both in hover and path following flights. Relatively small position errors obtained from the experiments prove the good performance of the proposed algorithm.


Introduction
The position estimation (location) problem of an object is a research subject that has attracted a lot of interest in recent years.Several applications on the location of expensive computer equipment [1, 2], management of products and transportation [3,4] and animal localization [5,6] are reported in the literature.The localization of terrestrial and aerial robots is also an important subject.In particular, Unmanned Aerial Vehicles (UAVs) are of interest since they have important industrial applications.In addition, recent advances in electronics have dramatically improved the degree of integration of onboard control systems for UAVs.However, UAV localization is still an issue which requires improvement.
For outdoor applications, GPS provides low errors in location estimation, but these errors are significant for UAVs.In addition, GPS can only be used outdoors and far away enough from buildings or large obstacles otherwise the GPS signal becomes unreliable.Furthermore, UAVs require an additional position estimation to keep operating after GPS failure.GPS outage has been addressed by combining measurements from GPS and an Inertial Measurement Unit (IMU) [7][8][9].The inertial data are used as a reference trajectory and GPS data are applied to update and estimate the state error of such a trajectory.
An alternative solution for indoor localization is based on the use of Wireless Sensor Networks (WSNs).These sensors are capable of measuring some radio signal metrics such as RSS (Received Signal Strength), AOA (Angle Of Arrival), TOA (Time Of Arrival) and TDOA (Time Difference Of Arrival).In addition, WSNs can be used both indoors and outdoors.More precisely, Chirp Spread Spectrum (CSS)-based communication protocol and chipset technology have shown to dominate the indoor localization market [10].The major advantage of the CSS technology is to provide robust performance for low rate wireless personal networks, even in the presence of path loss, multi-path and reflection [11].Symmetric Double Side Two Way Ranging (SDS-TWR) is the key to CSS [10,12].It consists on repeating the packets exchange twice, inverting the role of the two devices in the second exchange.By evaluating the propagation time using the sum of the two measurements, the impact of clock offset is highly reduced.SDS-TWR technology demonstrates a superiority over other approaches since it does not require time synchronization (quite a strong requirement) between devices, leading to an improved ranging accuracy.
Noise in radio signals poses a problem for all localization methods.Several localization algorithms have been developed based on different approaches [13][14][15].Basically, the Least Squares (LS) and Kalman Filter (KF), and modified versions of them have been used for different applications.Based on range measurements, some works have addressed target positioning using LS.In [16] the authors prove, by simulations, a constrained Weighted Least Squares (CWLS).In [17] and [18], closedform solutions are derived based on the spherical interpolation method [19,20].We refer to these LS versions as Quadratic Correction (QCLS) and Linear-Corrector (LCLS), respectively.The Kalman filter is a well-known and widely-used method to deal with noisy data and sensor fusion.In addition, internal states of a process cannot always be estimated directly.It is based on two main assumptions [21]: target motion and observation models are linear, and theirs errors and initial estimated probability distribution should be Gaussian.The first assumption is not always true, however, variants such as the Extended Kalman Filter (EKF) [22] or Unscented Kalman Filter (UKF) [23] can be used.Diverse applications of localization using KF can be found in the literature.In [1] and [2], the authors propose the tracking of computer equipment in buildings while animal localization is researched in [6].Based on CSS ranging, localization of a static wireless node is researched in [24] with some practical improvements.Localization of a forklift truck and pallets in a warehouse is proposed in [3] and [25] using SDS-TWR ranging.Some other applications of the KF to localize a sound source are made in [26] and [27].In both cases, estimation is based on TDOA measurements to track a speaker in a noisy and reverberant environment by means of a microphone array.
The problem addressed in this paper is the determination of the position and velocities of a UAV using SDS-TWR measurements [28].The originality of this work relies on the fact that it proposes an alternative solution for positioning based on a technology that has not yet been used in UAVs.The estimates are obtained by using the extended KF for hover flight and path following in indoor experiments.
The paper is organized as follows.In Section 2, we introduce the measurement models from TDOA.Section 3 deals with the proposed localization algorithm and its application to a UAV.Experimental setup and results are described in Sections 4 and 5, respectively, and the concluding remarks are finally given in Section 6.

Measurements Models
The localization problem can be stated mathematically for 2D as follows: Let , be the target position to be determined and , be the known position of the th i base station ( BS i ).The distance between x and i x denoted by i d , is given by where N is the total number of base stations.The TOA is the one-way propagation time taken for the signal to travel from the target to a base station.In the absence of disturbance, the TOA is measured at the BS i , denoted by where c is the speed of light.The range measurement based on i t in the presence of disturbance, denoted by where TDOA,i n is the range error.
The TDOA is the difference in TOAs from x signal at a pair of base stations and is often referred to as the hyperbolic system [20].This is because the time difference is converted to a constant distance difference between two reference points to define a hyperbolic curve.The intersection of two hyperbolas determines the position of the target.
The TDOA method does not require knowledge of the transmitting time from the source to the receiver, and therefore it does not need any strict clock synchronization between the source and receiver.As a result, TDOA techniques do not require additional hardware or software implementation within the mobile unit.TDOA are based on range measurements and can be obtained directly from the TOA data.Likewise, this method allows reducing common errors experienced at all receivers due to the radio channel.
In TDOA modelling, any BS can be assigned as the reference.Generally, a base station is fixed as the reference node.Now, in the case of disturbances, range measurements of TDOA can be expressed simply as

 
TDOA, , TOA, TOA, TDOA, , , , 1,2, , where . Taking 1 BS as the reference, expression (3) can be rewritten in vector form as follows where The goal of the location estimation algorithms is to find out the closest coordinates to the actual position.The position of the target should be estimated from the set of nonlinear equations (3)-( 6) constructed from the TDOA measurements and geometry knowledge of the set of BSs.

Localization algorithm based on TDOA measurements
Based on the geometry of Figure 1, the inter-receiver range vector , i j x is written as follows , 1,2, , x on Figure 1.Thus, from the law of cosines it yields where and combining equations ( 7) and ( 8), we finally obtain Similar equations can be derived for triangles   1 , , j x x x , 2,3, , j N   .Hence, the following nonlinear system equations are obtained where . In the presence of noisy measurements, the above equation is affected and the measurements error vector Note that if the number of base stations increases, equation ( 9) is over-determined.Several different approaches have been proposed for such an over determined system.In [17] and [18], improvements to the LS approach were made based on the Lagrangian multipliers optimization technique.In [17], a Linear-Corrector (LCLS) factor is added to a standard LS solution, whilst in [18], a Quadratic-Correction (QCLS) version is developed for TOA, TDOA, AOA and RSS measurements.The LCLS was applied in real-time to localize a static sound source.
Another study of static target was presented in [16] with a Constrained Weighted Least Squares (CWLS) using TDOA measurements.
In the linear-Gaussian case, the KF has a low computational complexity because only the first two moments of the probability distribution need to be stored [21,29].In addition, when nonlinear relationships are presented, the EKF can be employed.In real-time applications, like unmanned aerial, ground and underwater vehicles, computation time is an important factor in achieving the tasks.In this sense, the KF is attractive since it makes estimation recursively and is easy to implement.
The precision of the estimated position strongly depends on the noise in the radio signals and NLOS conditions.Furthermore, the UAV is an unstable system with very fast dynamics.The assumption of having a static target cannot be considered at all as in [3] and [24].Then, the localization should be precise enough to at least localize the UAV in an acceptable neighbourhood of the real position.

UAV localization
The KF is a well-known method for localization and tracking systems, but the possible drift of the estimated state is considered to be a crucial weaknesses.This drift is indeed a serious problem, especially when the nonlinear system model is based on TDOA measurements.

Kalman filter
The KF is an algorithm for efficient state estimation minimizing the covariance error [21].The basic filter is well-established and it is assumed that the transition and the observation models are linear.When the input is zero, such a model is given by where k  is the state vector at time k , k  is the state transfer matrix and k  is the observation process.The vectors k w and k v are respectively noise process and measurement vectors which are assumed to be independent with Gaussian distribution, that is, If the process system to be estimated or the measurements system is nonlinear, the EKF should be applied.For this case, a Taylor series is used to linearize the nonlinear stochastic difference equations system around a known vector ϑ.The EKF is then applied to the linearized nonlinear process as follows where ˆk  is the current estimate state vector and k A , k B , k W and k V are the Jacobian matrices are the nonlinear process and observation functions, respectively.The reader can find more details on the KF in [29] and [30].

Localization
In this part, we develop the localization algorithm for a UAV.To simplify, we will consider the following assumptions:


The workspace of the flying robot is the XY plane defined by four base stations.


The translational movements are given in manual mode.


An onboard attitude control stabilizes the orientation of the UAV. Linear motion at constant velocity is performed by the vehicle.
Under these assumptions, the localization problem reduces to estimate the position and velocity of the aircraft in a plane.Consequently, the state transfer matrix, k A , yields a 4 4  constant matrix

A
where  is the constant sampling period of the TDOA measurements.We are using a sampling period of 100 milliseconds.Therefore, the state vector, k  , only contains the UAV position and velocity components, i.e., From equation (1), note that the observed process is not linear and the function The Jacobian matrix k B for equation ( 10) is Notice that all elements of equation ( 11) have singularities if 0 , 1,2,3,4 i  .

Quadrotor Vehicle
The flying robot is a classical quadrotor which is mechanically simpler than a classical helicopter since it does not have a swashplate and it have constant pitch blades, see Figure 2. It has two counter rotating pairs of propellers arranged in a square.The onboard computer, the IMU, the location sensor board and the batteries are mounted in the middle of the square.
The thrust is provided by four Robbe Roxxy BL-Motor 2827-34 brushless motors and four YGE30i speed controllers.A homemade IMU is used to compute the attitude angles.The roll  , pitch  and yaw  angles are estimated by using a three-axis accelerometer (ADXL203) and a compass module (CMPS03).The angular rates,  and   , are measured by an integrated dual axis gyroscope (IDG-500), and the yaw angle rate,  , is measured by a single yaw rate gyroscope (ADXRS613).The altitude is determined using an ultrasonic sensor (SPF03).The onboard hardware is based on a RCM3400 analogue RabbitCore which runs at 29.4 MHz.An I2c protocol control signal is sent to the speed controllers every sampling period.The attitude dynamic is stabilized onboard using the nested saturation control [31].

Distance Measurement System
The NanoLOC development kit from Nanotron Technologies has been used to measure the distances between the BS.Nanotron Technologies has developed a WSN which can work as a Real-Time Location Systems (RTLS) [10].The distance between two wireless nodes is determined by their Symmetrical Double-Sided Two Way Ranging (SDS-TWR) technique, which allows a distance measurement by means of the signal propagation delay.It estimates the distance between two nodes by measuring the TDOA symmetrically from both sides.
Wireless communications as well as the ranging methodology SDS-TWR are integrated in a single chip, the NanoLOC TRX Transceiver [32].The transceiver operates in the ISM band of 2.4 GHz and supports location-aware applications including Location Based Services (LBS) and asset tracking applications T is the processing delay in Node A. Time instants i T are approximately four milliseconds and an accuracy smaller than one metre can be obtained [28].Furthermore, environmental conditions can affect the accuracy of the measured distances.The NanoLOC development kit contains five sensor boards with sleeve dipole omnidirectional antennas.One target board, TAG board, is mounted in the UAV and the base station boards are placed in the desired geometry.In our case, we chose a rectangular geometry (see Figures 4  and 10).
To measure the real distances between the helicopter and the base stations, a wireless link is added.This link is made by using two XBee-PRO modems that connect the helicopter to an external computer where the algorithm is executed in real-time.Figure 3 shows the real-time closed-loop localization algorithm scheme.The target board and the BS boards exchange data.Then, the TAG board sends a codified message with the measured distances via the serial communication link to a XBee-PRO modem.This last message is sent immediately to another XBee-PRO connected to a computer where the algorithm is executed in real-time.A frame data of 68 bytes is decoded to find out the four actual measured distances values The prediction and the update steps of the localization algorithm are performed at _ = 100ms.Once these tasks have completed (

P
), a one step Kalman smoother is executed to obtain

P
, see Figure 3.Even if the KF (and also other approaches) is common, the convergence is not totally ensured if initial conditions are badly conditioned.Moreover, if we want to carry out trajectory tracking tasks, it is necessary to improve the above algorithms.Notice that the UAV is an unstable system with very fast dynamics where no convergence of one state could be critical.

Experimental Results
Experiments V are considered as identity matrices.

Hover Flight Test
The workspace of the UAV is delimited by the BS placed in positions 1 BS =(0,0),  shown in Figure 6.From this graph, we can see that the estimated position remains within the confidence interval, i.e., 90% of data are around the mean value and very few values are beyond this range.Therefore, we conclude that the proposed location algorithm correctly estimates the position of the UAV in hover, with a relatively small error.
Remember that the helicopter is hovering and from Figure 7 note that the estimated speeds are close to zero in hover flight, speed must be zero or at least very close to zero The pick values ( 1 m/s) follow from the small corrections given by the pilot.

Path Following Flight Test
In this part, the experiment is devoted to evaluating the path following accuracy of the proposed localization algorithm.In order to allow sufficient movement for the UAV, a larger workspace is used.In this case, the base stations were place at points Three parts are included in this experiment, see Figure 8.The first and third parts are devoted to trajectory tracking, whereas the second one is dedicated to hover flight.Thus, from 0 t to 1 t the pilot moves the UAV following a straight line from point (1.75m, 2m) to point (6m, 5m).From Figures 9 and 11a, note that both x and ŷ are a little far from the desired path.This is because when the UAV was launched, the pilot tried to bring the UAV along the path.This fact can also be observed in the estimated speeds shown in Figure 10.The velocity magnitudes are great at the beginning which expresses the abrupt linear movements of the vehicle given by the pilot.t to 2 t , Figure 11b, the helicopter is in hover flight over the point (6m, 5m).We note that during this period, the best estimation is obtained, see also Figure 9. Finally, from 2 t to 3 t the helicopter returns to the starting point (Figure 11c).From Figures 9-11 we can conclude that for tracking, and obviously for hover, the results are satisfactory.
It is also important to point out that commonly in path following applications, the state transfer matrix Ak is derived from the accelerated motion and this kind of dynamic is not considered here.However, we note that, in our study, the estimated position is good with a relatively small error.

Conclusions
Real-time estimation of position and velocity of a UAV has been presented in this paper by using the time difference of arrival method applied to a Local Positioning System (LPS).This application is original because it offers an alternative method for localizing UAVs indoors.The LPS is composed of five transceiver cards which exchange information between them.
The obtained TDOA measurements model is nonlinear and an EKF was applied in order to estimate the states.The proposed localization algorithm has been successfully tested in closed-loop real-time experiments for hover flight and for path flight tracking.The best results were found in the hover test and this performance is related to the form of the transfer matrix.
To improve the path tracking task, the accelerated motion must be included in k A .However, with quasi-constant displacement, the algorithm continues making a good estimation of position.Finally, the experimental results have proved the good performance of the proposed algorithm with small errors.
Current research is focused on including the proposed localization algorithm into the stabilization control loop of the UAV.We are working to carry out autonomous navigation experiments with a UAV.

Figure 1 .
Figure 1.TDOA target localization scheme Now, let us consider the triangle formed by points x , 1 x and 2x on Figure1.Thus, from the law of cosines it yields

Figure 2 .
Figure 2. Quadrotor UAV . The wireless communication is based on Nanotron's patented modulation technique CSS according to the wireless draft standard IEEE 802.15.4a.SDS-TWR measures the round trip time and avoids the need to synchronize the clocks.Time measurement starts in Node A by sending a package.Node B starts its measurement when it receives this packet from Node A and stops when it sends it back to the former transmitter.When Node A receives the acknowledgment from Node B, the accumulated time values in the received packet are used to calculate the distance between the two stations.The difference between the time measured by Node A and the time measured by Node B is twice the time of signal propagation.To avoid the drawback of clock drift, the range measurement is preformed twice and symmetrically.A ranging measurement, DAB, between Node A and Node B is obtained using the following formula[28] is the propagation delay time of a round trip between Node A and Node B, 2T is the processing delay in Node B, 3 T is the propagation delay time of a round trip between Node B and Node A, and 4

Figure 4 .
Figure 4. Estimation position in hover flight have been carried out indoors with the UAV flying in manual mode.Sets of data were collected several times (over 40 seconds) to verify the correct functioning of the proposed localization algorithm in closed-loop.The covariance values of matrices R and Q were adjusted to obtain an acceptable filter response.The final values are 0 4m), see Figure4.The experiment consists of a hover flight of the UAV over the point (1.75m, 2m).Figure4show the estimated position when the algorithm is applied in real-time.Note that the UAV estimated position remains around the desired point during all experiments.Figure5illustrates the histograms for x and ŷ which are approximately Gaussian with mean values x  =1.5713m and ŷ  =2.1685m, and standard deviations x  =0.1374m and ŷ  =0.1224m.

Figure 5 .
Figure 5. Histograms for x and ŷAnother way to evaluate the performance of the proposed algorithm is through confidence intervals as

Figure 6 .
Figure 6.Variances of x and ŷ in hover flight