Tightly Coupled UWB/IMU Pose Estimation - Xsens

More documents

Recommendations

Info

7866x (m)45x (m)4161412108y (m)642020(a)(b)Fig. 5. Overview (a) and detail (b) of the trial with a test subject walking an eight-shaped trajectory. Shown are the estimated trajectory b n (–), triangulatedpositions, classified according to whether the UWB measurements are clean (+) or contain outliers (◦), and the UWB receivers (⋄). The tightly coupledapproach successfully bridges the ‘gaps’ in the triangulated positions and is not affected by outliers.12111098y (m)765324D. Sensor fusionCombining (3)–(9) we obtain a discrete-time nonlinearstate-space model with state vector(Tx = (b n ) T , (ḃn ) T , (q bn ) T , (δ b a) T , (δ b ω) T , τ, ˙τ). (10)In this paper, we use it in combination with an extendedKalman filter (EKF) [16] to fuse the TOA and inertial measurements.The EKF handles the different sample rates and avarying number of measurements straightforwardly. It runs atthe high data rate of the IMU (200 Hz) and the 50 Hz UWBupdates are only performed when measurements are available.Outliers from NLOS and/or multipath effects are detectedusing hypothesis testing on the residuals/innovations of theEKF,ɛ t = y t − ŷ t|t−1 , (11)the difference between the observed measurement y t and theone-step ahead prediction from the model ŷ t|t−1 . In absenceof errors, the residuals are normal distributed as)ɛ t ∼ N(0, C t P t|t−1 C T t + R t , (12)where P t|t−1 denotes the state covariance, C t denotes themeasurement Jacobian and R t denotes the covariance of themeasurement noise. This allows the calculation of confidenceintervals for the individual measurements and in case theseare violated, the measurement is considered an outlier and isignored.IV. EXPERIMENTAL RESULTSThe proposed system has been used to track a test subjectwalking around in a relatively large room, approximately18 × 8 × 2.5 m in size. The room is equipped with 6 synchronizedUWB receivers at known locations that are attached tothe ceiling. The sensor unit has been mounted on a foot of thetest subject, a position with relatively high dynamics. Regularoccurring NLOS conditions due to occlusion by the body — amedium with a reduced speed of light — as well as multipathfrequency0 1 2 3 4 5 6used TOA measurementsFig. 6. Histogram of the number of TOA measurements used in the EKFafter outlier rejection. Triangulation requires ≥ 5 TOAs and is only limitedlypossible.effects from signals reflected by the floor result in difficultiesduring triangulation.In this section we present the results for a 35 s trial wherethe test subject walked an eight-shaped path. Fig. 5 shows atop view of the estimated trajectory. Note that the triangulatedpositions (standalone UWB) contain many gaps as well asmany outliers. In contrast, the proposed system is capable toestimate a continuous trajectory of the test subject. The tightlycoupled fusion of UWB and inertial measurements makes itpossible to make use of any number of UWB measurementsand is hence capable to bridge the ‘gaps’ where not enoughUWB measurements are available for 3D triangulation. Furthermore,the classification of the UWB solutions in Fig. 5show that our approach successfully detects and deals withoutliers in the UWB measurements.The advantage gained by being able to utilize all availableinformation is quantified in Fig. 6. Although in theory 4 TOAmeasurements are sufficient for a 3D position solution, inpractice at least 5 measurements are required for successfultriangulation of UWB measurements, implying that more thanhalf of the available UWB measurements would have to bediscarded. This results in prolonged periods without a positionsolution where also loosely coupled UWB inertial approachesare bound to fail.The proposed system not only estimates the position of thesensor unit, but also provides very smooth orientation andvelocity estimates, shown in Fig. 7 and Fig. 8. These are very
oll (°)pitch (°)yaw (°)x (m/s)y (m/s)z (m/s)900−900 5 10 15 20 25 30 35900−900 5 10 15 20 25 30 3536018000 5 10 15 20 25 30 35time (s)Fig. 7.50Estimated orientation q nb , expressed in Euler angles.−50 5 10 15 20 25 30 3550−50 5 10 15 20 25 30 3520−20 5 10 15 20 25 30 35time (s)Fig. 8. Estimated velocity ḃn .hard or impossible to obtain using standalone UWB systems.The presented results show that tightly coupled fusion ofUWB and inertial measurements results in accurate and robusttracking. However, the estimated height is not as accurateas can be expected, see Fig. 9. The test-subject walkedon the floor, implying that heights close to 0 m are to beexpected. Especially during motion the height variation ofthe UWB solution is larger what can be expected accordingto the dilution of precision (DOP). This could indicate thatcalibration errors are present in the UWB system.V. CONCLUSIONIn this paper a 6DOF tracking algorithm is proposed estimatingboth position and orientation based on tightly coupledfusion of UWB and inertial sensors. Experiments show that arobust and accurate system is obtained even in the presenceof multipath and NLOS conditions. The system is capable tobridge periods with limited UWB measurements and successfullydetects and deals with outliers in the individual TOAmeasurements.height (m)210−10 5 10 15 20 25 30 35time (s)Fig. 9. Estimated height b n z . Shown are the estimated trajectory (–) andtriangulated positions (+,◦). The large variation in height is an indication forthe presence calibration errors.ACKNOWLEDGMENTThis work was partly supported by MC IMPULSE, a EuropeanCommission, FP7 research project and by the strategicresearch center MOVIII, funded by the Swedish Foundationfor Strategic Research, SSF.REFERENCES[1] Time Domain, Jan. 2009. [Online]. Available: http://www.timedomain.com/[2] Ubisense, Jan. 2009. [Online]. Available: http://www.ubisense.net/[3] S. Sczyslo, J. Schroeder, S. Galler, and T. Kaiser, “Hybridlocalization using UWB and inertial sensors,” in Proc. IEEEInt. Conf. Ultra-Wideband, vol. 3, Hannover, Germany, Sep.2008, pp. 89 – 92.[4] S. Pittet, V. Renaudin, B. Merminod, and M. Kasser, “UWB andMEMS based indoor navigation,” The Journal of Navigation,vol. 61, no. 3, pp. 369–384, Jul. 2008.[5] M. D. Shuster, “A survey of attitude representations,” TheJournal of the Astronautical Sciences, vol. 41, no. 4, pp. 439–517, Oct. 1993.[6] H. L. V. Trees, Detection, Estimation, and Modulation Theory,Part I. John Wiley & Sons, Ltd, 1968.[7] O. J. Woodman, “An introduction to inertial navigation,” Universityof Cambridge, Computer Laboratory, Tech. Rep. UCAM-CL-TR-696, Aug. 2007.[8] A. Chatfield, Fundamentals of High Accuracy Inertial Navigation,3rd ed. USA: American Institute of Aeronautics andAstronautics, 1997, vol. 174.[9] D. H. Titterton and J. L. Weston, Strapdown inertial navigationtechnology, ser. IEE radar, sonar, navigation and avionics series.Stevenage, UK: Peter Peregrinus Ltd., 1997.[10] S. Gezici, Z. Tian, G. Giannakis, H. Kobayashi, A. Molisch, andZ. Poor, H.V.and Sahinoglu, “Localization via ultra-widebandradios: a look at positioning aspects for future sensor networks,”IEEE Signal Process. Mag., vol. 22, no. 4, pp. 70–84, Jul. 2005.[11] P. Misra and P. Enge, Global Positioning System: Signals,Measurements, and Performance, 2nd ed. Lincoln, MA, USA:Ganga-Jamuna Press, 2006.[12] A. Sayed, A. Tarighat, and N. Khajehnouri, “Network-basedwireless location: challenges faced in developing techniques foraccurate wireless location information,” IEEE Signal Process.Mag., vol. 22, no. 4, pp. 24–40, Jul. 2005.[13] Y. Chan and K. Ho, “A simple and efficient estimator forhyperbolic location,” IEEE Trans. Signal Process., vol. 42,no. 8, pp. 1905–1915, Aug. 1994.[14] J. Nocedal and S. J. Wright, Numerical optimization. NewYork: Springer-Verlag, 2006.[15] J. D. Hol, “Pose estimation and calibration algorithms forvision and inertial sensors,” Lic. Thesis No 1379, Dept. Electr.Engineering., Linköping University, Sweden, May 2008.[16] T. Kailath, A. H. Sayed, and B. Hassibi, Linear Estimation.Prentice-Hall, Inc, 2000.
Page 1 and 2: Tightly Coupled UWB/IMU Pose Estima
Page 3: only accurate and reliable for a sh

Tightly Coupled UWB/IMU Pose Estimation - Xsens

Create successful ePaper yourself

Delete template?

Save as template?