WPI Precision Personnel Locator System: Inertial Navigation Supplementation

Transcription

1 WPI Precision Personne Locator System: Inertia Navigation Suppementation V. Amendoare, D. Cyganski, R. J. Duckworth, S. Makarov, J. Coyne, H. Daempfing, B. Woodacre Worcester Poytechnic Institute, Worcester, Mass., USA BIOGRAPHY Mr. Vincent Amendoare is a Ph.D. candidate in Eectrica and Computer Engineering at WPI. Since competing his BS and MS degrees at WPI in and 7, he has served as a research assistant in the WPI Convergent Technoogy Center sponsored by the NIJ/DOJ on the topic of precision personne ocation. His MS thesis determined techniques to attain synchronization necessary for the WPI precision personne ocation system, and his Ph.D. research concerns improving the performance of this system with inertia navigation suppementation. Dr. David Cyganski is professor of Eectrica and Computer Engineering at WPI where he performs research and teaches in the areas of inear and non-inear mutidimensiona signa processing, communications and computer networks, and supervises the WPI Convergent Technoogy Center. He is an active researcher in the areas of radar imaging, automatic target recognition, machine vision and protocos for computer networks. He is coauthor of the book Information Technoogy: Inside and Outside. Prior to joining the facuty at WPI he was an MTS at Be Laboratories and has since hed the administrative positions of Vice President of Information Systems and Vice Provost at WPI. Dr. R. James Duckworth is an Associate Professor in the Eectrica and Computer Engineering department at WPI. He obtained his Ph.D. in parae processing from the University of Nottingham in Engand. He joined WPI in 197. Duckworth teaches undergraduate and graduate course in computer engineering focusing on microprocessor and digita system design, incuding using VHDL and Veriog for synthesis and modeing. His main research area is embedded system design. He is a senior member of the IEEE, and a member of the ION, IEE, and BCS and is a Chartered Engineer of the Engineering Counci of the UK. ABSTRACT This paper describes the atest deveopments in the Worcester Poytechnic Institute (WPI) Precision Personne Location (PPL) system. An RF-based system with inertia sensor suppementation is being deveoped for tracking of first responders and other personne in indoor environments. The system assumes no existing infrastructure, no pre-characterization of the area of operation and is designed for spectra compiance and rapid depoyment. The RF 3D ocation system has previousy demonstrated sub-meter positioning accuracy of a transmitter even in difficut indoor environments with high muti-path with a receivers outside the buiding. However, accuracy can be severey degraded at particuar ocations within a buiding such as when a arge and nearby meta pane bocks the direct path from the mobie transmitter to severa receiving antennas. This paper describes the performance of our existing RF ocation system s raw position estimate performance and how these raw position estimates may be fitered with motion constraints via the Kaman fiter for improved accuracy. The paper goes on to investigate the use of inertia sensors to suppement the information from our RF position estimates, and how this information is fused together to provide improved overa system performance. I. INTRODUCTION Previous papers by the authors [1-] have described the genera system architecture, precision ocation approach, hardware design chaenges, signa structure, and experimenta resuts paraeing the deveopment and refinement of the system s hardware reaization. Fig. 1 is an artist s rendition of the PPL concept. The goa of the system is to provide a robust rea-time tracking soution that requires no pre-instaed infrastructure. To be tracked, firefighters and other emergency personne each carry a transmitter emitting a muti-carrier wideband (MC-WB) signa [], which is sensed at receiving

2 1 Tx Rx Error 1 Fig.. Campus Ministry: Mean Absoute Error -.9m Fig. 1. PPL concept iustration TABLE I LOCATION TESTING MEAN ABSOLUTE ERRORS Test Location Error Bandwidth Kaven Ha.37 m MHz Atwater Kent 1. m MHz Campus Ministry 1st f..9 m MHz Campus Ministry nd f..7 m MHz Campus Ministry 1st f..9 m 1 MHz Campus Ministry nd f.. m 1 MHz stations fixed upon emergency response vehices. Upon arriva, the receiving stations form an ad-hoc network and estabish a oca coordinate system. Given the receiving stations positions within the oca coordinate system, the signas coected at each station are transferred to a centra ocation where the reative positions of the transmitters within the coordinate system are estimated. These estimates are then conveyed to command-and-contro software in the hands of the incident commander. Accumuation of track information for many transmitters has the effect of providing the incident commander with vauabe information on buiding ayout to assist with exit guidance or search and rescue operations. Efforts are aso underway to integrate additiona information into the incident commander s dispay to enabe monitoring of personne physioogica status and ambient environmenta conditions. II. EXPERIMENTAL LOCATION TESTS Pubications [] by the authors have presented ocation testing resuts from a MHz RF system and from a new 1 MHz wideband system [1]. The resuts of these tests are summarized here in Tabe I; a resuts invove D ocation of a transmitter inside the buiding by antennas paced outside of it. The Kaven Ha test ocation comprises brick and stee-beam construction and houses a geotechnica ab on the WPI campus; the Atwater Kent ocation was a arger scae test centered around the wing of a arge brick buiding, passing through stee-studded was and under meta-corrugated ceiings; and the Campus Ministry ocation is a typica woodconstruction three-story residentia structure compete with furniture and meta appiances in the kitchen. The errors in Tabe I provide a reference for the genera accuracy of our system. Fig. dispays a resut from the Campus Ministry test on the first foor with 1 MHz of bandwidth. The circes represent our receive antenna (Rx) ocations outside the house, whie the squares represent severa transmitter (Tx) ocations at which the transmitter was truy positioned. At each ocation the transmitter was stationary and it s position was estimated using the received RF signas at the receive antennas. The pot shows error vectors which point from the true ocation of the transmitter to the estimated position. We see that in many cases the error is quite sma, however some ocations are more chaenged due to direct path bockage and refections. The positioning agorithm used in our system is computationay intensive and takes approximatey.3 May, Monterey, CA

3 1 Fig. 3. Raw Est. 3 7 Campus Ministry - Continuous Capture seconds to compute a position estimate with our current microprocessor based hardware. It is possibe however for us to capture signa data at a faster rate continuousy and post-process it. During a recent test at the Campus Ministry we performed a continuous data coection at a rate of approximatey 3 captures per second whie the transmitter was moved around the first foor of the house. The resut of this data set is shown in Fig. 3. The transmitter was moved through severa surveyed truth ocations depicted by bue circes. The bue ine is a spine curve passing through the truth ocations that approximatey describes the true path of the transmitter. Note that the transmitter was brought to a compete stop at the stop ocations (depicted by bue squares). This resut provides great insight into how our position estimates change as the transmitter is moved. We see that generay it foows the true path. There are apparenty two types of errors. There are highy ocaized arge deviations from the truth path; fortunatey these are often ony present for isoated sampes so they can be more easiy rejected in a tracking impementation. There are aso consistent deviations from the true path that ast for severa sampes. Fortunatey these are reativey sma (ess than 1 meter) and return shorty to the truth path. This same data set wi be used throughout this paper for comparison of different tracking approaches. Based upon motion constraints of the person we wish to track, these raw soutions can be fitered to obtain a more precise tracking estimate. III. TRACKING WITH KALMAN FILTER Our system can be modeed as a discrete time inear system with state variabes in a vector x k, where k is a discrete time index. The states in our system are the position, veocity and acceeration coumn vectors of the tracked personne denoted p, v, a respectivey. x = p v a (1) The vaue of x k+1, the next state is a function of the current states and a process noise term w k. This process noise term is a random vector, which modes the changes in states of the tracked personne as a random process. x k+1 = Ax k + w k, () where the A is the state transition matrix that describes the dynamics of the state variabes. For our case I 3 τi 3 3 A = 3 I 3 τi 3, (3) 3 3 I 3 where τ is the eapsed time between k and k + 1. This state transition matrix derived from the fact that veocity is the derivative of position, and acceeration is the derivative of veocity. Our system provides raw position estimates which can be represented as y k = Cx k + v k () where C is caed the observation matrix and v is the measurement noise. For our case since our system measures position C simpy masks out the veocity and acceeratoin [ ] C = I () The errors in our raw position measurements are modeed by v. This system mode we have deveoped is the system mode for a discrete time Kaman fiter [7]. This fitering technique is a common soution to the probem 3 May, Monterey, CA

4 of tracking with noisy estimates. The Kaman fiter is optima for estimating the parameters of inear systems with Gaussian noise. The noise in our system may not be truy Gaussian, but it may be approximated as such. For our system we assume that the process and measurement noise both have zero mean. The covariance of each of these noise sources must be known. The Kaman fiter agorithm has two phases, prediction and update []. The prediction phase makes a prediction of the state variabes from the state estimate ˆx k 1 of the previous time step. x k = Aˆx k 1 () The Kaman fiter not ony estimates the state variabes, but it aso keeps track of the covariance of the state variabes. The Kaman fiter treats the state variabes as a mutivariate Gaussian with a covariance matrix P k. The prediction phase aso invoves the prediction of the covariance matrix based upon the estimate from the previous time step ˆP k 1. P k = AˆP k 1 A T + Q (7) where Q is the covariance matrix of the process noise. The second phase of the Kaman fiter agorithm is the update phase []. First the Kaman gain K k is computed Fig.. Raw Est.:.3m Fit. Est.:.1m Raw estimates and fitered estimates K k = P k C (CP k CT + R) 1 () where R is the covariance matrix of the measurement noise. The Kaman gain takes an important roe in the next step, where it acts as a reative weight between the predicted state x k and the actua measurement z k to produce the fitered measurement ˆx k. ( ) ˆx k = x k + K k z k Cx k (9) Simiary the covariance matrix P k is estimated. ˆP k = (I K k C) P k (1) This process is repeated for each new measurement. In our system we use the discrete time Kaman fiter to improve our tracking performance based on estimates of our position estimate noise variance and personne motion process variance. This method of tracking based on position estimates wi be denoted as the position ony Kaman fiter agorithm. The primary difference between our impementation and the cassic Kaman fiter described above is outier rejection ogic intended to ignore arge, isoated errors. Fig. shows a set of raw position estimates from our RF system as we as the fitered version of this data. The sampe rate for this data was reduced to approximatey 3 updates per second to repicate reaistic timing for our rea-time positioning hardware. The error reative to the approximate truth path was cacuated before fitering to be.3 meters and after fitering to be.1 meters. This is a significant improvement and yieds our desired accuracy of better than one foot. We are sti interested in techniques for further improvement because we expect our raw position estimates to be worse in more chaenging mutipath environments, hence the next pursuit of inertia suppementation to be discussed beow. IV. INERTIAL SUPPLEMENTATION This paper investigates the potentia benefit of suppementing our existing RF based positioning system with inertia sensors to improve overa performance. Recent work has been aimed towards proof of this concept. We have been working with the nimu (nano Inertia Measurement Unit) created by Memsense. This unit contains a three axis acceerometer, three axis gyroscope and three axis magnetometer with measurements at a rate of 1Hz in a sma form factor [9]. For our testing this unit was boxed and attached to a cart aong with our RF May, Monterey, CA

5 Ang. Rate [deg/s] 1 yaw pitch ro Time [s] Ang. [deg] 1 yaw pitch ro Time [s] Fig.. Fig.. Orientation Tracking by an orthogona rotation matrix Rc. This matrix wi map any vector ac in the cart coordinate system to it s equivaent vector in the oca coordinate system a. Locator Cart a = Rc ac ocator and a aptop for nimu data ogging, as shown in Fig.. One significant issue with inertia measurement units is caibration. Whie inertia measurements are generay considered to have Gaussian noise, they aso may have deterministic errors. The most prominent of these are biases and incorrect scae factors. Our device was characterized by the manufacturer to determine these vaues in order to caibrate the device. Temperature can affect these vaues as we, and athough the nimu features interna temperature compensation [9], its biases and scaing vaues can sti change consideraby. Ideay the device woud be fuy caibrated immediatey before tracking, however an eaborate caibration procedure is not practica for our system in the fied. We empoy a simpe caibration procedure, hoding the device stationary for a few seconds, before tracking begins. This aows us to determine any gyroscope bias and aso estimate the initia pitch and ro of the device from the acceerometer measurements. Before an attempt was made to integrate inertia and RF position estimates we observed how we we coud track the unit with inertia data aone. The agorithm used to do this is based upon the Kaman fiter impementation described previousy. First the orientation of the cart (which we wi ca the cart frame) reative to our system s oca coordinate system must be tracked. This reative orientation is a rotation in three dimensions, which can be described (11) This rotation matrix can parameterized by three Euer anges. For our impementation we use the aeronautics convention of yaw, pitch and ro. Which refer to positive rotations about the z, y and x axis sequentiay [1]. As the orientation of the cart changes over time, this rotation matrix changes. The derivative of this matrix can be expressed as R c (t) = Rc (t)ωc (1) where Ωc is the skew symmetric representation of the three anguar rates ωcx ωcy ωcz, the anguar rates about the three axes of the cart measured by the gyroscopes [1]. ωc z ωc y (13) Ωc = ωc z ωc x ωcy ωcx In discrete time, the orientation Rc,k can be estimated from the previous estimate and the current gyroscope measurement. Rc,k = Rc,k 1 I3 + Ωc,k τ I3 Ωc,k τ 1 [1] (1) This is currenty being done with our three axis gyroscope measurements. This is certainy not a vaid ong term soution as it is tracking based on anguar rate, open oop; however for the short data captures we have been working with thus far (1- minutes) it has proven to be quite accurate. Fig. shows the orientation May, Monterey, CA

6 tracking for the same data set we have been treating (in Fig. 3 and Fig. ). As the cart was wheeed about the foor, it remained fat on the foor for the most part, thus its pitch and ro remained fairy constant. It s yaw however varied consideraby as shown in Fig.. There was no way for us to measure our true orientation over time for comparison with our estimated orientation. We wi soon observe however that using this orientation information for inertia tracking can yied quite accurate resuts, impying that our orientation is being we tracked. The acceerometers in the nimu measure the motion induced acceeration of the device summed with the acceeration of gravity. Our oca coordinate system is chosen such that gravity is in the z direction. The acceeration due to gravity can be represented in the oca coordinate system as g = g (1) where g is the magnitude of acceeration due to gravity (approximatey 9. m s ). The measurement of the acceerometer in the cart frame f c is Fig. 7. Inertia Tracking Fit. Inert.: 11.m f c = R ct (a + g ) (1) where a is the acceeration of the object in the oca coordinate system. Thus a can be estimated at discrete time index k â,k = R c,kf c,k g (17) These oca frame acceeration estimates can be processed by our Kaman fiter if we change our observation matrix to [ ] C = I 3 3 I 3. (1) Or if we have acceeration measurements and no position measurements [ ] C = 3 3 I 3. (19) For our impementation we have inertia measurements at 1Hz and position estimates at approximatey -3Hz, so our observation matrix changes depending on which measurements are present. Fig. 7 shows the outcome of tracking our data set with inertia information aone. We see that our estimated path quicky diverges from the true path. A sma bias in our anguar rate measurements causes our estimated orientation to deviate from our true orientation. Thus when we attempt to subtract the effect of gravity it wi not be canceed perfecty and eave some residue. This residue acts as a bias on our acceeration measurements (in the oca coordinate frame), which quicky yieds a arge position error. One common and powerfu technique to curb the error growth in inertia navigation systems is to use zero veocity updates (ZUPTs).[11] If the unit is known to have zero veocity at some moment in time this information can be used to reset some of the growing errors in the Kaman fiter. First, the veocity state in the Kaman fiter may be set to zero. Second, between two zero veocity update times the average acceeration in the oca frame must be zero. To take fu advantage of this fact, one must wait for a ZUPT to repair the inertia data from before that ZUPT and use it for tracking. This is acceptabe if the time between ZUPTs is short and some ag in the tracking soution is acceptabe. For the anaysis in this paper it has been assumed that this ag is acceptabe. As our system is intended to track personne it is possibe for us to obtain these ZUPTs if our unit is paced on a part of the body that stops frequenty such as the foot. Reca for our data set the cart was brought to a compete stop at the ocations depicted on the truth path. May, Monterey, CA

7 Inert. w/zupt:.19m 1 Pos.+ Inert.:.1m Fig.. Inertia Tracking with ZUPTs Fig. 9. Tracking with Inertia and RF Position Estimates We can take advantage of these with ZUPTs to improve our tracking performance dramaticay. Fig. shows the tracking performance with inertia information and ZUPTs. We see that the error is now.19m, comparabe to our position ony tracking resut. The data set was taken over a short enough period of time that this technique resuted in estimates which did not deviate significanty from the true path. This is certainy not a viabe ong term soution however as it is an open oop inertia tracker and the path estimate wi thus utimatey diverge due to sensor bias. The goa of these investigations is to use both the position estimates from our RF system as we as inertia information to create an accurate ong term stabe tracking system. Since the inertia tracker is quite accurate in the short term we can appy a sma weight on information from our RF position estimates in our Kaman fiter. Our RF position estimates may have arge errors, but as ong as they don t tend towards a certain direction (that is, as ong as they have zero mean) they wi keep the inertia tracker from sowy drifting away from the proper path. Using this fusion of data in our Kaman fiter we achieve the resut shown in Fig. 9. We obtain the smaest error seen thus far for this data set at.1m. Whie this is not dramaticay smaer than our position ony tracker, it does show inertia information can be used to improve the RF tracking whie providing RF based ameioration of inertia drift that woud eventuay accumuate and then rapidy degrade position information. In more chaenging environments such as arger buidings we expect our RF position estimates to present arger errors if empoyed aone, whie the inertiay suppemented soutions wi be substantiay unaffected. V. FUTURE WORK The idea of an inertia suppementation to our RF system seems promising at this time but more work is required to bring it to fu fruition. Future research wi focus on the severa areas of effort described beow. We sti need to cose the oop on our orientation tracking, as currenty we are ony using gyroscope measurements. The most common soution to this probem is to use the direction of measured gravity as a pitch and ro reference and magnetometer measurements as a yaw reference. We are investigating means to use our RF information to provide more reiabe information for this purpose. This wi ikey invove a soution with couping 7 May, Monterey, CA

8 between orientation tracking and position tracking such as an extended Kaman fiter. [1] We wish to evauate our tracking performance in more difficut RF environments, as we as with tracking over a onger period of time. Resuts in this paper were obtained by postprocessing. Utimatey the inertia suppementation wi need to be integrated into our rea-time tracking system. As the inertia unit must be worn by the personne to be tracked, the recorded data must be sent wireessy to our signa processing station. VI. CONCLUSIONS This paper has expored the proposed idea of improving WPI s existing RF-based Precision Personne Location system by adding information from an inertia measurement unit. We have shown that indeed such information can improve the tracking performance of our system compared to tracking with RF position estimates aone. ACKNOWLEDGMENT The support of the Nationa Institute of Justice of the Department of Justice is gratefuy acknowedged. REFERENCES [1] J. Duckworth et a., WPI Precision Personne Locator System: Evauation by First Responders, Proc. ION GNSS, Sepetember 7, Fort Worth, Texas. [] D. Cyganski, et a., WPI Precision Personne Locator System: Indoor Location Demonstrations and RF Design Improvements, Proc. Institute of Navigation, 3rd Annua Meeting, Apri 7, Cambridge, Mass. [3] D. Cyganski, J. A. Orr, R. Angiy, and B. Woodacre, Performance Limitations of a Precision Indoor Positioning System Using a Muti-Carrier Approach, Proc. Institute of Navigation, Nationa Technica Meeting, January -, San Diego, Caif. [] D. Cyganski, J. A. Orr, and W. R. Michason, Performance of a Precision Indoor Positioning System Using Muti Carrier Approach, Proc. ION NTM, January -, San Diego, Caif. [] D. Cyganski, J. A. Orr, D. Breen, B. Woodacre, Error Anaysis of a Precision Indoor Positioning System, Proc. ION AM, June 7-9,, Dayton, Ohio. [] D. Cyganski, J. Orr, W. Michason, A Muti-Carrier Technique for Precision Geoocation for Indoor/Mutipath Environments, Proc. Institute of Navigation GPS/GNSS 3, September 9-1, Portand, Oregon. [7] Kaman, R. E., A New Approach to Linear Fitering and Prediction Probems, Transaction of the ASME-Journa of Basic Engineering, (Series D), 19. [] Brown, R. G., & Hwang, P. Y. C. Introduction to Random Signas and Appied Kaman Fitering. New York: John Wiey & Sons [9] MEMSense, Nano Inertia Measurement Unit Series Documentation, Document:DN1, November 7. [1] E. Foxin, Inertia Head-Tracker Sensor Fusion by a Compementary Separate-Bias Kaman Fiter, Proc. IEEE VRAIS 199, March 3-Apri 3, Santa Cara, CA. [11] E. Foxin, Pedestrian Tracking with Shoe-Mounted Inertia Sensors, IEEE Computer Graphics and Appications, Voume, Issue, Nov.-Dec.. [1] A. Schumacher, Integration of a GPS aided Strapdown Inertia Navigation System for Land Vehices, Master s Thesis, Roya Institute of Technoogy, Sweden, March. May, Monterey, CA