A Novel Procedure for In-field Calibration of Sourceless Inertial/Magnetic Orientation Tracking Wearable Devices D. Campoloa , M. Fabrisa, G. Cavalloa, D. Accotob , F. Kellerc , E. Guglielmellia a Biomedical Robotics & EMC Lab Neuroscience and Neural Plasticity Lab Università Campus Bio-Medico via Longoni, 83 00155 Roma - Italy c Developmental Abstract— Recent research in the emerging field of Phenomics aims at developing unobtrusive and ecological technologies which allow monitoring the behavior of infants and toddlers. Orientation tracking devices based on accelerometers and magnetometers represent a very promising technology since orientation in 3D space can be derived by solely relying upon the direction of the natural geomagnetic and gravitational fields which constitute an absolute coordinate frame of reference, i.e. sourceless. Many commercially available devices allow on-board calibration by means of addition of external circuitry, mainly used to generate artificial fields which act on the sensor itself as a known forcing input. Addition of external circuits is a major drawback in applications such as the one of interest, where the technology has to be worn by infants. When external fields, (e.g. gravitational and geomagnetic fields) are present, alternative calibration techniques are possible which rely on predefined orientation sequences of the sensor. In standard procedures, prior knowledge of the external field (magnitude and direction) as well as accuracy in performing the predefined orientation sequences contribute to determine the calibration parameters. In this work, a novel procedure for in-field calibration of magnetometric sensors is presented which does not rely on previous knowledge of magnitude and direction of the geomagnetic field and which does not require accurately predefined orientation sequences. Such a method proves especially useful in clinical applications since the clinician is no longer compelled to execute accurate calibration protocols. Index Terms— Wearable Technology, Posture Tracking, Movement Analysis, Inertial/Magnetic Sensors. I. I NTRODUCTION Motion tracking finds application in a broad set of fields, ranging from virtual reality to medicine, from film industry to biomedical research. As shown in [1] and references therein, although no “silver bullet” exists, motion tracking can count on a “respectable arsenal” of available technologies, each with its own advantages and disadvantages with respect to a given application. At Campus Bio-Medico University, research is being carried out in the emerging field of Phenomics [2] with the aim of developing unobtrusive and ecological technologies corresponding author: d.campolo@unicampus.it b CRIM Center for Research in Microengineering Scuola Superiore Sant’Anna P.zza Martiri della Libertà, 33 56127 Pisa - Italy which allow monitoring the behavior of infants and toddlers, especially during their first two years of life. Behavior monitoring includes, for example, tracking a child’s posture, tracking the head direction (which mainly relates to the child’s attention), tracking position and/or orientation of toys while the child plays with them, etc. . . At this moment, the most suitable technologies are being screened for this purpose. The selected ones will be deployed to be embedded in a child’s everyday environment, e.g. in toys and clothes. Special attention is paid to technologies which do not require costly equipment (e.g. photogrammetric systems) and/or a structured environment (e.g. motion analysis laboratories). The technology of interest should be able to work in clinical settings as well as at home and should be operated by minimally trained personnel, e.g. the child’s caregiver. In this sense, orientation tracking based on inertial/magnetic sensors [3], [4], [5] represents a promising technology since, as shown in next sections, orientation of a rigid body can be measured solely relying upon gravitational and geomagnetic fields, which are present everywhere on earth, without the need of other sources of fields, i.e. sourceless. Accelerometers and magnetometers are nowadays available in packages small enough to be worn or embedded into toys and can be used to track position/orientation in unstructured environments. Section II develops the mathematics needed to relate the inertial/magnetic sensors read-outs to the spatial orientation of the system itself. Emphasis is given to the geometrical description since geometrical considerations are also at the basis of the proposed calibration procedure. Orientation in space is described in terms of rotation matrices, fully exploiting the geometry of the problem. An invariant, i.e. coordinate-free, formulation has been adopted which presents, to the authors’ knowledge, a certain degree of novelty, at least when applied to compass-based navigation systems [6], [7]. In Section III, the proposed calibration procedure is described. Section IV presents the experimental verification of the proposed calibration procedure and the discussion of the results. Finally, conclusive remarks are provided in Section V. II. S OURCELESS I NERTIAL /M AGNETIC O RIENTATION T RACKING Accelerometers, in static conditions, directly provide a measure of the gravity vector g which is always vertical with respect to the earth surface. Magnetometers measure the geomagnetic field b when no other magnetic source is present. Geomagnetic field has an horizontal component b which points towards North and a vertical component b ⊥ which depends on the latitude. Define now a third vector: ∆ (1) h =g×b where “×” is the vector product in 3D Euclidean space. It is worth noting that h is never null (since g and b are never collinear) and always points towards East. The three vectors g, b, and h are therefore independent and can be used to define a convenient fixed coordinate frame {x 0 , y0 , z0 }: - z0 : unitary vector pointing Up - x0 : unitary vector pointing East - y0 : unitary vector pointing North This can be expressed in invariant (i.e. valid in every coordinate system) geometrical terms: ∆ z0 = −g/g x0 = h/h y0 = z0 × x0 = h × g/h × g ∆ ∆ where g = g ≈ 9.8 m/s2 . It is worth noting how components simplify with respect to the fixed coordinate frame: g = b = h = 0 x0 0 x0 h x0 + + + 0 y0 b y 0 0 y0 − + + g z0 b⊥ z0 0 z0 (2) where h, the scalar equivalent of Eq.(1), is defined as: ∆ h = g b (3) Let now {x1 , y1 , z1 } be an orthonormal frame (referred to as moving frame) defined via the sensitive axes of the sensorized system (the tilted box in Fig. 1) whose orientation should be determined with respect to the fixed frame. With reference to Fig. 1, the fixed frame {x 0 , y0 , z0 } and the moving frame {x 1 , y1 , z1 } are related by a rototranslation. The translation is ignored hereafter (the origins of the two coordinate frames shall always be imagined as coincident) since only relative orientation is of interest. The rotation can be expressed by a matrix R defined as: 0 1 x · x x0 · y1 x0 · z1 (4) R = y0 · x1 y0 · y1 y0 · z1 z0 · x1 z0 · y1 z0 · z1 R Up North z0 East y0 y1 z1 x1 x0 Fig. 1. Fixed and moving coordinate frames. The rotation matrix R can be viewed as: i) an operator which transforms each versor of the fixed frame (x0 , y0 , z0 ) into the corresponding versor of the moving frame (x1 , y1 , z1 ); ii) a change of coordinates mapping components of a vector expressed in the moving frame into the components of same vector expressed with respect to the fixed frame. where “·” represents the Euclidean scalar product. A peculiar property of rotation matrices is that the inverse and the transpose coincide, i.e.: 0 1 x · x y0 · x1 z0 · x1 R−1 = RT = x0 · y1 y0 · y1 z0 · y1 (5) x0 · z1 y0 · z1 z0 · z1 Accelerometers and magnetometers are displaced along with the moving frame and provide read-outs which correspond, respectively, to the three components (g x , gy , gz ) of the gravitational field 1 and to the three components (b x , by , bz ) of the magnetic field with respect to moving coordinate frame. By the invariance of Eq.(1), auxiliary read-outs (h x , hy , hz ) with respect to the moving frame can readily be defined: hx gx bx gy bz − gz by hy = gy × by = gz bx − gx bz (6) hz gz bz gx by − gy bx Geometrically, each gx = g · x1 gy = g · y 1 gz = g · z1 which can gx gy gz be expressed as: hx = h · x1 hy = h · y 1 hz = h · z1 be compacted into an equivalent g · x1 b · x1 b x hx b y hy = g · y 1 b · y 1 b z hz g · z1 b · z1 By substituting gx gy gz or equivalently: R−1 set of read-outs can bx = b · x1 by = b · y 1 bz = b · z1 matrix notation: h · x1 h · y1 (7) h · z1 Eq.(2), Eq.(7) can be rewritten b x hx 0 0 by hy = R−1 0 b b z hz −g b⊥ gx = gy gz bx by bz hx 0 hy ∗ 0 −g hz 0 b b⊥ as: h 0 0 (8) −1 h 0 0 where “∗” is usual the matrix product. 1 In static conditions the gravitational field is the only source of acceleration. In general, the apparent acceleration should also be considered but in applications such as the one of interest it can be negligible, see [3]. Eq.(8) simply states that R −1 , besides being an operator which rotates vectors, can also be interpreted as a change of coordinates which transforms components of a vector expressed in the fixed frame into components of same vector expressed with respect to the moving frame. Recalling that R = (R−1 )T for rotation matrices and that h was defined in Eq.(3), the following holds: hy hz hx 1 R = gx b⊥ + g bx gy b⊥ + g by gz b⊥ + g bz (9) h −gx b −gy b −gz b with respect to the fixed frame (i.e. [0 b b⊥ ]T ) are constant throughout space 2 . As the device is oriented in space, the moving reference attached to it (refer to Fig. 1) is also subjected to the same orientation and the field components in the moving frame (i.e. [b x by bz ]T ) change accordingly. As pointed out in the previous section, this change of coordinates is fully determined by the rotation matrix R. Rotations in space are linear transformations which act on vectors preserving their modules (a major property of rotation matrices): Eq.(9) allows determining the rotation matrix R once the sensors read-outs (g x , gy , gz ) and (bx , by , bz ) are known, remaining values (h x , hy , hz ) can be computed via Eq.(6). b2x + b2y + b2z = constant = b2 + b2⊥ III. M AGNETOMETER C ALIBRATION P ROCEDURE In the previous section, it was implicitly assumed that accelerometers and magnetometers would provide direct measurement of the components of the gravitational fields (gx , gy , gz ) and of the geomagnetic field (b x , by , bz ). In fact sensors are just transducers and provide an output voltage v that, in the best scenario, is proportional to variations of the measurand m. In practical terms: v = k m + v o , where k and vo respectively represent the linear gain and the offset value. A calibration procedure is needed to determine such coefficients in order to derive the measurand m = (v −v o )/k, i.e. the fields components to be used in formulas such as Eq.(9). Parameters vo and k are easily determined when situations exist where the measurand assumes (at least) two known values. In the case of accelerometers, the measurand can easily assume values 0, +g and −g by simply aligning (e.g. by means of mechanical set-ups such as a pendulum) the sensor’s axis, respectively, orthogonally, parallel and anti-parallel with the vertical direction. When it comes to the magnetic field, alignment of the sensor’s axis with the field’s direction is not straightforward. For this reason an additional field has to be generated. Commercially available devices such as the Honeywell HMC105X, a family of multi-axes magneto-resistive sensors, contain purposefully designed “offset straps”, i.e. spirals of metallization that couple with each sensitive axis of the device producing an additional magnetic field. Such patented feature can be used for auto-calibration of the sensor. Such procedure requires addition of extra circuitry used to drive each offset strap. In applications where infants are supposed to wear such technology, reduction of components is highly desirable. For this reason a procedure which would only rely on the natural geomagnetic field distribution and that would not require any accurate alignment was investigated, as presented in the following. A. Proposed Calibration Procedure The proposed procedure solely relies upon uniformity of the natural geomagnetic field. This means that components This means that, as the device is being displaced and oriented in space, the geomagnetic vector is seen by the moving frame as a time-varying vector of constant module, i.e. the trajectory of its end-point (b x , by , bz ) is bound to stay on a sphere centered in the origin and of radius: b = b2 + b2⊥ . (10) In terms of sensor output voltages (v x , vy , vz ), considering different offset voltages (v ox , voy , voz ) and different linear gains (kx , ky , kz ) for each axis, the sphere becomes now an off-centered ellipsoid: vx − vox kx 2 + vy − voy ky 2 + vz − voz kz 2 = b2 or equivalently: vx − vox b kx 2 + vy − voy b ky 2 + vz − voz b kz 2 = 1 (11) Such an ellipsoid is uniquely identified by the six parameters (vox , voy , voz , b kx , b ky , b kz ), the first three identify the center of the ellipsoid, the remaining three identify the semi-axis length of the ellipsoid. At least six independent equations are needed in order to determine the unknown parameters. Such equations can be derived as part of the calibration procedure: a sequence of N orientations in space allows measuring the sensor read-outs (vxi , vyi , vzi ) relatively to each orientation i = 1 . . . N . As a result of such measurements, a set of N equations becomes available, the i-th equation being: vxi − vox b kx 2 + vyi − voy b ky 2 + vzi − voz b kz 2 = 1 (12) Because of the errors (measurement noise) affecting the sensor read-outs, least-squares fitting methods applied to a larger number (N 6) of measurements can be deployed to obtain an estimate of the six unknown parameters which is less sensitive to measurement noise. 2 Such requirement is not too strict although some care should be taken, refer to [3] for details of usage in clinical practice. + p N 3 The 4 It’s appendix at the end of this paper provides explanation for this choice. worth recalling that translations do not influence the sensors. z−axis y−axis x−axis 2.3 2.2 2.1 2 1.9 1.8 i=1 A. Experimental Setup In order to validate the previously described calibration procedure, an experimental setup consisting of: - one HMC1051: a 1-axis magnetometer sensing the field in the z−direction; - one HMC1052: a 2-axis magnetometers sensing the field in the xy−plane; - three amplifying stages: one for each axis; was developed. Fig. 2 shows how the signal coming from each axis of the device was amplified by means of an operational amplifier in a differential configuration. The device was placed in a wooden box and randomly (manually) moved around in 3D space for a few seconds while amplified data were being acquired and stored on a computer for later processing. Care was taken not to simply translate 4 the box but to provide random orientation as well. Fig. 3 shows the experimental data as acquired voltages at the output of the three amplifying stages. R3 2.4 e2i (p) IV. E XPERIMENTAL V ERIFICATION In this section, the experimental setup and how the experimental data are derived is first discussed. Then the numerical procedure that implements the least-squares method to fit acquired data is presented. As a result, the six parameters defining the ellipsoid that best fits the experimental data are determined together with the statistics of the fitting error. R2 Rd 1.7 1.6 0 100 Fig. 3. 200 300 400 number of acquisitions 500 600 Measurement sequence: 3-axis sensor’s amplified read-outs. 2.3 2.3 2.2 2.2 2.1 2.1 x min output + 2.5 c2 (13) where p = [x0 y0 z0 a b c]T is the vector of 6 unknown parameters which fully determine the ellipsoid. As shown next, such vector p will be eventually determined by numerically solving: _ x b2 R1 Fig. 2. Differential amplification (R1 = R2 = 2.2 kΩ and R3 = R4 = 150 kΩ) of the magneto-resistive bridge (Ra = Rb = Rc = Rd = 850 Ω, nominal values) relative to a single axis. output voltage [V] + R4 Rc Rb v [V] a2 Ra v [V] (xi − x0 )2 (yi − y0 )2 (zi − z0 )2 + + −1=0 2 2 a b c2 where i = 1 . . . N and where obvious changes of notations (e.g. vxi → xi , vox → x0 , b kx → a, etc. . . ) are made. Least-squares fitting is probably one of the most widely used approaches for estimating ellipses’ parameters [9]. In general there exist no exact solution satisfying all of the N equation, what can be found is an estimate which minimizes a given error-of-fit (EOF) function. Several choices are possible for the EOF, see [9] for details. In this paper, the following EOF was chosen 3 : ∆ ei (p) = (xi − x0 )2 + (yi − y0 )2 + (zi − z0 )2 · 1 1 − (xi −x0 )2 (yi −y 2 (zi −z0 )2 0) +5V Magnetoresistive bridge B. Least-Squares Fitting For sake of clarity, rewrite the N nonlinear equations in Eq.(12) as: 2 2 1.9 1.9 2.2 2.1 2.1 2 2 1.9 1.9 1.8 vy [V] 1.7 1.8 1.9 1.8 vy [V] v [V] z 1.7 1.9 1.8 v [V] z Fig. 4. Left: “cloud” of measurements, i.e. the trajectory of measurement sequences in 3D space. Right: best fitting ellipsoid (thin lines) superimposed with cloud of measurements (thick lines). B. Numerical Procedure Considering the 3D space of measurements (x i , yi , zi ), the sequence of N measurements appears as a cloud of points (see the left plot in Fig. 4) distributed along the surface of the ellipsoid yet to be determined (presence of measurement noise causes these points not to perfectly lie on the surface). In order to numerically determine the ellipsoid (i.e. its six parameters), calculations were carried out in the MATLAB environment, specifically making use of lsqnonlin (a MATLAB function for nonlinear least-squares problems). Such a function requires two input arguments: - the EOF: implemented as another function returning an N dimensional vector [e 1 (p) e2 (p) . . . eN (p)]T , here each ei (p) is defined as in Eq.(13); - an initial guess p0 of the unknown parameter p. As an initial guess for first three parameters, the coordinates of center of mass of the cloud of measurements were chosen (every measurement is given a unitary mass). For the remaining three parameters (representing the semi-axis length of the ellipsoid), the mean radius between the previously guessed center of mass and the measured data points was chosen. In particular: ∆ p0 = [x̄ ȳ z̄ r̄ r̄ r̄ ]T where x̄, ȳ, z̄, and r̄ are defined as follows: x̄ = N 1 xi ; N i=1 r̄ = ȳ = N 1 yi ; N i=1 z̄ = N 1 zi ; N i=1 N 1 (xi − x̄)2 + (yi − ȳ)2 + (zi − z̄)2 N i=1 Remark: a “good” initial guess leads the lsqnonlin to quickly converge towards the optimal solution. The more uniform is the distribution of the measurements on the ellipsoid, the closer is the center of mass (x̄, ȳ, z̄) to the center of the ellipsoid. A “good” guess of the center of mass also leads to a “good” guess of the radius. The proposed initial guess together with the EOF proposed in Eq.(13) led the lsqnonlin to converge to the vector: p = [2.07 1.94 1.87 0.27 0.27 0.07]T (V olts) The relative ellipsoid (centered in x 0 = 2.07 V , y0 = 1.94 V , z0 = 1.87 V and with semi-axis lengths a = b = 0.27 V and c = 0.7 V ) is shown in the right plot of Fig. 4, where the cloud of measurements is also superimposed. The goodness of the fitting, besides the graphical representation, can be inferred by statistics of the errors e i (p): - average: −0.38 mV - RMS: 0.15 V C. Discussion of Results The best fitting ellipsoid, besides being off-centered due to bridge offsets, revealed different sensitivities among different axes. In particular, the z-axis (c = 0.07 V ) is much less sensitive than the x and y axis. This is due to the fact that an HMC1051 was used to sense the field in the z-axis while an HMC1052 was used for the remaining ones. In practical situations, different devices may display different sensibilities for a variety of reasons. The proposed calibration procedure proved capable of overcoming such problems. In order to guarantee convergence of the least-squares algorithm, as previously remarked, no restriction is posed on the calibration trajectory of measurements except for the uniformity of its distribution. This directly translates into a clinical protocol which requires the human operator (who, for example, wears such devices on his/her upper limbs) to perform a set of predetermined actions (lifting an arm, pointing right or left etc. . . ) which need to be only qualitatively described, i.e. a procedure which is more suitable to a clinical practice. V. C ONCLUSION AND F UTURE W ORK In this work a novel procedure for in-field calibration of inertial/magnetic wearable devices for orientation tracking was presented. Although several techniques for calibrating such devices already exist, a novel method was investigated with the specific aim of being deployed in clinical practice, where existing procedures often prove impractical and lead to disuse. Emphasis was placed on the fact that a human operator would perform the orientation sequence needed for calibration and that such orientation sequence should be only qualitatively described, requiring no particular dexterity or performance accuracy. A geometrical description of the problem was used to show that, although randomly generated, all the measurements were expected to lie on the surface of an ellipsoid. The least-squared fitting method was used to determine such an ellipsoid described by six parameters representing three offsets and three gains values needed for calibrating a 3-axis magnetic device (this work focused on magnetometers since accelerometers generally pose much fewer problems). Accurate calibration was finally obtained by having a large number of random orientation sequences rather than a few accurately performed ones. To this end, the least-squares fitting approach proved essential both to exploit the large number of measurements and to gain robustness with respect to measurement noise. As future work, robustness of the proposed calibration procedure will be statistically tested against several clinical protocols (of increasing complexity) performed by minimally trained personnel. ACKNOWLEDGEMENT The authors would like to thank Oliver Tonet for useful discussions on least-squares methods. A PPENDIX E RROR - OF -F IT F UNCTION In this section a brief explanation of the reasons behind the choice of Eq.(13) as a candidate error-of-fit function is provided. For sake of simplicity, the problem will be stated in terms of ellipses (2D) since generalization to ellipsoids in 3D space is straightforward. (xP,yP) A (xo,yo) P B O Fig. 5. Distance of a point P from an ellipse: although BP represents the true geometrical distance, AP is used instead as an analytically convenient approximation. The purpose of an error-of-fit function is defining a sort (not necessarily positive definite) of distance of a generic point from an ellipse. With reference to Fig. 5, for a point P of coordinates (xP , yP ) the most intuitive choice would be the geometrical distance BP . Unfortunately for a generic point P off the ellipse, the analytical derivation of B is not so straightforward. As an analytically convenient approximation, the length of AP can be used instead, where the point A is simply where the ray OP intersects the ellipse. Define the following function: 2 2 x − xO y − yO ∆ + F (x, y) = a b and let F (x, y) = 1 be the equation of the ellipse. A generic point P of coordinates (x P , yP ) will therefore lie on the ellipse if F (xP , yP ) = 1, outside the ellipse if F (xP , yP ) > 1 and inside the ellipse if F (xP , yP ) < 1. Since by definition the point A lies on the ellipse, it is straightforward computing its coordinates (x A , yA ): 1 xA xO xP − xO = + yA yO F (xP , yP ) yP − yO The length of OA is simply: xO )2 + (yA − yO )2 dOA = (xA − 1 = F (xP ,yP ) (xP − xO )2 + (yP − yO )2 = F (xP1,yP ) dOP Since O, A and P are collinear, d AP = dOP − dOA , i.e.: 1 2 2 dAP = (xP − xO ) + (yP − yO ) 1 − F (xP , yP ) Eq.(13) is simply a generalization of the previous formula to the 3D case. Note: dAP is not strictly speaking a distance because it is not positive definite but it does not matter since in the leastsquares method its squared value will be used instead. R EFERENCES [1] G. Welch and E. Foxlin, “Motion Tracking: No Silver Bullet, but a Respectable Arsenal”, IEEE Computer Graphics and Applications, Vol. 22, No. 6, pp. 24-38, 2002. [2] R. Gerlai, Phenomics: fiction or the future?, in Trends Neurosci., Vol.25, No. 10, pp. 506-9, 2002. [3] B. Kemp, A. J. M. W. Janssen, B. van der Kamp, “Body position can be monitored in 3D using miniature accelerometers and earth-magnetic field sensors”, Electroencephalography and Clinical Neurophysiology, Vol. 109, pp. 484-488, 1998. [4] E. R. Bachmann, X. Yun, D. McKinney, R. B. McGhee, M. J. Zyda, “Design and Implementation of MARG Sensors for 3-DOF Orientation Measurement of Rigid Bodies”, Proc. of the 2003 IEEE Intl. Conf. on Robotics and Automation (ICRA 2003), Taipei, Taiwan, September 1419, 2003. [5] R. Zhu and Z. Y. Zhou, “A Real-Time Articulated Human Motion Tracking Using Tri-Axis Inertial/Magnetic Sensors Package”, IEEE Trans. on Neural Systems and Rehabilitation Engineering, Vol. 12, No. 2, 2004. [6] M. J. Caruso, “Applications of Magnetoresistive Sensors in Navigation Systems” Sensors and Actuators, SAE SP-1220, pp. 15-21, 1997. [7] M. J. Caruso, “Applications of magnetic sensors for low cost compass systems”, in Proc. IEEE Position Location and Navigation Symp., pp. 177-184, Mar. 2000 [8] F. Ferraris, U. Grimaldi, M. Pavis, “Procedure for Effortless In-Field Calibration of Three-Axis Rate Gyros and Accelerometers”, Sensors and Materials, Vol. 7, No. 5, pp. 331-330, 1995. [9] P. L. Rosin, “Assessing Error of Fit Functions for Ellipses”, Graphical Models and Image Processing, Vol. 58, No. 5, pp. 494-502, 1996.