Improved Orientation Estimation in Complex Environments Using Low-Cost Inertial Sensors

Transcription

1 14th International Conference on Information Fusion Chicago, Illinois, USA, July 5-8, 211 Improved Orientation Estimation in Complex Environments Using Low-Cost Inertial Sensors M. E. Johnson Information and Communication Technologies Center CSIRO Marsfield, NSW, Australia. T. Sathyan Information and Communication Technologies Center CSIRO Marsfield, NSW, Australia. Abstract Localization in outdoor environments is widespread, providing improved functionality and benefits for many applications. Extending this ability to in-building environments is a critical prerequisite for many location aware services. Low cost inertial sensors have the potential to improve the performance of new and existing localization systems. A challenging problem in inertial navigation is the accurate tracking of orientation. In this paper we propose a new orientation tracking algorithm that uses a complementary Kalman filter (CKF) for obtaining a reference orientation. A feedback loop is developed, which estimates the gyro bias from the CKF reference. The final orientation is the direct integration of the gyro, after removing the estimated bias. Experimental results show that the proposed algorithm provides significantly improved orientation estimates in the presence of magnetic anomalies and sensor movement, as well as increased stability and reduced noise compared the original CKF algorithm. Position calculations, using a zero-velocity update process, demonstrate the advantages of the enhanced orientation estimate as clear improvements in consequent localization. Keywords: Orientation tracking, sensor fusion, Kalman filter, drift correction, dead reckoning, zero-velocity update. I. INTRODUCTION Location awareness is an important feature of many modern technological systems [8]. Sometimes the location is significant in supplying the appropriate information to the user, in other applications the position itself is the critical information. The use of GPS in motor vehicles and mobile phones is growing to the point where universality of such services seems not far ahead. While GPS usage is clearly an advantage, in terms of its global coverage and free access, neither it nor any of the other satellite-based positioning systems offers a complete solution to the problem of obtaining accurate positions in all environments. Some environments of significance, where GPS is not directly useful, include the interiors of buildings, underground mines, and outdoor areas of high propagation complexity, such as near buildings or cliffs. Ground based radio localization systems [1] [9] can offer improved performance in such environments. While ground based systems extend the range of application of localization, they do not change the limitations inherent in radio based systems. Performance of a radio localization system is intrinsically constrained by the available bandwidth and the local propagation environment [5]. Both practical and legislative requirements restrict the bandwidth available to these systems, and consequently limit the timing resolution and the ability to resolve multipath propagation components. While increased frequency of operation often broadens the available bandwidth, poorer propagation characteristics of high frequency signals limit the range both directly and by increased attenuation due to environmental absorption. There are a number of proposals for systems where other forms of positioning are used to augment the radio based systems to overcome the limitations of radio propagation. Such proposals range from simple sensors such as barometric altimeters to more sophisticated laser ranging and image analysis. The most commonly used system for augmenting radio based systems, however, is the inertial measurement unit (IMU), which does not depend on radio propagation. This is especially true for mobile localization systems, where the rapidly changing environment, and the rapid measurement update rate make long term filtering approaches difficult. The low cost of MEMs based IMUs, and their steadily improving performance, have made them a preferred option for most inertial based augmentation [2]. One of the very useful aspects of this type of combined system is that the performance characteristics of the radio and inertial systems are essentially complementary. Inertial sensors preserve fine details of the dynamics of the motion, but sensor bias produces drift in orientation and position estimates over longer timescales. In contrast, radio based systems are subject to environmental noise and local propagation limitations, which make them unable to be certain of short term variations. Their advantage is that they do not have drift-based variations over time. In this paper we consider the problem of accurate tracking of the orientation over long periods of time. The motivation for the work comes from the desire to add inertial sensors to a radio positioning system called WASP (Wireless Ad-hoc System for Positioning) that has been developed at the Commonwealth Scientific and Industrial Research Organization (CSIRO). Field trials, conducted in a number of application domains, including sports, mining, and public safety, have shown that WASP can provide high positioning accuracy. While in open outdoor environments the positioning accuracy of WASP is very reliable and of the order of.2 m, ISIF 1671

2 indoors the accuracy varies from.5 m to 2 m depending on the propagation environment. The objectives of inertial augmentation to WASP are to achieve consistent high accuracy across all environments, and to provide short term tracking capability in wireless black areas. WASP s technical details can be found in [9] and the references cited therein, but are not directly relevant to this presentation. An orientation tracking algorithm was proposed in [11] where the orientation reference obtained from the accelerometer and magnetometer data, which is stable in the longer term, is fused with the directly integrated gyro orientation, which accurately preserves the shorter term dynamics. Given that the two orientation estimates are complementary, [11] developed a complementary Kalman filter (CKF) algorithm for the fusion. The CKF algorithm adaptively estimates the measurement noise covariance for the accelerometer and magnetometer data to be used in the CKF, which allows it to reduce the impact of the sensor movement on the gravity vector estimation, and magnetic anomalies on the magnetic vector estimation. The original work was used to provide orientation estimates of multiple limb segments for a motion capture process. While the CKF results provided a non-drifting estimate of the orientation, there were still a number of problems with the orientation values in some measurement environments. The major limitations appear to be with the ability of the filter to deal with the complexity of the magnetic environments of narrow corridors in an office environment, and to some extent the complexity of the accelerometer dynamics associated with one of the applications in a foot mounted sensor. Neither of these issues was a necessary part of the original application considered in [11]. The algorithm that we propose uses a feedback loop that tracks the gyro bias accurately and hence, allows drift-free gyro measurements. In particular, the orientation estimates from the CKF algorithm and the directly integrated gyro measurements are used to estimate the gyro bias, which in turn is used to correct the gyro measurements. As we show through several experimental trials the proposed algorithm significantly outperforms the CKF orientation estimation, especially in complex magnetic environments. We also show that the improved orientation leads to considerable improvement in pedestrian dead reckoning. Note that the dead reckoning operates by summing the vector of displacements of the person at each measured step and hence, even if accurate orientation is available, the accuracy will degrade due to the drift in the accelerometer measurements. The zero-velocity update (ZUPT) process is used to correct the accelerometer drift, which identifies the stance phase during the walking gait, and uses the fact that the velocity of the foot is zero when it is in the stance phase [6] [1]. An overview of the dead reckoning process used in the experiments is shown in Figure 1. This paper is organized as follows. Section II briefly describes the CKF algorithm for orientation tracking and discusses the limitations of the algorithm. Details of the proposed algorithm are described in Section III, and the ZUPT process Figure 1. Overview of the dead reckoning process. is briefly discussed in Section IV. Experimental results showing improvement in orientation and dead reckoning accuracy through the proposed algorithm are presented in Section V, and concluding remarks are presented in Section VI. II. CKF FOR ORIENTATION TRACKING A number of algorithms have been proposed [4] [7] [12] for tracking the orientation by fusing data from gyroscope, accelerometer, and magnetometer. The algorithm proposed in [4] is in the context of head tracking application, and uses a separate-bias Kalman filter. An unscented Kalman filter is used in the orientation tracking algorithm developed in [7]. In [12], an extended Kalman filter for tracking orientation of the human limb segments is developed. The preprocessing of accelerometer and magnetometer data using the QUEST algorithm is used to reduce the dimension of the state vector, which also makes the measurement equation linear. The CKF algorithm proposed in [11] has been shown to outperform these algorithms for the orientation tracking in the context of human motion capture. A brief summary of the CKF algorithm for orientation tracking is now provided. A. CKF Algorithm The process model of the CKF algorithm of [11] is developed as an error state model, where the state of the algorithm is the error in the quaternion rather than the quaternion itself. Let q(k) = [q 1 (k),q 2 (k),q 3 (k),q 4 (k)] T = [e T (k),q 4 (k)] denote the quaternion vector at time k, where e(k) = [q 1 (k),q 2 (k),q 3 (k)] T, and let q ε (k) denote the quaternion error. Also let ˆq(k) denote the one step prediction of the quaternion. The process model for the CKF algorithm is given by [11] q ε (k) =F (k)q ε (k 1) + v(k) (1) where the state transition matrix F (k) is given by ( ) 1 F (k) =exp 2 Ω[g(k)] Δ where Δ is the sampling time, g(k) =[g x (k),g y (k),g z (k)] T is composed of the angular rate measurements from the gyro, and Ω[g(k)] is a skew symmetric matrix defined as Ω[g(k)] = g z (k) g y (k) g x (k) g z (k) g x (k) g y (k) g y (k) g x (k) g z (k) g x (k) g y (k) g z (k) (2) (3) 1672

3 Process noise v(k) is assumed to be a zero-mean Gaussian distribution with covariance matrix Q(k), which is given by ( ) 2 Δ Q(k) = Ξ(k 1)Σ G Ξ T (k 1) (4) 2 where Σ G is gyro measurement error covariance matrix. Ξ(k) is given by [ ] [ˆe(k) ]+ˆq4 (k)i Ξ(k) = 3 ˆe(k) T (5) where [e(k) ] = ˆq 3 (k) ˆq 2 (k) ˆq 3 (k) ˆq 1 (k) ˆq 2 (k) ˆq 1 (k) (6) The measurement to the CKF is the difference between the measured and estimated accelerometer and magnetometer data. Given the estimate of the quaternion q(k 1) at time k 1, by applying the state transition matrix F (k), the predicted quaternion ˆq(k) at k is obtained, i.e., ˆq(k) =F (k)q(k 1). Now the known (normalized) values of the gravity vector r A and the magnetic vector r M in the world frame can be transformed to the sensor frame using ˆz A (k) =C (ˆq(k)) r A (7) ˆz M (k) =C (ˆq(k)) r M (8) where C (ˆq(k)) is the direction cosine matrix (DCM) of ˆq(k). The calibrated and normalized accelerometer and magnetometer measurements are denoted as z A (k) and z M (k). The measurement to the CKF is then [ ] [ ] [ ] zεa (k) za (k) ˆzA (k) z(k) = = (9) z εm (k) z M (k) ˆz M (k) The measurement equation of CKF is given by [ ] HA (k)q z(k) = ε (k) + ν(k) (1) H M (k)q ε (k) where ν(k) is the measurement noise, and H A and H M are the Jacobians of C(q(k)) with respect to the quaternion q(k). A Kalman filter operates on the state-space model defined in (1) and (1) to estimate the quaternion error q ε (k), which is added to the predicted quaternion to give the drift-free orientation q(k), i.e., q(k) =ˆq(k)+q ε (k) (11) In summary the algorithm: integrates the gyro data to estimate the new orientation; uses the new orientation to estimate the gravity and magnetic vectors; uses the error between the estimated and measured gravity and magnetic vectors as the input to the Kalman filter; then uses the quaternion error output from the Kalman filter to correct the estimated orientation. One interesting aspect of the CKF algorithm is that it does not provide a direct estimate of the gyro bias. In essence it operates by estimating the orientation error and correcting it directly. Also the effect of the accelerometer and magnetometer measurement noise is handled by adaptively weighting the Figure 2. Y B Y W Z B X B Sensor has fixed orientation to Body frame X B Z W X S Z S X W Body (and Sensor) rotate relative to World frame Relationship between sensor, body, and world coordinate frames. covariance of the noise of these individual measurements. The details of adaptive weighting are omitted here, but can be found in [11]. B. Limitations of the approach Unfortunately, one issue that became apparent quite early in our analysis of the CKF algorithm for orientation tracking of moving sensors with actual sensor data was that, in the short term, the directly integrated open loop gyro (OLG) data gave better estimates of orientation than the CKF algorithm. This is because in complex building environments with narrow corridors, and continuous movements of the sensor, the body motion signals on the accelerometer and the changes and magnitudes of the magnetic field produce significant short and medium term deviations in the CKF calculated orientation. The adaptive weighting of the two is not adequate to handle the large anomalies that occur in such cases. III. CKF FEEDBACK REFERENCED GYRO The algorithm that we propose in this paper stems from the observation that the open loop gyro integrated orientation is stable in the short term and the CKF estimated orientation is stable in the long term. The proposed algorithm, called CKF feedback referenced gyro (CKF-FRG), merges the two estimates of the orientation by developing a feedback loop that calculates a gyro bias correction and feeds it to the gyro measurements. The output of the CKF-FRG algorithm is the direct integration of the gyro, with the estimated bias removed. Before explaining the proposed algorithm various frames of reference are defined that are essential to the description of the algorithm. These are defined for a foot mounted sensor, which is used in the experiments for evaluating the performance of the algorithm. A. Frames of reference Figure 2 illustrates the relationship between the three frames with respect to a foot mounted sensor. The first frame is the sensor frame (S), which is defined by the measured inertial signals in the IMU. Typically the IMU is mounted with a downward pitch angle with respect to the floor, on the upper foot area (where the laces would be on a shoe). It is not essential but typically the roll angle of the sensor is approximately Y S Z B Y B 1673

4 zero with respect to the floor. There is also a slight yaw error, which results from the difference in orientation between the shoe carrying the sensor and the direction of stride of the user. Before the orientation integration processing begins, all sensor data is rotated into a second frame, the body frame (B), defined by the initial orientation of a foot flat on the floor. This frame has zero pitch, roll, and yaw for the initial flat foot state. This intermediate frame is used as it simplifies data presentation, and makes some of the later processing easier. As the foot mounted sensor moves away from its initial state, the signals continue to be converted from the S frame to the B frame. The conversion rotation for the data is defined by a DCM CS B. This value is defined during initialization and is a constant throughout the experiment. The final frame is the world frame (W) in which the body orientation is measured. The gyro angular accelerations in the B frame are integrated to give the orientation of the body in the world frame. The orientation is preserved in the DCM denoted CB W. B. The CKF-FRG Algorithm The full sensor data is fed to the CKF to obtain a good quality orientation reference, with excellent long term stability, but with some low level noise and some significant errors, mostly due to uncompensated magnetic anomalies. A second loop operates as a direct gyro integrator, with a gyro bias correction at the input. The secondary loop calculates the orientation difference between the two estimates and computes a bias correction value. This allows the secondary loop to track most of the dynamics of the gyro data, at the same time as providing removal of the long term drift. It also reduces the output noise, since the feedback is to the input of the integration loop, rather than directly to the output as in the CKF. The gyro g(k), accelerometer a(k), and magnetometer m(k) data from the IMU are corrected for known biases and scale factors, which are calculated in an initialization step. Note that each of the values g(k), a(k), and m(k) is a three-dimensional vector. Let g Scal (k) denote the gyro measurement with the initially estimated bias removed, i.e., g Scal (k) =g(k) G SB (12) where G SB is the initial bias, which is constant throughout an experiment. Similarly, let g SFcal (k) denote the gyro measurement with both the initial and feedback biases removed, i.e., g SFcal (k) =g(k) G SB G FB (k 1) (13) where G FB (k 1) is the feedback bias term. The gyro values are converted to DCM rotations, by scaling the gyro angular velocities by the sampling time Δ and using the standard yaw, pitch, and roll (YPR) to DCM conversion function f YPR2DCM (.) [3]. C GS (k) =f YPR2DCM (g Scal (k)δ) (14) C GSF (k) =f YPR2DCM (g SFcal (k)δ) (15) The measured gyro (i.e., the DCM), accelerometer, and magnetic data are converted from the S frame to B frame. a B (k) =CS B a Scal (k) (16) m B (k) =CS B m Scal (k) (17) C GB (k) =CBC S GS (k)cs B (18) C GBF (k) =CBC S GSF (k)cs B (19) where C S B =(CB S )T. The IMU data in the body frame is fed to the CKF algorithm described in Section II to obtain the orientation DCM in the world frame. Let the output DCM of the CKF be C W B (k). The feedback corrected gyro data is integrated to obtain another estimate of the orientation. CF W B (k) =C GBF (k)cf W B (k 1) (2) where CFB W (k) is the feedback corrected gyro orientation. The two orientation estimates, namely, the CKF calculated and FRG integrated, are converted to YPR angles using the standard conversion function f DCM2YPR (.) [3]. Θ CKF (k) =f DCM2YPR (CB W (k)) (21) Θ FRG (k) =f DCM2YPR (CFB W (k)) (22) where, as the names imply, Θ CKF (k) consists of the YPR angles calculated from the CKF and Θ FRG (k) consists of the angles calculated from the direct FRG integration. Note that this step can fail if the pitch angle is near ±9 degrees, as the conversion from a DCM to a set of Euler angles does not produce a unique solution a problem related to the classical gyro issue of gimbal lock. This is one reason for using data in the B frame rather than the S frame, as the default pitch of the sensor increases the likelihood of a critical pitch angle inducing an error. A double differenced angular error Θ B (k) in the body frame is calculated from the two YPR values as where Θ B(k) =Θ (k) Θ (k 1) (23) Θ (k) =Θ FRG (k) Θ CKF (k) (24) Θ B (k) is actually the change in the error between the current and the previous steps, i.e., the incremental error between the orientations for this step. Note that the more obvious approach of subtracting by multiplication with the transpose of the DCM allows crosstalk between the orthogonal axis channels and therefore allows feedback between channels. If this occurs feedback from one gyro axis becomes an induced error in the bias estimate of another gyro axis, a problem that can lead to instability of the loop. The double differenced angular error calculated above is in the B frame and must be rotated back into the S frame before applying it as feedback to the gyro data. The converted double difference error Θ S (k) is given by Θ S(k) =C S BΘ B(k) (25) 1674

5 Figure 3. Block diagram of the CKF-FRG algorithm. The angular error in the sensor frame is divided by the sampling time to convert it to angular rate error and scaled. The feedback term to the gyro G E (k) is then given by G E (k) =k FB Θ S(k)/Δ (26) where k FB < 1 is the scale factor. The feedback term is not applied directly on a sample-bysample basis. Instead a block of N B feedback samples are accumulated and the average of these samples is calculated. G E (j) = 1 N B j i=j N B+1 G E (i) (27) The averaged feedback term is applied to the subsequent N B samples, while the new average is being calculated. This time delay is critical to the stability of the feedback loop. The functional output of the orientation code is the set of accelerometer data rotated into the world frame, ready to have the gravity component removed and the remaining acceleration components integrated to obtain a position. Figure 3 shows the functional block diagram of the CKF-FRG algorithm. The upper section of the figure is the CKF process, the dotted lines connect it to the FRG loop. IV. ZUPT PROCESSING The objective of implementing the ZUPT processing is to demonstrate the effect of the improved orientation estimation from the CKF-FRG on positioning. A brief description of the ZUPT process is provided here for reference. Note that the accuracy of the ZUPT process depends on having the input accelerometer data correctly oriented in the world frame, before calculating the position. The process of calculating position from acceleration is straightforward in principle. Velocity is the integral of acceleration with respect to time, and position is the integral of velocity with respect to time. One obvious problem is that the measured acceleration is the actual acceleration plus a bias term, so when the sensor data is integrated the velocity has an extra term. In the simple case, where bias is a constant, this results in a linear growth in the velocity error. The second integration to obtain position increases the problem, producing a quadratic position error with time. Further problems are that sensor noise integrates to provide a random walk in the velocity data, and any initial orientation error produces another source of apparent signal in the acceleration input. For MEMS sensors the bias term varies rapidly with time, so direct double integration to obtain position is impractical for any substantial duration. The ZUPT process works because at each footfall, the velocity of the sensor is known to be zero. In the case where the integration is from one footfall to the next the acceleration integrates to zero. Using the biased sensor data the final velocity becomes the integral of the bias over the step. If the assumption is made that the bias is constant from one footfall to the next, we can compute an estimate of the bias from the integrated velocity during the step and the time duration of the step cycle. An estimate of the actual acceleration is made by subtracting the estimated bias from the measured value. Integrating using the ZUPT process thus removes growth in velocity error, and the final position error growth becomes linear instead of a higher order term. The period for stability of a bias estimate is about 3 to 5 seconds for the experimental sensor (Analog Devices ADIS1645), and the typical double step duration (i.e., the time for a sensor on one foot to be 1675

6 Yaw Angle (deg) CKF 15 OLG CKF FRG Time (s) Figure 5. Yaw estimate obtained from direct integration of the gyro measurements, CKF, and the CKF-FRG algorithms in an hour long experiment. Figure 4. Experimental setup showing WASP node and foot mounted IMU. 2 1 CKF CKF FRG picked up and returned to ground) for a normal walker is about 1.5 seconds. This means that the assumption that the bias is constant between footfalls is plausible. The remaining requirement is to detect the beginning and end of the step. This process is described in detail in [6]. The step detection process looks for the reduced variance in the accelerometer data when the foot is stationary on the ground to define the zero-velocity stance phase, and selects a middle point in the stance period to define the end of the old step and the beginning of the new step. The ZUPT process defines a corrected velocity output which is then integrated to give position. V. EXPERIMENTAL RESULTS The foot mounted IMU setup used for experimental data collection is shown in Figure 4, where the WASP node is attached to the hip of the person and the IMU is fixed to the foot. For non foot mounted applications, the IMU can be fixed inside the WASP unit itself. Note that the use of the WASP unit in these experiments is to aggregate the data. Several trials in varying environments with time duration ranging from a few minutes to over an hour are conducted to evaluate the performance of the proposed algorithm and comparison is made with the original CKF algorithm. Improvements are demonstrated in terms of improved YPR estimates, and in terms of position estimates. A. Comparison of Orientation Estimation The primary aim of this procedure is to produce a result that captures the orientation variations of the original IMU sensor, but without the drift that is inevitable from a purely inertial integration process. The gravity and magnetic vectors provide an orientation reference without the drift problem, but Yaw Angle (deg) Time (s) Figure 6. Comparison of CKF and CKF-FRG yaw estimates, showing clear noise reduction when using the proposed algorithm. are subject to errors from: sensor noise; actual sensor motions which modify the gravity vector estimate; and environmental magnetic anomalies. 1) Experiment A: This experiment is designed to show drift removal during an extended period of operation. It consists of periods when the sensor is static, and periods when the sensor is in motion. The movement allows the function of the algorithm in tracking variations to be demonstrated, while the static periods provide a visual reference to confirm the output does not drift. The initial data sequence shows a series of calibrated 9 degree yaw rotations on a fixed reference table. The remaining active periods are walking sequences around the reference table. Figure 5 shows a comparison of the yaw components obtained using OLG integration, CKF, and CKF-FRG processes during the extended sampling period. It is clear that the OLG orientation values have drifted far from the actual values. In this fairly simple environment the CKF gives very good results, showing good orientation values and no drift. The CKF-FRG results show similar performance to that of 1676

7 4 3 2 CKF OLG CKF FRG Yaw Angle (deg) Time (s) Yaw Angle (deg) 5 5 Y1 P1 1 R1 Y2 15 P2 R Time (s) Figure 7. Yaw estimates in a straight line walk. The walking gait is clearly seen in the CKF-FRG algorithm. Figure 8. YPR estimates obtained from CKF (Y1,P1,R1) and CKF-FRG (Y2,P2,R2) algorithms in Experiment C. CKF. Figure 6 is a zoomed in view between times 85 s to 12 s and only shows the yaw estimates of CKF and CKF-FRG algorithms. While both estimates are in agreement, it is evident that the estimate of the proposed algorithm is smoother, indicating noise reduction. The remaining results in this section will be for shorter duration, but are presented against the backdrop of successful long term drift removal. 2) Experiment B: This consists of a walk down a long straight corridor between hallways, offices, labs, and service rooms. The sensor is mounted on the foot as shown in Figure 4. The yaw estimates obtained from the CKF and CKF-FRG algorithms are shown in Figure 7. Observe that both the yaw estimates show the rocking of the sensor due to the walking gait. The CKF results show some of the walking pattern, but have superimposed orientation problems as a result of the building materials and facilities passed on the walk down the corridor. The CKF-FRG results have almost no environmental problems, having the benefits of the direct gyro integration with the benefit of the removal of gyro drift. The experiment is not really long enough to have a significant drift, but the issue is that these benefits apply for longer experiments as well. The position estimates of this walk will be shown in the next section. 3) Experiment C: This consists of a foot mounted sensor used during a walk around a reinforced concrete path in a grassed area in an outdoor field. The actual track is a mixture of walking down the edges of the path and walking in a long oval loop around the path. Figure 8 shows the YPR estimates obtained using the CKF and CKF-FRG algorithms. This shows the basic walking pattern including the long round the oval slope of the yaw data. The impact of the steel in the path on the foot mounted sensor can be seen in the regions where the CKF fails to track the underlying orientation due to incompletely corrected magnetic anomalies. The most significant deviations occur between about 4 and 412 s where yaw, pitch and roll errors are evident, but additional excess yaw variations are seen throughout the periods from 295 to 315 s and 335 to 375 s. The experiment is just long enough to have a significant drift, but the particular benefit is that the CKF-FRG estimates are not affected in the short term by the magnetic effects that cause the CKF to show orientation errors. B. Comparison of Positioning Figure 9 shows the position estimates obtained using the ZUPT processing during the walk along the corridor (Experiment B in the previous section). The effects of the improved CKF-FRG orientation are clearly seen with the estimated path being straighter and following the corridor. As a result of the path straightening the estimated straight line distance is closer to the actual distance walked. In another experiment inside the building, a figure eight walk around the central corridors of the building was undertaken, followed by a 18 degree turn, and returning along the original path. Figure 1 shows the position estimates of CKF and CKF-FRG algorithms, where CKF loses the orientation badly enough that it is hard to recognize the path or the fact that the return path is the same as the starting path. The final point is shown as about 15 m away from where the experiment started. With the CKF-FRG loop operating, the end position is less than 2 m from the start. The final positioning results come from an extended walk around the whole building site. This experiment ran for over 1 min, through many of the building corridors, returning to cover some areas again. The estimated positions are shown in Figure 11. Again the loss of orientation in the CKF causes incorrect positioning traces which are substantially improved by the CKF-FRG augmentation. While the basic structure of the walk is discernable in the CKF results, they could not be used for localization. VI. CONCLUSIONS In this paper we presented a new algorithm, the CKF-FRG, which provides improved orientation tracking using IMU sensor fusion. The proposed algorithm uses a CKF to generate 1677

8 drift-free, but noisy and distorted, estimates of the orientation and uses this as a reference to develop a feedback loop that computes a bias term for the gyro. When the calculated bias is removed from the gyro measurements, the integration of the resulting data provides a more accurate estimate of the orientation than the estimate provided by the CKF alone. We also showed that the CKF-FRG algorithm can handle magnetic anomalies and the effects of sensor movement on the accelerometer data better than the original CKF orientation tracking algorithm. The improved orientation estimation has also led to improved dead reckoning performance, which we demonstrated using the ZUPT process. In the near future, we are planning to incorporate the CKF-FRG algorithm to augment the WASP radio positioning. ACKNOWLEDGMENTS We would like to acknowledge the contributions of two CSIRO vacation scholars Malcolm Vu and Tim Patten to the development of the ZUPT processes, and Dr. Mark Hedley for proofreading and comments on this paper. We would also like to recognize the significant effort and assistance of the WASP development team in conducting these trials Figure Position estimates of the straight line walk. REFERENCES 8 [1] V. Amendolare, D. Cyganski, R. J. Duckworth, S. Makarov, J. Coyne, H. Daempfling, and B. Woodacre, WPI precision personnel location system: Inertial navigation supplementation, in Proc. Position Location And Navigation Symposium, Monterey, CA, May 28. [2] A. Brown, GPS/INS uses low-cost MEMS IMU, IEEE Trans. Aerosp. Electron. Syst., vol. 2, no. 9, pp. 3 1, Sep. 25. [3] J. Diebel, Representing attitude: Euler angles, unit quaternions, and rotation vectors, Stanford University, Tech. Rep., Oct. 26. [4] E. Foxlin, Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter, in Proc. IEEE Virtual Reality Annual Int l Symp., Mar. 1996, pp [5] C. Gentile and A. Kik, An evaluation of ultra wideband technology for indoor ranging, in Proc. IEEE Global Telecommunications Conf., San Francisco, CA, Nov. 26, pp [6] A. Jimenez, F. Seco, C. Prieto, and J. Guevara, A comparison of pedestrian dead-reckoning algorithms using a low-cost mems imu, in IEEE International Symposium on Intelligent Signal Processing, Budapest, Hungary, 29, pp [7] E. Kraft, A quaternion-based unscented Kalman filter for orientation tracking, in Proc. Int l Conf. on Information Fusion, vol. 1, 23, pp [8] N. Patwari, J. N. Ash, S. Kyperountas, A. O. Hero III, R. L. Moses, and N. S. Correal, Locating the nodes: Cooperative localization in wireless sensor networks, IEEE Signal Process. Mag., vol. 22, no. 4, pp , Jul. 25. [9] T. Sathyan, D. Humphrey, and M. Hedley, Wasp: A system and algorithms for accurate radio localization using low-cost hardware, IEEE Trans. Syst., Man, Cybern. C, vol. 41, no. 2, pp , Mar [1] R. Stirling, K. Fyfe, and G. Lachapelle, Evaluation of a new method of heading estimation for pedestrian dead reckoning using shoe mounted sensors, Journal of Navigation, vol. 58, pp , Jan. 25. [11] S. Sun, X. Meng, L. Ji, J. Wu, and W.-C. Wong, Adaptive sensor data fusion in motion capture, in Proc. Int l Conf. on Information Fusion, Edinburgh, UK, Jul. 21. [12] X. Yun and E. Bachmann, Design, implementation, and experimental results of a quaternion-based Kalman filter for human body motion tracking, IEEE Transactions on Robotics, vol. 22, no. 6, pp , Dec Figure 1. Figure 8 walk inside the building Figure 11. An extended walk through the whole building. 1678