NAVAL POSTGRADUATE SCHOOL Monterey, California

Transcription

1 NPS-MV-- NAVAL POSTGRADUATE SCHOOL Monterey, California Real-Time Tracking and Display of Human Limb Segment Motions Using Sourceless Sensors and a Quaternion-Based Filtering Algorithm Part I: Theory by Robert B. McGhee Eric R. Bachmann Xiaoping Yun Michael J. Zyda Noember Approed for public release; distribution is unlimited. Prepared for: NPS Moes Academic Group

2 REPORT DOCUMENTATION PAGE Form approed OMB No Public reporting burden for this collection of information is estimated to aerage hour per response, including the time for reiewing instructions, searching existing data sources, gathering and maintaining the data needed, and completing and reiewing the collection of information. Send comments regarding this burden estimate or any other aspect of this collection of information, including suggestions for reducing this burden, to Washington Headuarters Serices, Directorate for information Operations and Reports, 5 Jefferson Dais Highway, Suite 4, Arlington, VA -4, and to the Office of Management and Budget, Paperwork Reduction Project (74-88, Washington, DC 5.. AGENCY USE ONLY (Leae blank. REPORT DATE Noember. REPORT TYPE AND DATES COVERED Technical Report 4. TITLE AND SUBTITLE Real-Time Tracking and Display of Human Limb Segment Motions Using Sourceless Sensors and a Quaternion-Based Filtering Algorithm Part I: Theory 5. FUNDING N6M ARO 6. AUTHOR(S McGhee, Robert B., Bachmann, Eric R., Yun, Xiaoping, and Zyda, Michael J. 7. PERFORMING ORGANIZATION NAME(S AND ADDRESS(ES Naal Postgraduate School Monterey, CA PERFORMING ORGANIZATION REPORT NUMBER NPS-MV-- 9. SPONSORING/MONITORING AGENCY NAME(S AND ADDRESS(ES. SPONSORING/MONITORING AGENCY REPORT NUMBER. SUPPLEMENTARY NOTES The iews expressed in this thesis are those of the author and do not reflect the official policy or position of the Department of Defense or the U.S. Goernment. a. DISTRIBUTION/AVAILABILITY STATEMENT Approed for public release; distribution is unlimited. b. DISTRIBUTION CODE. ABSTRACT (Maximum words. Recently, a new impetus has been gien to work to produce a whole body human/computer interface system, which encumbers the wearer to a minimum degree and operates oer long distances. The input portion of such an interface can be deried by appropriate processing of signals from a nine-axis sensor package consisting of a three-axis angular rate sensor, a three-axis magnetometer, and a three-axis linear accelerometer. This paper focuses on data processing algorithms for MARG sensors. Since human limb segments are capable of arbitrary motion, Euler angles do not proide an appropriate means for specifying orientation. Instead, uaternions are used for this purpose. Since a uaternion is a four-dimensional ector, and a MARG sensor produces nine signals, this data processing problem is oerspecified. This fact can be used to discriminate against sensor noise and to reduce the effects of linear acceleration on measurement of the graity ector by accelerometers. Detailed computer simulation studies and the results of preliminary experiments with a prototype body tracking system confirm the effectieness of the uaternion body-tracking filter. 4. SUBJECT TERMS Inertial Tracking, Magnetic Tracking, MARG Sensors, Complementary Filtering, Quaternions 5. NUMBER OF PAGES 7 6. PRICE CODE 7. SECURITY CLASSIFICATION OF REPORT 8. SECURITY CLASSIFICATION OF THIS PAGE 9. SECURITY CLASSIFICATION OF ABSTRACT. LIMITATION OF ABSTRACT Unclassified Unclassified Unclassfied UL NSN Standard Form 98 Re. -89 Prescribed by ANSI Std 9-8 ii

3 NAVAL POSTGRADUATE SCHOOL Monterey, California RADM Daid R. Ellison, USN Superintendent Richard Elster Proost This report was prepared for NPS_MoVES Academic Group and funded by _Army Research Office (ARO, Modeling and Simulation Office (N6M This report was prepared by: Robert B. McGhee Professor Eric R. Bachmann Research Assistant Professor Xiaoping Yun Professor Michael J. Zyda Professor Reiewed by: Released by: Michael J. Zyda Chair, MoVES Academic Group D. W. Netzer Associate Proost and Dean of Research iii

4 Real-Time Tracking and Display of Human Limb Segment Motions Using Sourceless Sensors and a Quaternion-Based Filtering Algorithm Part I: Theory Abstract R.B. McGhee, E.R. Bachmann, X.P. Yun, and M.J. Zyda Naal Postgraduate School, Monterey, CA994 mcghee@cs.nps.nay.mil Tracking and display of human limb segment motions has been the topic of much research and deelopment oer many years for purposes as aried as basic human physiological studies, physical rehabilitation, sports training, and the construction of anthropomorphic robots. More recently, a new impetus has been gien to such work by the desire to produce a whole body human/computer interface system, which encumbers the wearer to a minimum degree and operates oer long distances. The input portion of such an interface can be deried by appropriate processing of signals from a nine-axis sensor package consisting of a three-axis angular rate sensor, a three-axis magnetometer, and a three-axis linear accelerometer. In this paper, such a sensor system is called a MARG (Magnetic field, Angular Rate, and Graity sensor. Recent adances in micromachining (MEMS technology, and magnetometer miniaturization, hae allowed the deelopment of MARG sensors of ery small size, suitable for attachment to indiidual human limb segments. At the same time, adances in wearable computers and wireless data communication techniues make it feasible to begin the deelopment of a practical full body tracking system using sourceless MARG sensors. This paper focuses on data processing algorithms for MARG sensors. Since human limb segments are capable of arbitrary motion, Euler angles do not proide an appropriate means for specifying orientation. Instead, uaternions are used for this purpose. Since a uaternion is a four-dimensional ector, and a MARG sensor produces nine signals, this data processing problem is oerspecified. This fact can be used to discriminate against sensor noise and to reduce the effects of linear acceleration on measurement of the graity ector by accelerometers. Detailed computer simulation studies and the results of preliminary experiments with a prototype body tracking system confirm the effectieness of the uaternion body-tracking filter.. Introduction Since early times, the graitational field of the Earth has been used to define the local ertical or down direction. Deices such as plumb bobs were used to measure the slope of Egyptian pyramids and many other early buildings and walls. With the adent of sailing ships (if not earlier, the notion of pitch and roll angles as a measure of deiation from the ertical was introduced. Much later, perhaps a thousand years ago or so, the compass was inented as a means of measuring a third orientation angle usually

5 called heading. In the nineteenth century these angles were named Euler angles and their mathematical description was formalized [McGhee, ]. As sailing ships grew in size and capability, long range naigation at sea, out of sight of land, became ery important. The compass was a critical inention in enabling long distance sailing, with Columbus oyages to the New World being the most widely known early example. Various forms of sextants were also in use in ancient times to determine latitude from measurement of eleation angles to known stars. Such measurements reuired correction for ship pitch and roll angles to be effectie. This was done with a plumb bob, bubble leel, or similar deice. Remoal of pitch and roll effects on compasses on shipboard was usually achieed by floating the compass needle (or heading rose on a liuid which remained more or less leel relatie to the horizon during ship pitching and rolling under wae and wind action. With the introduction of compasses to aircraft naigation, this approach no longer worked and instead spinning ertical gyros were used to maintain an artificial horizon during sustained aircraft maneuers. In such applications, three-axis magnetometers are sometimes used to measure the Earth s magnetic field, and heading then computed from alues for pitch and roll angles obtained from sensors attached to the ertical gyro [Frey, 996]. A similar approach is currently used in miniaturized electronic compasses intended for marine naigation applications [Precision Naigation, ]. For real-time human limb segment tracking, most current methods use actie sources of some sort to track either angles or the position of reference points on limbs or joints [Meyer, 99; Molet, 999]. While this has worked well for some applications, such approaches typically either seriously encumber the subject, or work only oer limited ranges, or both. Howeer, in the case of head tracking, recent adances in sensor technology and small computers hae made it possible to achiee effectie sourceless tracking using only the Earth s graitational and magnetic fields as references for determination of orientation, analogous to the ship and aircraft systems described aboe. Head trackers of this type are now commercially aailable [Foxlin, 994], and typically use some ariant of MARG (Magnetic field, Angular Rate, and Graity sensors to determine alues for head Euler angles. Full details of a similar system deeloped for small unmanned submarine naigation can be found in [Yun, 999]. The purpose of the present paper is to describe and ealuate alternatie algorithms needed to extend sourceless tracking technology from head motion to full body motion. As will be seen, major changes in MARG sensor data processing techniues are needed if robust and costeffectie body tracking is to be achieed.. Quaternion Basics A fundamental problem with full body tracking using Euler angles arises from the fact that when a rigid body (or limb segment is in a ertical orientation, heading and roll Euler angles are undefined, and cannot be uniuely determined. Een more seriously, the body rate to Euler angle rate transformation matrix (needed to derie Euler angle rates from body-fixed angular rate sensors is singular for this orientation [McGhee, ]. Fortunately, uaternions do not suffer from this problem since they express rigid body

6 orientation in terms of a single rotation about an inclined axis. A common way of expressing such an angle-axis pair in terms of a unit uaternion is: cos( Θ / u sin( Θ / ( where Θ is the rotation angle, and u is a unit three-dimensional rotation axis ector [McGhee, ]. In this form, a unit uaternion looks ery much like the polar form of a unit complex number: z iθ cos( Θ isin( Θ e ( where i is a flag designating the second ( imaginary element of a two-dimensional ector. Indeed, it is well known that a unit complex number accomplishes planar rotation of two-dimensional ectors under complex multiplication. That is, if and i Θ z x iy r e ( then Θ z e i (4 i( Θ Θ z z zz r e (5 For uaternion rotation of three-dimensional ectors, the corresponding relationship is: (6 where is any three-dimensional ector (uaternion with for first component and denotes the uaternion product [McGhee, ]. There are seeral euialent ways to define the uaternion product [McGhee, ]. The original approach, used by Hamilton, the discoerer of uaternions, was to iew a uaternion as a four-dimensional generalization of a complex number, written as w u s (7 where w is the real part and s is the magnitude of the ector part. More precisely, what this euation really means is: ( w ( x y z ( w x y z (8 where is a ector sum and u is the unit ector u u u u (9 ( z y z

7 Since u has only three non-zero components, it is also possible to describe it using the flag notation: u i u j u k u ( By analogy to complex numbers, Hamilton defined a flag algebra as: and From these two definitions, it follows that x y z i i j j k k ( ( i j k ( ( i j k j i, k i j i k, j k i k j ( The uaternion product is thus seen to hae some of the features of the two-dimensional complex product, and some of the features of the three-dimensional ector cross product [McGhee, ]. Analogous to the case with complex numbers and threedimensional ectors, flag algebra allows scalar algebra and scalar calculus to be applied to uaternion analysis. Again by analogy to complex numbers, the conjugate of a uaternion, denoted as *, is obtained by merely negating its ector part. That is, if is written in the form of E. (8, then * ( w x y z (4 It is a straightforward exercise in uaternion algebra (using E. ( and ( to show that * * w x y z (5 From this result, it is eident that E. ( does in fact describe any unit uaternion. It is also clear from this relation that the inerse of any unit uaternion is just its conjugate. Eidently, for an arbitrary uaternion, * * (6 These definitions allow the computation of all terms appearing in E. (6, the basic relation for transforming points attached to a rigid body to the corresponding points in world coordinates. It is noteworthy that this approach is much less computationally demanding than the more common rotation matrix approach typically used in computer graphics for such transformations [McGhee, ]. 4

8 . Quaternion Filter While all possible Euler angle sets are singular (non-uniue in some orientation, and body rate to Euler rate transformations are computationally expensie since they reuire ector-matrix products inoling many trigonometric functions, determination of uaternion rates from body rates is free of both of these problems [McGhee, ]. Specifically, if the resered symbols p,, and r are used for roll rate, pitch rate, and yaw rate respectiely, then [Cooke 99; Kuipers 998]: & ( p r (7 In understanding this euation, it is important to realize that the symbol has been oerloaded, standing both for a uaternion and for pitch rate. Since both usages are standard, the authors hae elected to write the euation in this form, trusting that any potential ambiguities will be resoled by context. If an extremely accurate rate sensor is aailable (such as a ring laser gyro for example, then orientation can be determined by simply integrating this rate [Lawrence, 998]. Howeer, such sensors are large, heay, and expensie, and not at all practical for human body segment tracking. Instead, micromachined ibrating beam sensors are more suitable for this purpose [Teegarden, 998]. While this type of sensor is inexpensie, lightweight, rugged, ery small in physical dimensions, and reuires negligible electrical power, its accuracy is such that errors resulting from integration of E. (7 become unacceptable after only a few seconds. This being the case, some form of drift correction is reuired to obtain accurate results from integrating this euation. Figure below shows one way that can be done [Bachmann, 999]. Accelerometers h h ( h ( h h h b b b T y r y r (ˆ - r ε (ˆ ( ˆ m ˆ, ˆ nˆ T Magnetometers b b ( b [ X T X ] X T full k Angular-rate Sensors ( p r ˆ ( p r & & ε ˆ Figure : Quaternion-Based Orientation Filter 5

9 As can be seen from this figure, drift correction is proided by the accelerometer and magnetometer components of a MARG sensor. Specifically, the ector h is a unit ector pointing in the direction of the total sensed acceleration (graity plus linear acceleration, while the ector b is a unit ector aligned with the total magnetic field ector (earth field plus local disturbances. The ector y r is thus a six dimensional ector composed of these two unit ectors. The uaternion ˆ is the estimated orientation uaternion for the r body being tracked, so y( ˆ is a computed measurement ector determined from ˆ. In order to make this computation, it is necessary to use the known alues m ( for the graity unit ector and n ( nx n y nz for the local magnetic field ector. With this understanding, it is eident that the modeling error ector, ε (ˆ, represents the difference between what the sensors measure and what the uaternion filter predicts should be measured. Further examination of Figure shows that the uaternion used to compute y( ˆ is the inerse of the orientation uaternion. This is because ˆ transforms points in body (sensor coordinates to the corresponding points in world coordinates whereas the reerse transformation is needed to compute r ε ( ˆ in sensor coordinates [McGhee, ]. At this point in understanding this filter, it is important to recognize that while r ε ( ˆ is a sixdimensional ector, the drift correction signal, &, is of dimension four. This means that e a 4 x 6 matrix or some other transformation is needed to compute a alue for the drift correction component of &ˆ from the modeling error ector. There is a ast body of literature on this type of problem generally referred to as Kalman filter theory, or more generally, optimal least-suares estimation theory [Brown, 997]. In this paper, a simpler approach called non-linear regression analysis will be used. As will be seen in the following section of this paper, this method explains the relation shown in Figure. T T & k[ X X ] X r e ε ( ˆ (8 4. Non-linear Regression and Gauss-Newton Iteration Suppose that a ector dependent ariable, y, depends on a ector independent ariable, x. The general form of such a relationship is y f (x (9 Now let y denote a measured alue for y obtained in some type of experimental setting. In general, any real physical measurement will be to some degree corrupted by random noise. When such noise is additie (the most common assumption [Brown, 997], y takes the form of r 6

10 y f ( η ( x where η is the (unknown measurement noise ector. The fundamental problem of nonlinear regression analysis is to obtain a best estimate for an unknown x from a gien function f and a known (measured alue for y [McGhee, 967; Eeritt, 987]. In regression analysis, it is reuired that the dimension of y be greater than the dimension of x. This means that there is in general no alue, x xˆ, such that f ( xˆ y. That is, to borrow terminology from linear algebra, there are fewer unknowns than euations when y has more components than x. Problems of this type are said to be oerspecified [Strang, 988]. In such cases, what is most often done is to define an error ector, r ε, as a column ector of M rows gien by: r ε ( x y f ( x ( and to then try to find an [McGhee, 967]: x xˆ that minimizes the scalar suared error criterion function r ( x ( x T r ϕ ε ε ( x ( The minimizing alue for ϕ, namely x xˆ, is called the least suares estimate of the (unknown true alue for x. Minimization of a non-linear function such as ϕ (x is a basic problem, which has receied much attention in mathematics for centuries. The widespread aailability of digital computers beginning about forty years ago allowed much more complex minimization problems to be soled, and spurred a renewed interest in the mathematical underpinnings of optimization problems in general. One approach, which has proed to be effectie for many regression problems, is Gauss-Newton iteration [Hartley, 96; McGhee, 96]. In this techniue, the M row ector function f (x is linearized about a gien alue, x x, by using the first two terms in its Taylor series expansion, namely: f ( x x f ( x X x O( ( x where x and x are column ectors of N rows, N < M, and X is the M x N matrix of partial deriaties: X ij f x i (4 j In utilizing E. (, it is important to recognize that X is ealuated at x x, and is therefore a constant matrix relatie to changes in x about that alue. Specifically, in what follows, 7

11 dx dx dx d x (5 as: Using only the linear part of E. (, the error ector, ε r, can be approximated r r ε ( x x y f ( x X x ε X x (6 From the inerse law of transposed products [Strang, 988], it follows that: r r ε ( x ε x T T x T X T (7 Thus, from E. (, the criterion function, ϕ (x, is approximated by: rt r rt T T r ϕ( x ε ε ε X x x X ε x T X T X x (8 which, proiding X is of full rank, is a positie definite uadratic form in x [Strang 988; Cormen 994]. Noting that eery term in this euation is a scalar, and again using the inerse law of transposed matrices, it follows that rt T T r ε X x x X ε (9 so E. (8 can be simplified to: r T r T T r ϕ( x ε ε x X ε x T X T X x ( From ector calculus [Rust, 97], using this expression, and E. (5, the gradient (ector deriatie of ϕ is gien by: When ( x dϕ T r X ε X dx T X x ϕ is positie definite, the uniue minimum of E. ( is found by euating the aboe gradient to with the result: [ ] r T T X X X ε ( x ( This euation defines Gauss-Newton iteration and explains E. (8, and the corresponding block of Figure, except for the scalar feedback gain factor, k. This factor relates to the stability and accuracy of the uaternion filter, and will be the subject of further discussion later in this paper. All of the aboe analysis assumes that the neglected O( x term in E. ( is not important. This is often not the case, so in many problems it is necessary to apply E. ( iteratiely. This issue is inestigated for the case of orientation uaternion estimation in a later section this paper. 8

12 5. Gauss-Newton Iteration for Quaternion Filter Appendix A deries the X matrix of Figure for an arbitrary non-zero uaternion,, and also shows that multiplying any such uaternion by a non-zero scalar has no effect on the computed measurement ector, y (, shown in Figure. Unfortunately, this latter fact means that ˆ is not uniue. This has the further conseuence that X is not of full rank, and therefore the regression matrix T S X X ( appearing in E. (8 and ( is singular [Strang, 988]. This means that S has no inerse, and therefore this X matrix cannot be used in Gauss-Newton iteration. Numerical experiments show that this is indeed the case [web site]. While this might at first seem to be extremely serious with respect to the correct functioning of the uaternion filter of Figure, this is not so. In fact, as described below, there are two distinct ways of dealing with the singularity of S. The most straightforward way to resole the non-uniueness problem for ˆ is to restrict it to be a unit uaternion. Specifically, if the output of the integrator block on Figure is labeled ~, then ~ can be normalized to a unit uaternion by adding an extra block that accomplishes the calculation: ˆ ~ ~ (4 Somewhat surprisingly, this constraint also permits computationally significant improements to the calculation of the X matrix. Specifically, since the inerse of a unit uaternion is just its conjugate, E. (A-5 simplifies to [Henault, 997]: (5 i (6 j (7 and 9

13 k (8 E. (A-7 is likewise simplified by this substitution. In particular, using these results, from the product rule of differential calculus: y ( m m n n T, (9 y ( i m m i i n n i T, (4 y y ( j m m j j n n j T, (4 ( k m m k k n n k T, (4 In all of these expressions, it is to be understood that only the ector part of each triple uaternion product is used, and that the comma denotes concatenation. Thus, each of the aboe expressions constitutes one 6 x column of the X matrix. While these euations yield a numerically different X matrix from that of Appendix A, simulation experiments show that, when ˆ is normalized to a unit uaternion on eery computation cycle, then the associated matrix is not singular and Gauss-Newton iteration using these relations succeeds [web site]. The second alternatie to the computation of ˆ results from noting that if ˆ ˆ (4 new old full and if both ˆ new and ˆ old are unit uaternions, then any small full must be orthogonal to ˆ. That is, the only way to alter a unit ector while maintaining unit length is to rotate it, and for small rotations must therefore be tangent to the unit four-dimensional sphere defined by euating E. (5 to. From the Orthogonal Quaternion Theorem of Appendix B, this means that: ( ˆ (44 With this constraint, linearization of the expected measurement ector, y(, in Figure, yields

14 ( y( X y( X ( T y (45 ( and conseuently: y X ( ( X ( i T (46 y X ( ( X ( j T (47 and y X ( ( X ( k T (48 From E. (44, it is eident that when Gauss-Newton iteration is applied to unit uaternions, it is sufficient to sole for only three unknowns rather than four as in the methods for estimation of considered until now. That is, if X is the 6 x matrix full y y y X (49 then, T [ X X ] X r ε ( ˆ (5 full and ( ˆ, (5 full full Simulation experiments [web site] show that this result functions correctly in Gauss- Newton iteration using either the X matrix of Appendix A or the modified X matrix defined by E. (9 (4. When both of the aboe improements are incorporated into the Gauss-Newton algorithm, simulation results are the same as those obtained from either one applied alone [web site]. Since E. (9 (4 notably simplify the computation of the X matrix, and E. (5 inoles a x matrix inersion rather than the 4 x 4 matrix inersion of the basic algorithm, a considerable reduction in the time reuired for execution of one cycle of Gauss-Newton iteration results from use of a combined algorithm. In this regard, it is

15 particularly important that it is known that the best algorithms for matrix inersion are of O n complexity [Cormen, 994]. ( As a final obseration on uniueness, while any of the aboe modifications to the results of Appendix A soles the problem of a singular regression matrix, from E. (A-, it is eident that ˆ produces the same computed measurement ector as ˆ. This means that if the initial error in ˆ is ery large, then Gauss-Newton iteration can produce either of two answers, one with a negatie real part, the other with a positie real part. While this is of no conseuence to body tracking, it does matter in some applications. One such example is proided by rigid body dynamics [McGhee, ]. In such cases, it is a simple matter to achiee uniueness by transforming the result of any uaternion calculation to positie real form by simply negating any result with a negatie real part. 6. Conergence and Accuracy of Orientation Quaternion Estimation by Gauss-Newton Iteration The conergence of Gauss-Newton iteration can be inestigated from either a local or global perspectie. Conditions for local conergence are known, and are deried by writing the Taylor series for the criterion function, ϕ (, through the second deriatie term, and then computing the eigenalues of a specific matrix inoling the regression matrix, S [McGhee 96; Bekey 964]. If local conergence conditions are satisfied at one or more points inside a bounded search olume, then an algorithm exists which returns, with probability arbitrarily close to one, the alue of that produces the ϕ within the search olume [McGhee, 967]. true minimum of ( By restricting ˆ to be a unit uaternion, the search olume is confined to the surface of a unit four-dimensional sphere. This fact guarantees global conergence to a true minimum alue of ϕ (, proiding that local conergence conditions are satisfied. Since it is ery difficult to establish by analytic means that this is the case for all unit uaternions and all possible leels of measurement noise, this uestion has been studied by computer simulation. Specifically, seeral hundred simulation trials were conducted in which a random unit uaternion called true was generated along with another random (uncorrelated unit uaternion called start. Using noiseless synthetic data generated from true by means of E. (A-6, and starting Gauss-Newton iteration at start, no failures to conerge were obsered after ten cycles of iteration, although as expected, conergence to true was found to be just as likely as conergence to true. Eidently, Gauss-Newton iteration is ery robust when applied to the orientation uaternion estimation problem. Interested readers are inited to explore this issue further by means of additional simulation trials using the test functions t through t included in the Lisp code aailable at [web site]. The aboe discussion relates to filter initialization only since any useful filter must hae the property that preious and updated alues for ˆ differ by only a small

16 amount. That is, the ery nature of tracking means that a reasonably good estimate for the current true is aailable at all times following initialization. To test tracking properties of Gauss-Newton iteration, another simulation was conducted in which start was produced by adding uniformly distributed noise with a maximum alue of /-. to each component of true. This is felt to be a rather seere test of the tracking capabilities of Gauss-Newton iteration since it corresponds to tracking errors of the order of.5% for unit uaternions. To see that this is so, let η (5 start true and let γ be a random ariable uniformly distributed in the interal [-, ]. Then, the ariance, σ γ, of γ is gien by [Leine, 97], σ γ γ d γ (5 From this result, if η max is the maximum absolute error in any component of start (eual to. in the aboe discussion, then since each of the four components of η is independently generated, it follows that the expected root mean suare error in start is gien by: 4 σ η 4 η max.5 (54 That is, the rms (root mean suare length of η is this number, which is.5% of the length of a unit uaternion. After generating true and start, a six-dimensional ector of noiseless synthetic data was again generated using E. (A-6. Following this step, six-dimensional uniformly distributed noise in the range [-max-noise, max-noise] was added to this data. That is, the simulated measurement was computed as: ( δ y (55 y true where each component of δ is a sample of uniformly distributed noise in the aboe interal. By analogy to E. (54, it follows that: σ δ 6 δ max. 44δ max (56 That is, the rms length of δ is approximately 4% more than the maximum absolute error in any of its six components. Using this type of synthetic data, a series of

17 simulation experiments was carried out in which the rms accuracy of Gauss-Newton iteration was ealuated as a function of max-noise and the number of cycles of iteration within each experiment. The results of this study are summarized in Table. Each cell of this table is associated with the length of the error in ˆ aeraged oer one hundred random trials. New data was generated for each cell, so this table shows both systematic tendencies and random (sampling fluctuations. The following paragraphs discuss the significance of the tabulated results. Max. Noise Number of Gauss-Newton Iteration Cycles per Component e e e e Table : Obsered RMS Error in Estimated Orientation Quaternion as a Function of Sensor Noise Leel and Number of Cycles of Gauss-Newton Iteration Considering first the top row of Table, it can be seen that no failures to conerge occurred in any of the three sets of one hundred experiments associated with each entry in this row. Moreoer, ˆ neer conerged to true as was obsered often in the initialization experiments described at the beginning of this section of this paper. This is because initialization inoles completely random alues for start, while all of the experiments of Table I use a start with a maximum error of /-. in each component. This means that true is not in the domain of attraction of start for these trials. Further examination of this row shows that, during tracking, three cycles of Gauss- Newton with noiseless data results in estimation errors which are in the leel of round-off errors. This is in contrast to initialization experiments in which it was found, as reported aboe, that somewhat more cycles are needed to achiee conergence when start is completely random. Turning to the second row of Table I, it can be seen that two cycles of Gauss- Newton iteration are sufficient when the maximum absolute alue for the noise on each data component is eual to.. In addition, the length of the ector error in ˆ is seen to be approximately 8% of the maximum noise per component. Examining the third row of the table shows that these two features persist when the maximum noise leel is increased to.. When the sensor maximum noise is increased to. (an unreasonably large alue, the last row of Table I reeals an apparently anomalous behaior in that more Gauss- Newton iteration cycles result in less accuracy. Howeer, as confirmed by additional experiments not reported here, this effect is just the result of statistical fluctuations due to the independent trials associated with eery entry in this table. The more important 4

18 conclusion from this row is that, with uniformly distributed noise on synthetic data samples, een at such unrealistically large noise leels, the length of the ector error in ˆ is seen to remain at approximately 8% of the maximum data component noise leel. Lisp code for the simulation study reported in Table I is aailable at [web site], and the interested reader is inited to use the functions t4 through t5 defined in this code to confirm or extend the results reported here. Howeer, the authors beliee that the results of this table are sufficient to show that Gauss-Newton iteration proides both the stability and accuracy needed for implementation of the drift correction feedback loop shown in Figure. The next section of this paper presents a modification to Gauss- Newton iteration which could be applied when errors in magnetometer data are either larger or smaller than those in accelerometer data, and also discusses criteria and methods for choosing the scalar gain factor, k, when Gauss-Newton iteration is combined with integration of & alues obtained from angular rate sensors. 7. Filter Tuning: Weighted Least Suares Regression and Complementary Filtering Referring again to Figure, the possibility exists of putting greater or less reliance on magnetometer data in comparison to accelerometer data. This could come about because one or the other of these signals could proe to be less accurate (or noisier than the other. This can be achieed by merely redefining the error ector, ε, as: ( ˆ ( ˆ ( ˆ ( ˆ ( ˆ 4 ( ˆ 5 ( ˆ y y y y ε (57 ρ ρ ρ y y ( y ( y 4 y y 5 ( y y Eidently, ρ > emphasizes magnetometer data, while < ρ < puts greater weight on accelerometer data. Clearly, this change also alters the X matrix, simply by multiplying the last three elements of each column by ρ. If a detailed statistical model is aailable for magnetometer and accelerometer errors for a particular experimental setting then, at least conceptually, the best alue for ρ could be obtained either from Kalman filter theory [Brown, 997] or from some other statistical theory such as maximum likelihood parameter estimation [Leine, 97]. Howeer, the authors are inclined to beliee that this will usually be impractical for human body tracking applications, and that it is probably more productie to think of ρ as a tunable parameter adjusted by eyeball optimization in a gien situation. All of the analysis and simulation up to this point in this paper has dealt only with the application of Gauss-Newton iteration to static problems. That is, it has been assumed that only one measurement ector is aailable, and the Gauss-Newton procedure is applied iteratiely to find as shown in Figure. While this kind of computation full 6 6 5

19 may be useful in some applications, body tracking is a dynamic problem in which estimation of the orientation uaternion must proceed in real-time, and in parallel with the acuisition of new data during the course of limb segment and body motion. As mentioned preiously, there is a large body of knowledge dealing with such problems, ordinarily called filter theory [Brown, 997]. This theory proides a means for analyzing the effects of the other tunable parameter in the uaternion orientation filter of Figure, namely the feedback gain factor, k. The facts that Table I shows that rms estimated orientation uaternion errors are linearly related to rms sensor noise leel, and that only one cycle of Gauss-Newton iteration may be sufficient in practical orientation tracking, imply that a linearization of Figure is possible for purposes of stability analysis and inestigation of the effects of the alue of the feedback gain, k. Such a linearization is based on the recognition that, during tracking, een on the first Gauss-Newton iteration it is a good approximation to assume that ˆ (58 Figure below shows a linearized ersion of Figure based on this assumption. full true - true k & & ε ˆ Figure : Linearized Orientation Quaternion Estimation Filter This filter has the form of a complementary filter [Brown, 997] in which the integral of &, which is accurate for short periods of time, is complemented by the integral of the drift correction deriatie, & ε, which is dependable only when aeraged oer longer periods of time to counteract the confounding effects of linear acceleration on the measurement of graity and of stray magnetic fields on the determination of heading. A key feature of complementary filters is the idea of a crossoer freuency below which signals from one type of sensor are gien greater weighting, and aboe which signals from another type of sensor are faored. From linear system theory [McGhee, 995; Brown, 997], the crossoer freuency for Figure is gien by f c k Hz (59 π 6

20 The meaning of this result is that below this freuency accelerometer and magnetometer signals are gien greater emphasis, while aboe this freuency, rate sensor signals are more trusted. While it is possible to optimize k if full statistical information about sensor signal and noise characteristics is aailable, the authors doubt that this will often be the case, and beliee instead that k is a parameter which, like the magnetometer emphasis factor, ρ, should be tuned by eyeball in a gien experimental setting. Experience with this approach to date suggests that f c should generally be somewhat less than the lowest significant freuency of linear acceleration resulting from limb segment motion. A more detailed discussion of guidelines for choosing a alue for k, along with preliminary experimental results of human arm tracking can be found in [Bachmann, 999]. In these experiments, for the MARG sensor used, and for the type of motion tracked, alues of k somewhere in the range. < k <. were found to be appropriate. Much more research on this issue in a ariety of experimental contexts is still needed. As a final remark, it should be recognized that Figure and Figure represent continuous time or analog realizations of orientation uaternion tracking. While this would be possible if the X matrix were constant, in fact it is not and must be reealuated on eery cycle of Gauss-Newton iteration. This means that only a discrete time or digital filter is possible. This represents no problem if the integration in these figures is simply approximated by a discrete sum, proiding that the update cycle time is short enough. The fact that Figure is linear allows this issue to be treated analytically [Bachmann, ], but such analysis is beyond the scope of this paper. 8. Summary and Conclusions While sourceless tracking of human head motion using only the graitational and magnetic fields of the Earth as orientation references has been accomplished and commercialized [Intersense Website], there has been no corresponding success in whole body limb segment tracking. The authors of this paper beliee that success in such an endeaor reuires not only better sensors, but also a completely different approach to sensor data processing than has been used for head tracking and related ship and aircraft naigation systems. The primary purpose of the present paper is to present an efficient and robust algorithm for sourceless real-time tracking and subseuent display of human limb motion, which is free of the singularity problems and computational complexity of prior algorithms based on Euler angles or anatomical joint angles [Semwai, 998; Molet, 999]. The algorithm which has been discoered in the course of this research relies on the Orthogonal Quaternion Theorem, which is belieed to be a new result. This theorem both resoles the singularity problem of Gauss-Newton iteration applied to orientation uaternion tracking, and reduces the size of the associated regression matrix from 4 x 4 to x. Since inersion of this matrix is probably the most time consuming part of Gauss- Newton iteration, and matrix inersion is of O ( n complexity, where n is the size of the matrix, the computational adantages arising from the application of this theorem are 7

21 substantial, and are especially important when simultaneously tracking a large number of human limb segments. An important feature of the algorithm we hae deeloped is that it contains two scalar gain factors that allow tuning of the filter to fit a particular tracking situation. Guidelines for choosing alues for these parameters are proided, but it is belieed that final selection of gains is best accomplished by adjustment during the course of an experiment, analogous to the way the controls of a radio are adjusted by a listener to get the desired tone uality, speaker balance, etc. We think that this is a new idea with respect to sourceless tracking, and of sufficient importance that we hae decided to name our procedure the Tuneable Quaternion Tracker (TQT algorithm. The TQT algorithm is designed to function with nine-axis MARG sensors as described in the body of this paper and illustrated in Figure. Howeer, it is important to recognize that in static problems (or in situations where angular rates and linear accelerations are sufficiently low the angular rate sensing and integration part of this algorithm may not be needed. That is, another way of looking at the TQT algorithm is that the angular rate portion of the algorithm is needed only to uicken the response of the filter to compensate for the lag in Gauss-Newton estimation introduced by the scalar gain k in the drift correction feedback loop. If the actual motion being tracked is sufficiently slow, such uickening may not be needed. In such cases, rate sensing will not be reuired and the MARG sensor can be simplified to a six-axis deice. As a final remark, we are aware that this paper does not present any details regarding the actual implementation and tracking of human limb motions in real time. In fact, we hae only preliminary results in this area [Bachmann, 999], but an intensie effort in sensor improement and in full real-time software deelopment is currently underway at our institution [Bachmann, ]. The computer simulation results contained in this paper and our published results on human arm tracking [Bachmann, 999] encourage us to beliee that we will succeed in producing a ery effectie sourceless whole body tracking system prototype in the next seeral years of our research. As in the our simulation studies reported here, we intend to continue to pursue an open source policy in this research, and inite all interested parties to join us in this work. We also realize that the linearization of the TQT filter presented in Figure, along with the Orthogonal Quaternion Theorem (which allows modeling of orthogonal noise so that the constraint of unit uaternions can be maintained een with sensor noise, permits the scalar gain k to be replaced by a Kalman filter gain matrix. We are not sure of the practicality of such a modification, but this is a topic we intend to pursue and would likewise be pleased to hae others join us in this effort 9. Acknowledgements The authors wish to acknowledge the support of the U. S. Army Research Office (ARO under Proposal No. 44-MA and the U. S. Nay Modeling and Simulation Management Office (N6M for making this work possible. 8

22 References Bachmann, E.R., et al., Orientation Tracking for Humans and Robots Using Inertial Sensors, Proc. Of 999 IEEE International Symposium on Computation in Robotics and Automation, CIRA 99, Noember 8-9, 999, Monterey, California, USA, pp Bachmann, E.R., Inertial and Magnetic Angle Tracking of Limb Segments for Inserting Humans into Synthetic Enironments, Ph.D. dissertation, Naal Postgraduate School, Monterey, CA994, December,. Bekey, G.A., and McGhee, R.B., Gradient Methods for the Optimization of Dynamic System Parameters by Hybrid Computation, in Computing Methods in Optimization Problems, ed. by A.V. Balakrishnan and L.W. Neustadt, Academic Press, New York, 964, pp Brown, R.G., and Hwang, P.Y.C., Introduction to Random Signals and Applied Kalman Filtering, rd Edition, John Wiley and Sons, New York, 997. Cooke, J.M., Zyda, M.J., Pratt, D.R., and McGhee, R.B., NPSNET: Flight Simulation Modeling Using Quaternions, Presence, Vol., No. 4, Fall, 99, pp Cormen, T.H., Leiserson, C.E., and Riest, R.L., Introduction to Algorithms, McGraw- Hill, New York, 994. Eeritt, B.S., Introduction to Optimization Methods and Their Application in Statistics, Chapman and Hall, New York, 987. Frey, W. Application of Inertial Sensors and Flux-Gate Magnetometers to Real-Time Human Body Motion Capture, M.S. thesis, Naal Postgraduate School, Monterey, CA994, September, 996. Foxlin, E., An Inertial Head-Orientation Tracker with Automatic Drift Compensation for Use with HMD s, Proc. Virtual Reality Software and Technology, VRST 94, edited by G. Singh, S.K. Feiner, and D. Thalmann, World Scientific, Singapore, 994. Also see Hartley, H.O., The Modified Gauss-Newton Method for the Fitting of Nonlinear Regression Functions by Least Suares, Technometrics, Vol., No., pp. 69 8, May, 96. Henault, G.A., A Computer Simulation Study and Computational Ealuation for a Quaternion Filter for Sourceless Tracking of Human Limb Segment Motion, M.S. thesis, Naal Postgraduate School, Monterey, CA994, March, 997. Kuipers, J.B., Quaternions and Rotation Seuences, Princeton Uniersity Press, Princeton, NJ,

23 Lawrence, A., Modern Inertial Technology, nd Edition, Springer-Verlag, New York, 998. Leine, A., Theory of Probability, Addison-Wesley, Reading, Mass., 97. McGhee, R.B., Identification of Nonlinear Dynamic Systems by Regression Analysis Methods, Ph.D. dissertation, Uniersity of Southern California, 96. Aailable from McGhee, R.B., Some Parameter-Optimization Techniues, Chapter 4.8, Digital Computer User s Handbook, ed. by M. Klerer and G.A. Korn, 967, pp McGhee, R.B., Bachmann, E.R., and Zyda, M.J., Rigid Body Dynamics, Inertial Reference Frames, and Graphics Coordinate Systems: A Resolution of Conflicting Conentions and Terminology, submitted to Presence, June,. Aailable at McGhee, R.B., et al., An Experimental Study of an Integrated GPS/INS System for Shallow-Water AUV Naigation Systems (SANS, Proc. Of 9 th International Symposium on Unmanned Untethered Submersible Technology, Marine Systems Engineering Laboratory, Northeastern Uniersity, Sept.5 7, 995, pp Meyer, K., Applewhite, H.L., and Biocca, F.A., A Surey of Position Trackers, Presence, Spring, 99, Vol., No., pp. 7. Molet, T., Boulic, R., and Thalman, D., Human Motion Capture Drien by Orientation Measurements, Presence, Vol. 8, No., April, 999, pp. 87. Precision Naigation, Inc., TCM Compass Product Specifications, Precision Naigation, Inc., Santa Rosa, CA954,. Aailable at Rust, B.W., and Burrus, W.R., Mathematical Programming and the Numerical Solution of Linear Euations, Elseier, New York, 97. Semwai, S.K., Hightower, R., and Stansfield, S., Mapping Algorithms for Real-Time Control of an Aatar Using Eight Sensors, Presence, Vol. 7, No., February, 998, pp.. Strang, G., Linear Algebra and Its Applications, rd Edition, Harcourt College Publishers, San Diego, CA, 988. Teegarden, D., Lorenz, G., and Neul, R., How to Model and Simulate Microgyroscope Systems, IEEE Spectrum, July, 998, pp

24 Yun, X., et al., Testing and Ealuation of an Integrated GPS/INS System for Small AUV Naigation, IEEE Journal of Oceanic Engineering, Vol. 4, No., July, 999, pp Web site reference. Appendix A: Deriation of X-Matrix for Quaternion Filter α Suppose is a unit uaternion and Then from E. (5: and ~ α ~, where α is any non-zero scalar. (A- α α (A- α * ~ ~ This result shows that there is no reuirement for orientation uaternions to be unit uaternions. As shown in the following paragraph, this fact can be used to derie the X matrix appearing in Figure. Suppose ( is an arbitrary non-zero uaternion. Then, by definition, so from the product rule of differential calculus u ( (A- i i (A-4 and i i (A-5 From Figure, for the uaternion filter, y (ˆ is gien by y( ˆ ( ˆ m ˆ, ˆ n ˆ T (A-6 where it is understood that the comma in this expression denotes concatenation and that only the ector part of the indicated expressions is used. Thus y (ˆ is a six-row column ector, the same as y. Using this result, together with E. (A-5 aboe, it follows from the product rule that

25 ( m i i m m i (A-7 and similarly for the last three components of y (ˆ. To complete the deriation of X, it is only necessary to note that, from E. (8 and (9, w ( (A-8 x ( i (A-9 y ( j (A- z ( k (A- Now if it is recognized that each column of X can be written as a six dimensional ector, then E. (A-7 through (A- define X. ANSI Common Lisp code for computing X is aailable at [web site], and proides a conenient executable specification for the details of this result. This code also includes a means of ealuating the X matrix by numerical differentiation of the computed measurement ector, y( ˆ. These two methods gie essentially identical results, lending considerable credence to both the aboe analysis and the associated Lisp code. Appendix B: Orthogonal Quaternion Theorem Theorem: Let p and be any two uaternions whose dot product is eual to. Such uaternions are said to be orthogonal. For all such p and, p where is a uniue ector and is gien by p. Proof: If and are written using flag notation, then (B- i j k and i (B- j k

26 Multiplying using flag algebra, and collecting terms, eidently: ( ( ( ( k j i p (B- Thus, the dot product of and p is gien by p (B-4 which proes orthogonality. For the second part of the theorem, p (B-5 which is uniue. Q.E.D.

27 INITIAL DISTRIBUTION LIST. Defense Technical Information Center 875 John J. Kingman Rd., STE 944 Ft. Beloir, VA Dudley Knox Library, Code Naal Postgraduate School Monterey, CA Research Office, Code 9 Naal Postgraduate School Monterey, CA