2013 IEEE International Conference on Robotics and Automation (ICRA) Karlsruhe, Germany, May 6-10, 2013
Sensor Data Fusion in UWB-Supported Inertial Navigation Systems for Indoor Navigation Lukasz Zwirello, Xuyang Li and Thomas Zwick Institut f¨ur Hochfrequenztechnik und Elektronik (IHE) Karlsruher Institut f¨ur Technologie (KIT) Kaiserstr. 12, 76131 Karlsruhe, Germany
Christian Ascher, Sebastian Werling and Gert F. Trommer Institut f¨ur Theoretische Elektrotechnik und Systemoptimierung (ITE) Karlsruher Institut f¨ur Technologie (KIT) Kaiserstr. 12, 76131 Karlsruhe, Germany
[email protected]
[email protected]
Abstract— Indoor navigation using inertial sensors with additional radio-signal support is considered in this paper. The experimental results of data fusion between a navigation system, based on inertial measurement unit (IMU) and impulsebased UWB localization system, are presented. The IMU is additionally supported by a pedestrian step length estimations, barometer and electronic compass. The Ultra-Wideband part consists of receiver, carried by the person, and access points distributed in the scenario. The focus of the paper is put on hardware implementation and choice of the optimal data fusion technique. The presented results indicate the clear benefit of tightly coupled navigation filter, where the time differences of arrival of UWB signals are directly processed, without prior calculation of the localization solution.
inertial-navigation, to profit from their advantages in the optimal way and at the same time to balance the drawbacks. This topic will be handled in this paper with special emphasis on practical implementation. In the literature a popular option is the so-called loosely coupled integration of both systems. In this case the localization solution, using only the radio signals is calculated first, and then processed by the fusion filter as position update. This is the simplest, however not the most efficient method. Problems occur when not enough APs are reachable, hence no stand-alone radio localization solution is possible. In this case the tightly coupled integration of the sensor data is more advantageous. The authors of [2] propose an indoor tracking system, consisting of an IMU, mounted on a foot, and supported by the time difference of arrival (TDOA) Ultra-Wideband localization system. The data is merged in a tightly manner. The additional increase of the IMU accuracy is achieved by the zero velocity update, when the person is standing still. The presented experimental results demonstrate the functionality, when the IMU helps to overcome the moments with lack of UWB coverage. As the TDOA measurements are always correlated with the reference AP (explained in the next section), it is proposed in [2] to handle the clock of the mobile user (MU) as an additional unknown. This however is not possible if the clocks are drifting fast. In this work we propose to apply the decorrelation method from [3]. In [4] a very similar method is proposed, however this time with IMU system mounted on a chest. In addition to TDOA, also the angle-of-arrival (AOA) measurements are incorporated. Differently to this, we propose a processing method of UWB measurements, combined with IMU and step updates, that is based on stochastic cloning technique. This method allows the usage of pure IMU measurements, alongside with SLU, for state prediction in a complete navigation filter. In the here presented work, we follow the chest-mounted approach, where the time differences obtained with the selfdesigned UWB system, are merged with the inertial data. They are both processed in the Kalman filter. Additionally the SLU is merged with the barometer and magnetometer to bridge the periods without UWB reception.
I. INTRODUCTION In the times, where the constant location awareness outdoors has become an obvious thing, mainly thanks to Global Positioning System, this is still not the case indoors. The GPS signal is too distorted and anyway to imprecise in order to provide the accuracy much better than 1 m, which is required for even the simplest indoor applications. For this reason, the indoor navigation has been of high interest in the last decade; especially in context of navigating people through buildings or machines within large industrial scenarios. The knowledge of own position in those cases is essential and could allow a whole new range of applications. In the recent years several systems, based on different technologies, such as e.g. ultrasound or laser/infrared, have been investigated in this context. It was shown, that in indoor scenarios the radiobased localization systems can achieve accuracy, laying even in the lower centimeter region, with the solution being longterm stable [1]. One of those technologies is impulse-based Ultra-Wideband (UWB). All radio technologies have however one severe drawback: the number of access points (APs), which is required for a coverage allowing reliable operation, is high. On the other hand, the tempting perspective of using Inertial Measurement Unit (IMU) for navigating through the scenario, without the need to rely on any infrastructure, is not so simple to implement either. The pure ”inertial” navigation, based on an IMU and additional sensor, such as magnetic compass and a barometric sensor, using also e.g. step length updates (SLU), is subject to drift with time. The interesting approach would be to merge the radio- and 978-1-4673-5643-5/13/$31.00 ©2013 IEEE
3154
Fig. 1. Torso-mounted IMU-based IPNS, with integrated barometer and magnetometer.
use a constant step length and combine it with the compass heading to a polygon of 2D points. IMU acceleration and gyro data are used for leveling and integrity monitoring. In a navigation filter, the polygon can be used as a Step Length Update (SLU). For a correct treatment of these delta position updates, we propose to use stochastic cloning [5]. This is advisable to get a correct estimation of the position uncertainty, so that absolute positioning updates (like e.g. UWB) can improve the solution according to their measurement noise. Stochastic cloning for the 2D position vector (~ χ) is applied: these states are cloned (~ χc ) after each user foot step. Also the uncertainty is cloned and the correlation between χ ~ and χ ~ c is set to 1 in the uncertainty matrix. During a new step, the cloned state is not updated, as can be seen in the following equations, only the normal position state is updated by Φ and with IMU measurements. χ ~ k+1 Φ 0 χ ~k Gk = + w ~ k. (1) χ ~ ck+1 0 I χ ~ ck 0 In the measurement step of the Kalman filter, the delta measurement, also called SLU, can be applied. A mapping matrix H can be defined, so that the delta measurement is the difference between χ ~ and χ ~ c: 1 0 . . . | −1 0 Hk = . (2) 0 1 . . . | 0 −1 Finally, the uncertainty is growing as desired. For more details, see [5]. In Figure 2, the growing position uncertainty is plotted due to the stochastic cloning technique. With this stochastically optimal estimation of position uncertainty, additional ranging sensors, such as UWB, can be added in an optimal way.
1.5 uncertainty north in m
II. HYBRID UWB-IPNS HARDWARE PLATFORM A. Integrated Pedestrian Navigation System In [2] a dead reckoning approach for torso mounted pedestrian navigation systems is presented. The step length is estimated and combined with the heading information of a magnetic compass. Similar to that approach and with further improvements concerning robustness of IMU measurements, we have developed a multi-sensor pedestrian navigation system with IMU, barometer and magnetic compass, as presented in Fig. 1. Following the dead reckoning approach, we
1.4 1.3 1.2 1.1 Xn
1 0
10
20
30 40 time in s
50
60
70
Fig. 2. Position uncertainty of the inertial system, due to the stochastic cloning technique.
Fig. 3. Schematic diagram of the UWB localization principle, based on TDOA measurements. Clock distribution system triggers the APs sequentailly, through different length cables (dN delay). The MU measures the relative signal delays τN corresponding to the AP-MU distances.
B. Impulse-based TDOA Localization The Ultra-Wideband is a wireless transmission technology, which allows signals to occupy large instantaneous bandwidth (absolute BW >500 MHz) and such systems can be operated without license. The most popular form of UWB is the impulse-based version (IR-UWB), where the large bandwidth is achieved by using sub-nanosecond electromagnetic impulses. Short pulses have a major advantage, compared to the carrier-based signals, of being immune against multipath fading and so offering more reliable radio coverage. This is of high interest, especially in multipath-rich scenario, such as buildings and industrial environments. Other interesting feature of short pulses is their time-resolution, making them a good candidate for precise distance measurements; hence also for localization system. However, due to the large BW, the restrictions are put on the emission power, in order not to disturb coexisting radio-systems, which limits the operation range to ≈15 m (depending on the receiver sensitivity) [6], where signal detection after propagation through wall is hardly likely. This characteristic predestines the UWB for use as a pico-cell sized (≈30 m2 ) localization system, where larger ares can be covered by interconnecting the cells [7]. The basic idea behind the UWB positioning system is based on precise time measurements between a mobile user (MU) and N known reference nodes (AP, access point), distributed in the scenario. This is presented in Fig. 3. As the radio channel is reciprocal, this system can be realized in either way: APs as transmitter and MU as receiver (the case in this work), or vice versa. The APs are controlled from a central unit (not synchronized with the MU), which triggers them sequentially (the trigger signal can additionally include coded information for the MU). AP transmit the
3155
UWB pulses, which then undergo additional delays in the channel and are then received by the MU. The position information is contained in the mutual delays (TDOAs) between the received UWB pulses. In general the localization accuracy/precision depend on following parameters: • calibration of the synchronization cable lengths dN , • knowledge about exact position of the APs, • quality of the TDOA measurements, • geometrical alignment of APs and MU. In the designed system one AP (in this case the 1st ) acts as a reference, and The TDOAs are calculated relative to the first received pulse (τ1 = 0): ∆τ1N = (dN + τN ) − (d1 + τ1 )
TABLE I
Parameter Synchronization / position update rate TDOA averaging Total weight of the BN Battery operation time Overall power consumption UWB/IPNS power consumption
Value 10 Hz 100-fold 9.5 kg 1h ≈40 W ≈2 W
improved by implementing the functionality of the PC and FPGA e.g. on an ASIC. D. Integration of both systems
LOCALIZATION SYSTEM PARAMETERS
Parameter Tx pulse bandwidth Center frequency of the pulse Tx pulse amplitude Pulse repetition rate Range of dN delays Receiver type
TABLE II I NTEGRATED SYSTEM PARAMETERS
(3)
Rest of the TDOAs is also calculated by applying (3). The precise measurements of the separations between received signals are carried out by the time-to-digital converter [9], which is capable of measuring the TDOAs down to ≈25 ps quantization steps. This time resolution, together with the spatial orientation of the MU and APs, described by the geometrical dilution of precision, allows this system to achieve an absolute accuracy better than 10 cm in indoor scenarios of this size [8]. The relevant parameters of the UWB
Fig. 4. Interconnection scheme of IMU and UWB system, carried by the MU. Elements within the grey-dashed box are mounted on a backpackplatform. Black arrows indicate synchronization by trigger signals.
Value 7 GHz ≈6.8 GHz 2 Vpp 1 MHz 20-250 ns non-coherent
desinged system are summarized in table I. Due to the relative time measurements a minimum number of 3 APs are required for 2D localization (2 TDOAs); and one AP more to achieve a unique 3D position. Due to those facts the amount of APs, required for achieving a full coverage in a multi-cell scenario (e.g. several rooms), would be very high. This is the starting point for considerations regarding the integration of this system with the IPNS and methods of data fusion between them.
Basically there exist two approaches to conduct the integration of both systems, at the data-level: either a looselycoupled integration can be performed, where a UWB positioning solution is calculated using the TDOAs, and then is used as a position update in the Kalman-filter (KF). This approach requires however a good radio-coverage, as 3 APs is the minimum needed for the 2D (pure UWB) positioning. Second method is the tightly-coupled-integration, where the TDOAs are processed by the KF, without calculating the sole-UWB solution. Here already one time-difference can improve the solution. Loosely- as well as tightly-navigation filters were implemented here as 15 state Error State Kalman Filter. The filter states are: position, velocity, attitude and sensor biases of gyroscope and accelerometer.
C. Synchronous IPNS-UWB measurements In Fig. 4 the interconnection scheme of the UWB and IPNS is depicted. As both systems were mounted on a wearable platform, we call it the Backpack Navigator (BN). Both, the UWB and the IPNS, systems are synchronized using a clock, which was implemented on a FPGA, and the measurement data is then collected by the PC for further processing. The most important parameters of the integrated system are summarized in table II. Fig. 5 shows a person wearing the BN. The UWB antenna is placed on the head-level (≈1.8 m), the computer is a commercial PC in micro-ATX casing and the DC power is supplied by the motorcycle battery. The total weight and power consumption of the BN could be greatly
Fig. 5. Left: mounting of the BN, with the IPNS attached to the torso, UWB antenna placed on the head-height and signal processing + battery on the back. Right: a person wearing the BN.
3156
0
20
40
time in s 60 80
100
120
0
8
8
7
7
6
40
time in s 60 80
100
120
6
2
5
2
5 3
south − north in m
south − north in m
20
4 3 2 1
3 4 3 2 1 1
1 0
0 groundtruth strapdown + corrections valid UWB positions UWB APs
−1 −2 −2
0
Fig. 6.
2 4 west − east in m
6
groundtruth strapdown + corrections valid UWB positions UWB APs
−1 −2 −2
8
0
2 4 west − east in m
6
8
Left: Tightly Coupled integration, AP 2 outage: 30 s–90 s. Right: Loosely Coupled integration, AP 2 outage: 30 s–90 s.
Due to the time difference approach a decorrelated KF measurement update is implemented, as all TDOAs are correlated with the noise of the reference AP. To overcome the non linear nature of the local triangulation, an iterative KF is implemented. Here, up to four iterations are performed to find a good linearization point. Lastly, in order to remove outliers, an innovation based integrity monitoring can be activated. Based on the prediction of the navigation filter the time-differences are discarded, if the residual between measurement and prediction exceeds the predefined confidence interval [3]. III. EXPERIMENTAL RESULTS In this section, the results of the UWB/INS integration are presented. Measurement data was acquired in a room, equipped with 3 UWB access points, that are indicated as red dots in the plots. No UWB APs were placed in the corridor. The localization was performed in 2D, hence the height of the person was assumed to be known at the beginning. The AP nr. 1 was used as reference for time-difference calculations. AP 1 and 2 were mounted on the window and AP 3 was fixed to a shelf. The UWB stand-alone solutions are marked with crosses in the map. The ground truth of the trajectory is given by 10 measurement points, which in the plots are interconnected with lines (dashed magenta). The person with the BN, walked during the measurements not along the lines, but rather only on the edge points. The color of the trajectory and UWB localization solution is changing over time, according to the legend. Step length updates are used to support the inertial navigation
filter solution with delta position measurements. For a correct handling of the uncertainty in the navigation filter, the stochastic cloning technique is used. When the UWB TDOA measurements are available, the drift of the solution can be eliminated thanks to an absolute position measurement. The comparison of tightly and loosely coupling is visualized in Fig. 6. In the first phase of the measurements all 3 APs are transmitting and the TDOA measurements are calculated relative to AP1. The tightly coupled (TC) navigation algorithm provides a similar navigation solution as the loosely coupled (LC) implementation. The main difference is the longer transient phase of the TC at the beginning. In case of a simulated AP-outage, Tightly Coupled presents its full potential. AP 2 was turned off between second 30 and 90. Only two APs were active during that time and a calculation of the position wasn’t possible with the pure UWB measurements. As a result, the LC update in the navigation filter cannot be evaluated and the trajectory drifts towards north. After 60 seconds, it is corrected, as soon the UWB update is available again. This fast correction is possible due to a correct estimation of the uncertainty by the presented stochastic cloning technique: the uncertainty after 60 s outage is several meters, so that the position update in the navigation filter improves the position immediately. In contrast to the LC, the TC approach benefits from every measurement, even if only one TDOA is available. In the present case, during the phase where AP 2 is not available, only the TDOA measurement between AP 1 and 3 was used for a correction, perpendicular to the time-difference hyperbola. As the person is moving on a circle, the direction
3157
0
20
40
time in s 60 80
100
120
0
8
8
7
7
6
time in s 60 80
40
100
120
6
2
5
2
5 3
south − north in m
4 3 2 1
3 4 3 2 1
1
1
0
0 groundtruth strapdown + corrections valid UWB positions UWB APs
−1 −2 −2
0
Fig. 7.
2 4 west − east in m
6
groundtruth strapdown + corrections UWB APs
−1 −2 −2
8
0
2 4 west − east in m
6
8
Left: Tightly Coupled integration, AP 2 outage: entire time. Right: Loosely Coupled integration, AP 2 outage: entire time.
of the corrections is changing over time. Although the result is not quite as good as with three APs, the 2D correction is done and the solution does not drift away.
10 6
8
4
3
south − north in m
7
In Fig. 8 the results of a longer walk are presented, where the user was leaving the UWB equipped room several times. The results in case of LC and TC are very similar. The difference is present in the door-region, where only one TDOA measurement can be estimated (because the AP 3 is shadowed). Like this, no fix position can be calculated (no UWB update is available) and the loosely coupled solution shows the wrong solution when passing the door. As a result the loosely coupled solution is displaced to north when passing the door. In the corridor, where no UWB measurements are possible, the accuracy of the navigation solution is based on the SLU implementation, so both solutions are very similar. Fig. 9 presents the horizontal dilution of precision (HDOP) in the scenario. It is obvious that outside of the room, even if measurements were available, the TDOA measurements won’t help much,as a change in the MU position doesn’t yield a similar change in the TDOA measurement. When equipping buildings with ranging systems, this surely must be taken into account [8].
9
2
If AP 2 is missing the entire time, (Fig. 7) this effect is even more obvious. The Loosely Coupled solution is only a pure strapdown + SLU solution, which slowly drifts away. The tightly coupled approach, despite the transient phase and one outlier, still profits even from only one valid TDOA.
2
6 1 5
0 4 −2
HDOP value
south − north in m
20
3 2
−4 1 −6 −4
Fig. 9.
−2
0 2 4 west − east in m
6
8
0
Horizontal dilution of precision (HDOP) for the area.
IV. CONCLUSIONS In this paper we presented the multi-sensor indoor navigation system, consisting of INS and UWB sensors, and investigated two possible data fusion techniques. The data fusion techniques are introduced, which can both be supported by step length updates. A correct processing of these
3158
time in s 0
50
time in s 100
150
6
0
50
2 3
4
south − north in m
4
south − north in m
150
6
2 3
2
1
0
−2
2
1 0
−2
groundtruth strapdown + corrections valid UWB positions UWB APs
−4
−6 −4
100
−2
0
2 4 west − east in m
6
groundtruth strapdown + corrections valid UWB positions UWB APs
−4
8
−6 −4
−2
0
2 4 west − east in m
6
8
Fig. 8. Left: Tightly Coupled, after transient time, the performance is better than loosely because of measurements in the door area available. Right: Loosely Coupled, no measurements outside the room, door area slightly drifting as no UWB position updates are available, transient phase at the first round of the trajectory.
measurements was presented by employing the stochastic cloning technique. The navigation filter, in tightly and loosely version, was tested with the real data, which were obtained by means of IMU/UWB measurement campaign. For measurements a self developed Backpack-Navigator was used, that was supported by the relative distance measurements to the UWB access points. The data was acquired in a typical indoor scenario. The results those two data integration methods are presented graphically and analyzed. After a transient phase, the tightly coupled navigation filter exhibits better performance, especially in situations when the UWB stand-alone localization solution cannot be calculated. This is so, thanks to the fact that every TDOA is processed separately in the tightly coupled implementation. The achieved results coincide very well with the simulations presented in the previous work. The integrated system achieves the decimeter-level accuracy and is long-term stable. Currently we work on the quantitative evaluation and the implementation of a vehicle with odometry. In the future the measurement campaign in a larger area, equipped with radio transmitters, is planned. R EFERENCES [1] J. Xu, M. Ma, and C.L. Law: Performance of time-difference-ofarrival ultra wideband indoor localisation, Science, Measurement and Technology, IET , vol.5, no.2, pp.46–53, Mar. 2011, doi: 10.1049/ietsmt.2010.0051.
3159
[2] J. D. Hol, F. Dijkstra, H. Luinge and T. B. Schoen: Tightly Coupled UWB/IMU Pose Estimation, IEEE International Conference on UltraWideband, pp.688-692, 9–11 Sep. 2009. [3] C. Ascher, L. Zwirello, T. Zwick and G. Trommer: Integrity monitoring for UWB/INS tightly coupled pedestrian indoor scenarios, International Conference on Indoor Positioning and Indoor Navigation (IPIN), pp.1–6, 21–23 Sept. 2011. [4] S. Pittet, V. Renaudin, B. Merminod and M. Kasser: UWB and MEMS Based Indoor Navigation, The Journal of Navigation (2008), The Royal Institute of Navigation. [5] M. Kleinert, C. Ascher, S. Schleith, G. Trommer, and U. Stilla: A new pedestrian navigation algorithm based on the stochastic cloning technique for kalman filtering, Proceedings of the 2011 International Technical Meeting of the Institute of Navigation, San Diego, CA, 2011. [6] J. Xia, C.L. Law, Y. Zhou and K.S. Koh: 3?5 GHz UWB Impulse Radio Transmitter and Receiver MMIC Optimized for Long Range Precision Wireless Sensor Networks, IEEE Transactions on Microwave Theory and Techniques, vol.58, no.12, pp.4040-4051, Dec. 2010. [7] P. Sharma, S. Krishnan and G. Zhang: A Multi-cell UWB-IR Localization System for Robot Navigation, IEEE Radio and Wireless Symposium, pp.851–854, Jan. 2008. [8] L. Zwirello, T. Schipper, M. Harter, and T. Zwick: UWB Localization System for Indoor Applications: Concept, Realization and Analysis, Journal of Electrical and Computer Engineering, vol. 2012, Article ID 849638, 11 pages, 2012. doi: 10.1155/2012/849638. [9] acam-Messelectronic GmbH: TDC-GPX: Ultra-high Performance 8 Channel Time-to-Digital Converter, Jan. 2007 www.acam.de/fileadmin/Download/pdf/English/DB GPX e.pdf
