The present inventions generally relate to apparatuses and methods for tracking and compensating for time varying magnetic fields (near fields) with respect to an earth-fixed reference coordinate system in a system including a magnetometer and motion sensors.
The increasingly popular and widespread mobile devices frequently include so-called nine-axis sensors which consist of a 3-axis gyroscope, a 3-D accelerometer and a 3-D magnetometer. The 3-D gyroscope measures angular velocities. The 3-D accelerometer measures linear acceleration. The magnetometer measures a local magnetic field vector (or a deviation thereof). In spite of their popularity, the foreseeable capabilities of these nine-axis sensors are not fully exploited due to the difficulty of calibrating and removing undesirable effects from the magnetometer measurements on one hand, and the practical impossibility to make a reliable estimate of the yaw angle using only the gyroscopes and the accelerometer.
A rigid body's (i.e., by rigid body designating any device to which the magnetometer and motion sensors are attached) 3-D angular position with respect to an Earth-fixed gravitational orthogonal reference system is uniquely defined. When a magnetometer and an accelerometer are used, it is convenient to define the gravitational reference system as having the positive Z-axis along gravity, the positive X-axis pointing to magnetic North and the positive Y-axis pointing East. The accelerometer senses gravity and other acceleration, while from magnetometer's measurement it can be inferred from the Earth's magnetic field that points to North (although it is known that the angle between the Earth's magnetic field and gravity may be different from 90°). This manner of defining the axis of a gravitational reference system is not intended to be limiting. Other definitions of an orthogonal right-hand reference system may be derived based on the two known directions, gravity and the magnetic North.
Motion sensors attached to the 3-D body measure its position (or change thereof) in a body reference system defined relative to the 3-D body. For example, as illustrated in
Based on Euler's theorem, the body reference system and the gravitational reference system (as two orthogonal right-hand coordinate systems) can be related by a sequence of rotations (not more than three) about coordinate axes, where successive rotations are about different axis. A sequence of such rotations is known as an Euler angle-axis sequence. Such a reference rotation sequence is illustrated in
A 3-D magnetometer measures a 3-D magnetic field representing an overlap of a 3-D static magnetic field including geomagnetic field (e.g., Earth's magnetic field), hard- and soft-iron effects, and a 3-D dynamic near field due to external time-varying electro-magnetic fields. The measured magnetic field depends on the actual orientation of the magnetometer. If the hard-iron effects, soft-iron effects and dynamic near fields were zero, the locus of the measured magnetic field (as the magnetometer is oriented in different directions) would be a sphere of radius equal to the magnitude of the Earth's magnetic field. The non-zero hard- and soft-iron effects render the locus of the measured magnetic field to be an ellipsoid offset from the origin.
Hard-iron effect is produced by materials that exhibit a constant magnetic field in magnetometer's body coordinate system, thereby generating constant offsets of the components of the measured magnetic field. As long as the orientation and position of the sources of magnetic field due to the hard-iron effects relative to the magnetometer is constant, the corresponding offsets are also constant.
Unlike the hard-iron effect that yields a magnetic field overlapping the Earth's field, the soft-iron effect is the result of material that influences, or distorts, a magnetic field (such as, iron and nickel), but does not necessarily generate a magnetic field itself. Therefore, the soft-iron effect is a distortion of the measured field depending upon the location and characteristics of the material causing the effect relative to the magnetometer and to the Earth's magnetic field. Thus, soft-iron effects cannot be compensated with simple offsets, requiring a more complicated procedure. Parameters for compensating for the hard-iron effect and the soft-iron effect can be calibrated by sampling measurements from magnetometer at different orientations in a absence of time-varying magnetic near-field.
The magnetic near fields are dynamic distortions of a measured magnetic field due to time-varying magnetic fields. The time-varying magnetic field in earth-fixed coordinate system may significantly affect measurements of the magnetometer. Such magnetic near fields can be generated by an earphone, a speaker, a cell phone, a vacuum cleaner, etc. In absence of a reliable estimate for the yaw from three-axis accelerometer and three-axis rotational sensor (e.g., the yaw angle drift problem due to no observation on absolute yaw angle measurement), a magnetic near field compensated magnetometer's measurement can provide an important reference making it possible to correct the yaw angle drift.
Therefore, it would be desirable to provide devices, systems and methods that enable real-time reliable use of a magnetometer together with other motion sensors attached to a device for determining orientation of the device (i.e., angular positions including a yaw angle), while avoiding the afore-described problems and drawbacks.
Devices, systems and methods using concurrent measurements from a combination of sensors including a magnetometer yield a local 3-D magnetic field value and then a corrected value of a yaw angle of a 3-D body.
According to one exemplary embodiment, a method for tracking dynamic near fields and correcting a magnetic field measured together with an angular position in a body reference system having an unknown yaw offset relative to a gravitational reference system is provided. The method includes calculating a magnetic field difference between (1) a magnetic field in the gravitational reference system, evaluated based on the measured magnetic field and assuming that the angular position is accurate, and (2) a previous estimated total magnetic field including previous tracked near fields, in the gravitational reference system. The method further includes estimating current near fields to be a sum of the previous near fields and a portion of the calculated field difference. The method also includes computing a magnitude difference between magnitudes of a current estimated total magnetic field including the estimated current near-fields and the measured magnetic field, and an angular difference between (1) a first angle between the current estimated total magnetic field and a fixed vector in the gravitational reference system, and (2) a second angle between the measured magnetic field and the fixed vector expressed in the body reference system. The method further includes the magnitude difference and the angle difference with noise to determine whether the current measured magnetic field is consistent with the previously tracked magnetic near fields. The method further includes comparing the magnitude difference and the angle difference with noise to determine whether the current measured magnetic field is consistent with the previously tracked magnetic near fields. The method finally includes, if the comparing determines that the current measured magnetic field is consistent with the previously tracked magnetic near fields, updating (S450) the angular position using the current estimated total magnetic field, and correcting the measured magnetic field for the current near field effects using the updated angular position.
According to another exemplary embodiment, an apparatus configured to perform a method for tracking dynamic near fields and correcting a magnetic field measured together with an angular position in a body reference system having an unknown yaw offset relative to a gravitational reference system is provided. The apparatus includes an interface configured to receive the magnetic field and the angular position measured by a magnetometer and motion sensor attached to a device. The apparatus further includes a data processing unit configured (A) to calculate a field difference between (1) a magnetic field in the gravitational reference system, evaluated based on the measured magnetic field and assuming that the angular position is accurate, and (2) a previous estimated total magnetic field including previous tracked near fields, in the gravitational reference system, (B) to estimate current near fields to be a sum of the previous near fields and a portion of the calculated field difference, (C) to compute compute a magnitude difference between magnitudes of a current estimated total magnetic field including the estimated current near-fields and the measured magnetic field, and an angular difference between (1) a first angle between the current estimated total magnetic field and a fixed vector in the gravitational reference system, and (2) a second angle between the measured magnetic field and the fixed vector expressed in the body reference system, (D) to compare the magnitude difference and the angle difference with noise to determine whether the current measured magnetic field is consistent with the previously tracked magnetic near fields, and (E) if determined that the current measured magnetic field is consistent with the previously tracked magnetic near fields, to update the angular position using the current estimated total magnetic field, and to correct the measured magnetic field for the current near field effects using the updated angular position.
According to another exemplary embodiment, a computer readable medium storing executable codes which when executed by a processor make the processor execute a method of tracking for dynamic near fields and correcting a magnetic field measured together with an angular position in a body reference system having an unknown yaw offset relative to a gravitational reference system is provided. The method includes calculating a magnetic field difference between (1) a magnetic field in the gravitational reference system, evaluated based on the measured magnetic field and assuming that the angular position is accurate, and (2) a previous estimated total magnetic field including previous tracked near fields, in the gravitational reference system. The method further includes estimating current near fields to be a sum of the previous near fields and a portion of the calculated field difference. The method also includes computing a magnitude difference between magnitudes of a current estimated total magnetic field including the estimated current near-fields and the measured magnetic field, and an angular difference between (1) a first angle between the current estimated total magnetic field and a fixed vector in the gravitational reference system, and (2) a second angle between the measured magnetic field and the fixed vector expressed in the body reference system. The method further includes the magnitude difference and the angle difference with noise to determine whether the current measured magnetic field is consistent with the previously tracked magnetic near fields. The method further includes comparing the magnitude difference and the angle difference with noise to determine whether the current measured magnetic field is consistent with the previously tracked magnetic near fields. The method finally includes, if the comparing determines that the current measured magnetic field is consistent with the previously tracked magnetic near fields, updating (S450) the angular position using the current estimated total magnetic field, and correcting the measured magnetic field for the current near field effects using the updated angular position.
The accompanying drawings, which are incorporated in and constitute a part of the specification, illustrate one or more embodiments and, together with the description, explain these embodiments. In the drawings:
The following description of the exemplary embodiments refers to the accompanying drawings. The same reference numbers in different drawings identify the same or similar elements. The following detailed description does not limit the invention. Instead, the scope of the invention is defined by the appended claims. The following embodiments are discussed, for simplicity, with regard to the terminology and structure of a sensing unit including motion sensors and a magnetometer attached to a rigid 3-D body (“the device”). However, the embodiments to be discussed next are not limited to these systems but may be used in other systems including a magnetometer or other sensor with similar properties.
Reference throughout the specification to “one embodiment” or “an embodiment” means that a particular feature, structure, or characteristic described in connection with an embodiment is included in at least one embodiment of the present invention. Thus, the appearance of the phrases “in one embodiment” or “in an embodiment” in various places throughout the specification is not necessarily all referring to the same embodiment. Further, the particular features, structures or characteristics may be combined in any suitable manner in one or more embodiments.
According to an exemplary embodiment illustrated in
A body coordinate system may be defined relative to the device's body 101 (see, e.g.,
The interface 140 and the data processing unit 130 constitute a static magnetic field extracting unit 150. In
The signals reflect quantities measured in the body reference system. These measurements in the body reference system are further processed by the data processing unit 130 to be converted into quantities corresponding to a gravitational reference system. For example, using rotation sensors and a 3-D accelerometer, a roll and pitch of the body reference system to a gravitational orthogonal reference system may be inferred. In order to accurately estimate a yaw angle of the device in the gravitational orthogonal reference system, determining the orientation of the Earth's magnetic field from the magnetic field measured in the body's reference system is necessary.
For determining the orientation of the Earth's magnetic field from the magnetic field measured in the body reference system, the data processing unit 130 corrects the measured 3-D magnetic field (which has been calculated from magnetometer signals ideally using calibration parameters) for hard-iron effects, soft-iron effects, misalignment and near fields using various parameters in a predetermined sequence of operations. Once the data processing unit 130 completes all these corrections, the resulting magnetic field may reasonable be assumed to be a local static magnetic field corresponding to the Earth's magnetic field. The Earth's magnetic field naturally points to North, slightly above or below a plane perpendicular to gravity, by a known angle called “dip angle”.
A toolkit of methods that may be performed in the system 100 is described below. The data processing 130 may be connected to a computer readable medium 135 storing executable codes which, when executed, make the system 100 to perform one or more of the methods related to extracting a local magnetic field.
Methods for dynamic tracking and compensating for the dynamic magnetic near fields affecting a magnetometer measurement by using the 3-D angular position estimate of the magnetometer with respect to the Earth-fixed gravitational reference system are provided. The 3-D angular position is not perfectly accurate and can include errors in roll, pitch angles, and at least yaw angle drift and/or an unknown offset. The magnetic field measurement compensated for dynamic near fields is useful for compass or 3-D angular position determination for a better yaw estimation. No conventional methods capable to achieve similar results have been found.
According to exemplary embodiments,
The following Table 1 is a list of notations used to explain the algorithms related to the methods for tracking and compensating near fields
n
i
E
D
−1
T
EHtot
EH0
EHNF
EĤNF
E{tilde over (H)}NF
DBn
DB0
EH0 in device's body reference system
D{circumflex over (B)}0
DBNF
D{circumflex over (B)}NF
E
DRn
E
D{circumflex over (R)}n
EA
DA
EV
DV
EĤtot
EĤNF
EĤNF
E{tilde over (H)}NF
E
D{tilde over (R)}n
E{tilde over (V)}
EH0 (1),
EH0 (2)
When the magnetic field in the Earth-fixed gravitational reference system is constant, the magnetic field measured by the magnetometer in the device's body reference system can be used to determine the 3-D orientation (angular position) of the device's body reference system with respect to Earth-fixed gravitational reference system. However, when the magnetic field in Earth-fixed gravitational reference system changes over time, the magnetometer measurement is significantly altered to reflect the combination of true 3-D orientation of the device and latest total local magnetic field including not only static part such as local geomagnetic field but also the dynamic part (the net change). Such time-dependent changes may be due to any near field disturbance such as earphones, speakers, cell phones, vacuum cleaner, etc.
If presence of a near field disturbance is not known when the magnetometer is used for orientation estimate or compass, then the estimated orientation or the North direction is inaccurate. Therefore, in order to practically use magnetometer measurements for determining 3-D orientation and compass, the magnetic near field tracking and compensation is desirable. Moreover, the angular position obtained from a combination including a 3-D accelerometer and a 3-D rotational sensor is affected by the yaw angle drift and/or an unknown offset because there is no direct observation of the absolute yaw angle of the device's body reference system with respect to the Earth-fixed gravitational reference system. The magnetic field value which is compensated for near fields corrects this deficiency, curing the yaw angle drift problem.
The calibrated magnetometer (including soft-iron and hard-iron effect calibration) measures:
D
B
n+1=(DB0+DBNF)n+1 Equation 1
where DB0=EDR×EH0 Equation 2
and DBNF=EDR×EHNF Equation 3
The method dynamically tracks EHNF and uses it to estimate the DBNF, then compensates it from DBn to obtain D{circumflex over (B)}0, the estimated D{circumflex over (B)}0 is ready to be used for 3-D orientation measurement and compass. The methods may include the following steps.
Step 1: two 3×1 vectors are used to store the estimate of EĤNFn and the latest estimate of steady E{tilde over (H)}NF
Step 2: Construct a constant 3×1 vector in the Earth-fixed gravitational reference system
E
A=[0 0|EH0|]T Equation 4
Step 3: Construct a vector of observations in Earth-fixed gravitational reference system
EV=[EH0 EA] Equation 5
The following steps are executed for each time step.
Step 4: Compute a representation of EA in the device's body reference system using the angular position
D
A
n+1=ED{circumflex over (R)}n+1×EA Equation 6
By constructing EA in the manner indicated in Equation 4, the DAn+1 is not affected by the yaw angle error in ED{circumflex over (R)}n+1. The value of z axis of EA can be set to be any function of |EH0| to represent a relative weight of vector EA with respect toEH0.
Step 5: Compute the angle ∠DBn+1DAn+1 between DBn+1 and DAn+1
Step 6: Predict the total magnetic field (including the near fields) in Earth-fixed gravitational reference system:
E
Ĥ
tot
=(ED{circumflex over (R)}n+1)T×DBn+1 Equation 7
Step 7: Compute the difference between the current total field estimate EĤtot and the best estimate of the total field from the previous time step
r
n+1=EĤtot
Step 8: Update the current near field estimate using, for example, a single exponential smooth filter.
E
Ĥ
NF
=EĤNF
Step 9: Compute the total magnitude of EĤNF
ΔLn+1=∥EĤNF
Step 10: Compute the angle ∠(EĤNF
Step 11: Compute the angle difference between ∠(EĤNF
Δβn+1=|∠(EĤNF
Step 12: Evaluate if the magnetic near field is steady using, for example, the following exemplary embodiment.
where a variable of sampleCount_ is used to record how long the magnetic near field does not vary. Exemplarily, k1 may be set to be 3, and k2 may be set to be 4. σ is given by
σ=√{square root over (σx2+σy2+σz2)} Equation 12
where σx, σy, and σz are the standard deviations of sample noise of a tri-axis magnetometer along the x-axis, y-axis, and z-axis, respectively. Those values can be predetermined from magnetometer manufacturer's specification sheet or static measurements in a controlled environment of constant magnetic field (e.g. manufacture calibration setup).
Step 13: Update E{tilde over (H)}NF
Step 14: Evaluate if a current sample is consistent with the latest estimated steady magnetic field by, for example, by performing the following sub-steps.
Sub-step 14.1: Compute angle difference between ∠(E{tilde over (H)}NF
Δ{tilde over (β)}n+1=|∠(EĤNF
Sub-step 14.2: Compute the total magnitude of EĤNF
Δ{circumflex over (L)}n+1=∥E{tilde over (H)}NF
Sub-step 14.3: Compare the differences computed at 14.1 and 14.2 with pre-defined thresholds using, for example, the following code
where k1 and k2 can be set to be reasonably large to allow more samples to be included. Note that one option for the “else” step in Code 3 is to update the current model so that it better reflects the current magnetic field.
Step 15: If the result of step 14 is that current sample is consistent with the latest estimated steady magnetic field, then perform the following sub-steps.
Sub-step 15.1: Construct the vector observations in the Earth-fixed gravitational reference system using E{tilde over (H)}NF
E
{tilde over (V)}
n+1=[E{tilde over (H)}NF
Sub-step 15.2: Construct the vector observations in device's body reference system
E
V
n+1=[DBn+1 DAn+1] Equation 16
Sub-step 15.3 Form the 3×3 matrix with the vector observations in both the device's body reference system and the Earth-fixed gravitational reference system:
G=
D
V
n+1×(E{tilde over (V)}n+1)T Equation 17
Sub-step 15.4: Solve the corrected ED{tilde over (R)}n. This sub-step may be implemented using various different algorithms. An exemplary embodiment using a singular value decomposition (SVD) method is described below.
(1) Decompose G using SVD
[u s v]=SVD(G) Equation 18
(2) Compute the sign and construct w
(3) Compute EE{tilde over (R)}n
E
E
{tilde over (R)}
n
=u×w×v
T Equation 20
Step 16: Compute D{circumflex over (B)}0 in which the magnetic near field is compensated
D
{circumflex over (B)}
0=ED{tilde over (R)}n×EH0 Equation 21
Step 17: Estimate the error associated with a yaw angle determination using D{circumflex over (B)}0
Parameters k1 and k2 may be set to be dynamic functions of the accuracy of magnetometer's calibration.
A flow diagram of a method 400 for tracking dynamic near fields and correcting a magnetic field measured together with an angular position in a body reference system having an unknown yaw offset relative to a gravitational reference system according to another embodiment is illustrated in
Further the method 400 includes estimating current near fields to be a sum of the previous near fields and a portion of the calculated field difference, at S420. Then, the method 400 includes computing a magnitude difference between magnitudes of a current estimated total magnetic field including the estimated current near-fields and the measured magnetic field, and an angular difference between (1) a first angle between the current estimated total magnetic field and a fixed vector in the gravitational reference system, and (2) a second angle between the measured magnetic field and the fixed vector expressed in the body reference system, at S430.
The method 400 also includes comparing the magnitude difference and the angle difference with noise to determine whether the current measured magnetic field is consistent with the previously tracked magnetic near fields, at S440.
If the comparing determines that the current measured magnetic field is consistent with the previously tracked magnetic near fields (i.e., “YES” branch), the step S450 updating the angular position using the current estimated total magnetic field, and correcting the measured magnetic field for the current near field effects using the updated angular position.
The disclosed exemplary embodiments provide methods that may be part of a toolkit useable when a magnetometer is used in combination with other sensors to determine orientation of a device, and systems capable to use the toolkit. The methods may be embodied in a computer program product. It should be understood that this description is not intended to limit the invention. On the contrary, the exemplary embodiments are intended to cover alternatives, modifications and equivalents, which are included in the spirit and scope of the invention as defined by the appended claims. Further, in the detailed description of the exemplary embodiments, numerous specific details are set forth in order to provide a comprehensive understanding of the claimed invention. However, one skilled in the art would understand that various embodiments may be practiced without such specific details.
Exemplary embodiments may take the form of an entirely hardware embodiment or an embodiment combining hardware and software aspects. Further, the exemplary embodiments may take the form of a computer program product stored on a computer-readable storage medium having computer-readable instructions embodied in the medium. Any suitable computer readable medium may be utilized including hard disks, CD-ROMs, digital versatile disc (DVD), optical storage devices, or magnetic storage devices such a floppy disk or magnetic tape. Other non-limiting examples of computer readable media include flash-type memories or other known memories.
Although the features and elements of the present exemplary embodiments are described in the embodiments in particular combinations, each feature or element can be used alone without the other features and elements of the embodiments or in various combinations with or without other features and elements disclosed herein. The methods or flow charts provided in the present application may be implemented in a computer program, software, or firmware tangibly embodied in a computer-readable storage medium for execution by a specifically programmed computer or processor.
This application is related to, and claims priority from U.S. Provisional Patent Application Ser. No. 61/414,582, entitled “Dynamic Magnetic Near Field Tracking and Compensation”, filed on Nov. 17, 2010, the disclosures of which are incorporated here by reference.
Filing Document | Filing Date | Country | Kind | 371c Date |
---|---|---|---|---|
PCT/US11/61171 | 11/17/2011 | WO | 00 | 5/14/2013 |
Number | Date | Country | |
---|---|---|---|
61414582 | Nov 2010 | US |