# Patent

## Title
Angular velocity estimation using a magnetometer and accelerometer

## Abstract

A system and method for estimating angular velocity are provided. The system and method use an accelerometer and magnetometer to estimate an angular velocity in place of a gyroscope in 9-axis sensor fusion to estimate angular orientation. The final angular velocity estimate is constructed from two partially independent angular velocity estimates, one using only magnetometer measurements and the other using only accelerometer measurements. The unobservable portion of each partial angular velocity estimate is provided by a projection from a third complete estimate that uses both accelerometer and magnetometer data.

## Background
BACKGROUND
1. Field
The embodiments are directed generally to angular velocity estimation, and specifically, to angular velocity estimation using a magnetometer and accelerometer.
2. Background Art
A MEMS rate gyroscope is a device that measures angular velocity. A MEMS gyroscope may be a stand-alone device or included in another electronic device as part of integrated circuit included in a micro-chip. Angular velocity can be integrated over time to compute changes in angular orientation. When combined with other sensors that provide an absolute reference for pitch and roll (accelerometer) or yaw (magnetometer), an estimate of absolute angular orientation can be produced.
Gyroscopes are included in electronic devices to quickly and accurately detect changes in electronic device orientation and to enable high quality estimates of angular orientation as it changes Applications can be developed that take advantage of these capabilities to provide useful functionality to end users. An example of an application that relies on angular rotation includes the positioning of a mobile device in a horizontal or vertical orientation to facilitate text entry and web application formatting and display. Another example includes applications, such as video games, that receive as an input the angular velocity and integrated angular orientation of the electronic device as it is manipulated by a user. Another example is an application that uses angular orientation estimates as inputs to a pointing device such as a motion based mouse.
Gyroscopes, however, are power intensive circuits when run on electronic devices. The intensive power consumption is an energy drain on the electronic device, particularly when the electronic device is powered using a re-chargeable power source, such as a battery. Additionally, a power intensive circuit increases an overall cost for using the electronic device.
Therefore, what is needed are systems and methods that generate an estimated angular velocity using efficient integrated circuits and software that have a low code and data footprint and a low CPU load.
BRIEF SUMMARY
In an embodiment, a system and method for estimating angular velocity are provided. The system and method use an accelerometer and magnetometer to estimate an angular velocity in place of a gyroscope in 9-axis sensor fusion to estimate angular orientation. The final angular velocity estimate is constructed from two partially independent angular velocity estimates, one using only magnetometer measurements and the other using only accelerometer measurements. The unobservable portion of each partial angular velocity estimate is provided by a projection from a third complete estimate that uses both accelerometer and magnetometer data.
In an embodiment, adaptive filter lengths are used to trade off measurement noise against system responsiveness. Filter lengths are set as a function of fast estimates of angular velocity and angular acceleration.
In another embodiment, a system and method for determining a presence of a time-varying magnetic anomaly are provided.
In another embodiment, a system and method for using an angular velocity estimate to estimate a bias offset and sensitivity of the physical rate gyroscope are provided.
Further features and advantages of the embodiments, as well as the structure and operation of various embodiments, are described in detail below with reference to the accompanying drawings. It is noted that the embodiment is not limited to the specific embodiments described herein. Such embodiments are presented herein for illustrative purposes only. Additional embodiments will be apparent to persons skilled in the relevant art(s) based on the teachings contained herein.



BRIEF DESCRIPTION OF THE DRAWINGS/FIGURES
The accompanying drawings, which are incorporated herein and form part of the specification, illustrate the embodiments and, together with the description, further serve to explain the principles of the embodiments and to enable a person skilled in the relevant art(s) to make and use the embodiments.
FIG. 1 is a block diagram of an electronic device, according to an embodiment.
FIG. 2 is a block diagram of an electronic device that includes filters for filtering data from an accelerometer and magnetometer, according to an embodiment.
FIG. 3 is a flowchart of a method for determining angular velocity, according to an embodiment.
FIG. 4 is a block diagram for calibrating a gyroscope using an estimated angular velocity, according to an embodiment.
FIGS. 5A-B are diagrams of source code that determine an angular velocity estimate, according to an embodiment.
FIG. 6 is a block diagram of an electronic device having a specialized processor, according to an embodiment.
FIG. 7 is a block diagram of a computing environment where the embodiments may be implemented.



The features and advantages of the embodiments will become more apparent from the detailed description set forth below when taken in conjunction with the drawings, in which like reference characters identify corresponding elements throughout. In the drawings, like reference numbers generally indicate identical, functionally similar, and/or structurally similar elements.
DETAILED DESCRIPTION OF THE EMBODIMENTS
The System
FIG. 1 is a block diagram 100 of an electronic device, according to an embodiment. FIG. 1 includes an electronic device 102. Electronic device 102 may be a programmable electronic machine that performs high-speed mathematical and logical operations, and that assembles, stores, correlates and otherwise processes data. Examples of electronic device 102 may include a personal computer, a mobile communication device (e.g. a smartphone, a tablet computing device, and a laptop notebook), a set-top box, a game-console, and an embedded system. The features and hardware generally included in electronic device 102 are described in detail in FIG. 6.
In an embodiment, electronic device 102 may execute or interpret multiple applications 104. Applications 104 may be installed or downloaded to electronic device 102, using a network or the external secondary storage units described in detail in FIG. 6. In another embodiment, applications 104 may be external to electronic device 102, and receive data from electronic device 102.
A network (not shown) may be any network or combination of networks that can carry data communication. Such a network may include, but is not limited to, a local area network, metropolitan area network, and/or wide area network, such as the Internet. A network can support protocols and technologies including, but not limited to, World Wide Web protocols and/or services. Applications 104 are executed on or receive data from electronic device 102.
In an embodiment, electronic device 102 includes an accelerometer 106 and magnetometer 108.
Accelerometer 106 measures the sum of the gravity vector and linear acceleration vector experienced by an object, such as electronic device 102. Magnetometer 108 measures strength and direction of magnetic fields. In an embodiment, magnetometer 108 may be used to measure the Earth's magnetic fields and detect different types of magnetic anomalies. Example magnetic anomalies may be magnetic fields generated by other magnetic objects that may be temporarily or permanently in a vicinity of electronic device 102.
In FIG. 1, accelerometer 106 and magnetometer 108 are shown within electronic device 102. The implementation, however, is not limited to this embodiment. Accelerometer 106 and magnetometer 108 may be outside of an electronic device and may be coupled to electronic device 102 using a wired or wireless network.
Conventionally, a gyroscope determines angular velocity in an electronic device. As described above, however, a gyroscope is a power intensive device that is a power drain on battery powered electronic devices.
Other conventional systems may use a magnetometer, such as the Aichi AMI307, along with an accelerometer, to estimate angular velocity. For example, there is a relationship between changes in the calibrated magnetic field observed by a conventional magnetometer and changes in an angle, such that an estimate of angular velocity can be constructed by examining the partial derivatives of the magnetic field with respect to angular rotation. This relationship, however, may hold true for only small angle changes between each magnetic sample. For example, for small angle changes, such as angle changes between 5 and 10 degrees which correspond to 500 to 1000 dpw with 100 Hz sample rate, the relationship between magnetic field values “x”, “y” and “z” and the corresponding angular changes may be approximated as linear.
One way to conventionally determine the estimated angular velocity is through a system of equations that define relationships between “x”, “y” and “z” field strength measurements and starting and ending angles of the rotation. The system of equations is constructed independently for rotation around each of the “x”, “y” and “z” axes. The system of equations is then solved to determine “x”, “y” and “z” axis rotation estimates for changes in “x”, “y” and “z” field strengths.
An example set of equations for a rotation of a vector measurement “x” about a “z” axis by an angle θz is below:

x1=x0 cos θz−y0 sin θz 

y1=y0 sin θz+y0 cos θz 

z1=z0 

An example set of equations for a rotation of a vector measurement “x” about a “z” axis by an angle θz is below:

x1=x0 cos θz−y0 sin θz 

y1=y0 sin θz+y0 cos θz 

z1=z0 

An example set of equations for a rotation of a vector measurement “x” about a “z” axis by an angle θy is below:

x1=x0 cos θy−z1 sin θy 

y1=y0 

z1=z0 cos θy−x1 sin θy 

In the examples above, the subscript 0 indicates an initial data value of a sample and the subscript 1 indicates a final data value of a sample, where the final data value of a sample is its value after a single rotation.
Assuming that for a small angle, cos x≈1−x2/2 and sin x≈x, the equations above may be reduced to:




  
    [
    
      
        
          
            Δ
            ⁢
            
              
            
            ⁢
            x
          
        
      
      
        
          
            Δ
            ⁢
            
              
            
            ⁢
            y
          
        
      
      
        
          
            Δ
            ⁢
            
              
            
            ⁢
            z
          
        
      
    
    ]
  
  ≈
  
    
      [
      
        
          
            0
          
          
            
              -
              
                z
                1
              
            
          
          
            
              -
              
                y
                1
              
            
          
        
        
          
            
              -
              
                z
                1
              
            
          
          
            0
          
          
            
              -
              
                x
                1
              
            
          
        
        
          
            
              -
              
                y
                1
              
            
          
          
            
              -
              
                x
                1
              
            
          
          
            0
          
        
      
      ]
    
    ⁡
    
      [
      
        
          
            
              θ
              x
            
          
        
        
          
            
              θ
              y
            
          
        
        
          
            
              θ
              z
            
          
        
      
      ]
    
  




The equation above connects observable angle changes with measured field strength changes. When solved for the rotation vector θ, as shown below, the equation transforms first order differences in the magnetic field to first order differences in angles, which are the estimates of an angular velocity.




  
    [
    
      
        
          
            θ
            x
          
        
      
      
        
          
            θ
            y
          
        
      
      
        
          
            θ
            z
          
        
      
    
    ]
  
  =
  
    
      
        1
        
          2
          ⁢
          xyz
        
      
      ⁡
      
        [
        
          
            
              
                -
                
                  x
                  2
                
              
            
            
              xy
            
            
              xz
            
          
          
            
              xy
            
            
              
                -
                
                  y
                  2
                
              
            
            
              yz
            
          
          
            
              xz
            
            
              yz
            
            
              
                -
                
                  z
                  2
                
              
            
          
        
        ]
      
    
    ⁡
    
      [
      
        
          
            
              Δ
              ⁢
              
                
              
              ⁢
              x
            
          
        
        
          
            
              Δ
              ⁢
              
                
              
              ⁢
              y
            
          
        
        
          
            
              Δ
              ⁢
              
                
              
              ⁢
              z
            
          
        
      
      ]
    
  




There are several limitations to this conventional approach. First, the equation above becomes indefinite when the position values for “x”, “y” and “z”, are equal to or close to zero. Second, the matrix inversion required to solve for the rotation vector θ, which is an estimate of the angular velocity, is a power intensive process.
To generate an angular velocity estimate without a gyroscope and the impediments described above, electronic device 102 includes an angular velocity processor 110. In an embodiment, angular velocity processor 110 may be included in a library stored in a memory of electronic device 102. The library may include code configured to execute or be interpreted by a processor. Angular velocity processor 110 determines an angular velocity estimate 122 from field measurements obtained using accelerometer 106 and magnetometer 108. In an embodiment, angular velocity processor 110 may obtain field measurements from accelerometer 106 and magnetometer 108 by way of sensor drivers included in the library or downloaded to electronic device 102.
Angular velocity processor 110 determines that when field measurements change, the axis of rotation of the field is normal to the vectors defined by the starting and ending field measurements. This relationship may be satisfied by a cross product of a single sample of rotation where the single sample of rotation=X0×X1, such that X0 is the rotation vector in the beginning of rotation and X1 is the rotation vector in the end of the rotation. When X0 and X1 are normalized to unit length, the rotation between samples is small and the magnitude of the cross product is approximately the angular rotation between measurements. In an embodiment, the measurements are measured in radians. In a further embodiment, when the small angle approximation breaks down, the rotation becomes increasingly underestimated. In an embodiment, the error may be small for a less than a 10 degree rotation between samples.
To determine angular velocity estimate 122, angular velocity processor 110 includes a magnetometer angular velocity calculator 112, an accelerometer angular velocity calculator 114 and an angular velocity calculator 116. Each of magnetometer angular velocity calculator 114, accelerometer angular velocity calculator 112 and angular velocity calculator 116 determines a respective component of angular velocity estimate 122.
Magnetometer angular velocity calculator 114 determines the partial angular velocity estimate from the field measurements provided by magnetometer 108. The partial angular velocity estimate from magnetometer field measurements may be defined as a cross product ωm_partial=−×, where  and  are beginning and ending magnetic field measurements normalized to a unit length. Additionally, magnetometer angular velocity calculator 114 defines the axis of rotation as:




  
    ω
    m_partial
  
  
    norm
    ⁢
    
      
    
    ⁢
    2
    ⁢
    
      (
      
        ω
        m_partial
      
      )
    
  




and the angle of rotation between samples as:

norm2(ωm_partial)

Accelerometer angular velocity calculator 112 determines the partial angular velocity estimate from the measurements provided by accelerometer 106. The partial angular velocity estimate from the accelerometer field measurements is defined as a cross product ωa_partial=−×, where  and  are beginning and ending accelerometer measurements normalized to a unit length. Additionally, accelerometer angular velocity calculator 112 defines the axis of rotation as:




  
    ω
    a_partial
  
  
    norm
    ⁢
    
      
    
    ⁢
    2
    ⁢
    
      (
      
        ω
        a_partial
      
      )
    
  




and the angle of rotation between samples as:

norm2(ωa_partial)

In an embodiment, the ωm_partial and ωa_partial angular velocity estimates are incomplete because they do not contain information regarding angular velocity around their respective characteristic axes. For instance, for ωa_partial the axis that cannot be observed is the axis in the direction of gravity, and for ωm_partial the axis that cannot be observed is the axis along the direction of the magnetic field. To compensate for the lack of information for the angular velocity provided by ωm_partial and ωa_partial, angular velocity calculator 116 determines the missing components of the angular velocity provided by ωm_partial and ωa_partial along the missing axes. To determine the estimate of the angular velocity along the missing axes, angular velocity calculator 116 derives an estimate of the missing angular velocity component by using the accelerometer and magnetometer measurements and projects the derived estimate of the missing angular velocity components onto the axes that are not observable from the accelerometer or magnetometer field measurements.
In an embodiment, let H be a magnetic field vector in world coordinates, and A be a gravity vector in world coordinates. A person skilled in the art will appreciate that world coordinates define objects by an orientation matrix, which includes in its three columns the Cartesian coordinates of three points. These points may then be used to define the orientation of axes, and are tips of the unit vectors aligned with those axes.
In an embodiment, vector A may be the same as a calibrated accelerometer measurement in the absence of linear acceleration. In an embodiment, let the x-axis be the magnetic North in world coordinates, such that the H vector in world coordinates has “x” and “z” components when the device is facing north. In an embodiment, the H vector may be defined such that it only has “x” and “z” components. When the H vector is defined as described above, ŷ=H×A, normalized to unit length, and {circumflex over (x)}=ŷ×{circumflex over (z)}, which is (H×A)×A, normalized to unit length.
Next, let the orientation at time 0 be a rotation matrix R0, and the orientation at time 1 be a rotation matrix R1, where R1=R_delta*R0 and where R_delta is the rotation matrix that transforms R0 into R1.
Solving for R_delta, R_delta=inv(R0)*R1.
Since R0 is a rotation matrix, the inverse is the same as the transpose, which yields R_delta=R0T R1. The R_delta matrix allows for the individual rotations to be read from the R_delta matrix by converting the individual rotations from the rotation matrix to an axis/angle format. Therefore, ωm_afull may be determined as a single sample rotation by R_delta that uses both accelerometer and magnetometer field measurements.
In an embodiment, angular velocity estimator 120 receives ωm_partial, generated by magnetometer angular velocity calculator 114, ωa_partial, generated by accelerometer angular velocity calculator 112, and ωm_afull, generated by angular velocity calculator 116. Angular velocity estimator 120 then generates angular velocity estimate 122 by combining ωm_partial, ωa_partial, and ωm_afull, as described below.
In an embodiment, angular velocity estimator 120 compensates ωm_partial and ωa_partial for an unobservable component described above. For example, angular velocity estimator 120 combines ωm_partial with a projection of ωm_afull to the magnetic field axis to fill the unobservable component associated with the magnetometer measurements. For instance, let ωm_fill be the projection of ωm_afull to fill in the unobservable component associated with magnetometer measurements as it is projected onto the magnetic field axis. The scale factor of the projection is the dot product of ωm_afull·H. The direction of the magnetic field estimate in the world coordinates is H normalized to unit length. Therefore, in an embodiment:




  
    ω
    m_fill
  
  =
  
    
      
        (
        
          
            ω
            
              m_a
              full
            
          
          ⁢
          H
        
        )
      
      
        norm
        ⁢
        
          
        
        ⁢
        2
        ⁢
        
          (
          H
          )
        
      
    
    ⁢
    
      H
      ^
    
  




Once angular velocity estimator 120 determines ωm_fill, angular velocity estimator 120 determines the magnetometer component ωm_full for angular velocity estimate 122, as shown below:

ωm_full=ωm_partial+ωm_fill 

In another example, angular velocity estimator 120 also combines ωa_partial with a projection of ωm_afull along the gravitation field axis to fill the unobservable component associated with the accelerometer measurements. For example, angular velocity estimator 120 combines ωa_partial with a projection of ωm_afull to fill in the unobservable component associated with accelerometer measurements. For instance, let ωa_fill be the projection of ωm_afull that fills the unobservable component associated with accelerometer measurements as it is projected onto the gravitational field axis. The scale factor of the projection is the dot product of ωa_full·A. The direction of the gravitational field estimate in the world coordinates is A normalized to unit length. Therefore, in an embodiment:




  
    ω
    a_fill
  
  =
  
    
      
        (
        
          
            ω
            
              m_a
              full
            
          
          ⁢
          A
        
        )
      
      
        norm
        ⁢
        
          
        
        ⁢
        2
        ⁢
        
          (
          A
          )
        
      
    
    ⁢
    
      A
      ^
    
  




When angular velocity estimator 120 determines ωa_fill, angular velocity estimator 120 determines the accelerometer component ωa_full for the angular velocity estimate 122, as shown below:

ωa_full=ωa_partial+ωa_fill 

In an embodiment, once angular velocity estimator 120 determines ωm_full and ωa_full angular velocity estimator 120 then determines angular velocity estimate 122, defined as ωest, using ωm_full and ωa_full. In a further embodiment, angular velocity estimator 120 may also use weights to determine angular velocity estimate 122. Angular velocity estimator 120 uses weights when it determines whether to rely more on (ωm_fill or ωa_fill while determining angular velocity estimate 122. For example, in one instance the noise associated with magnetometer field measurements may be lower than the noise associated with accelerometer field measurements. In this instance, angular velocity estimator 120 relies more on ωm_full than ωa_full to determine angular velocity estimate 122. On the other hand, when noise associated with magnetometer field measurements may be higher than the noise associated with accelerometer field measurements, angular velocity estimator 120 relies more on ωa_full than ωm_full to determine angular velocity estimate 122. Thus, angular velocity estimate 122 becomes a weighted sum of ωm_full and ωa_full, where a weight determines the trustworthiness of A and H estimates for the current measurement sample. Below is an example of a weighted angular velocity estimate 122, ωest:

ωest==(khωm_full+kAωa_full)/(kA+kH)

where kh and kA are weight factors as applied to full and ωa_full respectively.

In an embodiment, kh and kA may be based on the sensor noise and anomaly detection associated with magnetometer 108 and accelerometer 106, respectively.
In an embodiment, kh may be determined as follows:

kh=σmf*qmet 

where σmf is a magnetic contribution to fusion angle noise and qmet is a current estimate of calibration quality associated with magnetometer 108. In an embodiment, qmet>=30.

In an embodiment, kA may be determined as follows:

kA=σaf*Amet 

where σaf is an acceleration contribution to fusion angle noise and Amet=1/(abs(1−mag(A)), where A components are measured in g(s). In an embodiment, Amet<30.

In an embodiment, σaf and σmf may be estimated by taking a noise standard deviation of accelerometer 106 and magnetometer 108, respectively, and dividing the noise standard deviation by a nominal field strength associated with accelerometer 106 and magnetometer 108. In an embodiment, the nominal field strength may be 1 g for accelerometer 106 (for gravity on Earth) and 400 milli-gauss for magnetometer 108 (which is an estimate of the Earth's magnetic field.) When noise components σa and σm are normal to the reference gravity and magnetic fields, converting from a sensor sample noise to angle noise for noise normal to the reference gravity and magnetic fields, yields angles of arctan (sample noise/nominal field strength). This conversion approaches a sample noise/nominal field strength value for small angles.
For example, a sensor noise value for the Kionix KXCJ9 accelerometer manufactured by Kionix, Inc., of Ithaca, N.Y., is 150 micro-g/sqrt(Hz). At 100 Hz, the sensor noise is therefore 1.5 milli-g. Applying these values to the equation above, the σa=1.5/1000=0.0015 radians or approximately 0.086 degrees.
In another example, the noise value for a magnetometer, such as the Aichi AMI 307, manufactured by Aichi Micro Intelligent Corporation of Japan, is 0.6 milli-gauss. Applying this value to the equation above, σmf=0.6/400=0.0015 radians or approximately 0.086 degrees.
In an embodiment, other estimates for determining σmf of magnetometer 108 may be used. For instance, instead of using a noise value for a magnetometer 108 (such as 400 milli-gauss), an estimate of the magnetic field strength may be used, when the estimate is of high confidence. For instance, the estimate may be of high confidence when a calibration of magnetometer 108 is highly converged. In an embodiment, using an estimate of the magnetic field strength that is of high confidence may result in using different and potentially more precise weight factors for different locations on Earth.
Using Filtered Input Data to Construct Angular Velocity Estates
In a further embodiment, angular velocity processor 110 may receive filtered H and A values from magnetometer 108 and accelerometer 106. FIG. 2 is a block diagram 200 that shows filters for filtering accelerometer and magnetometer data. Block diagram 200 includes filter 202 and filter 204. Filter 202 filters H values from magnetometer 108 and filter 204 filters A values from accelerometer 106. Once filters 202 and 204 filter H and A values, filters 202 and 204 pass the filtered H and A to magnetometer angular velocity calculator 114, accelerometer angular velocity calculator 112 and angular velocity calculator 116.
In an embodiment, a filter (such as filter 202 or 204) may be a data window that includes data samples collected during a particular time interval. The data samples in the filter may be combined, averaged, etc., to provide one data point to angular velocity processor 110.
In an embodiment, the length of the filter may be varied as a function on the estimated angular velocity. For instance, since the noise is more visible on top of slower movements, a longer filter may be used for slower estimated angular velocity calculations, than for the filter used for quicker estimated angular velocity calculations. In a further embodiment, the length of filters 202 and 204 may be dynamically changed as angular velocity estimate 122 changes.
In another embodiment, the length of the filter may be varied as a function on the estimated angular acceleration. For instance, a longer filter may be used for low angular acceleration and a shorter filter may be used for higher angular acceleration. As with angular velocity, the length of filters 202 and 204 may be dynamically changed as angular acceleration changes.
In an embodiment, angular velocity processor 110 performs some or all calculations using fixed point arithmetic. A person skilled in the art will appreciate that in fixed point arithmetic, a fixed point number representation is a real data type for a number that has a fixed number of digits before and after a radix point.
In electronic devices 102 that may consume power, or that may not be as sensitive to precise angular velocity estimates, angular velocity estimate 122 may be generated using a ωm_afull measurement, where accelerometer 106 and magnetometer 108 field measurements were filtered using filters 202 and 204.
Method for Determining an Angular Velocity Estimate
FIG. 3 is a flowchart of a method 300 for determining an angular velocity estimate, according to an embodiment.
At step 302, an angular velocity component associated with a magnetometer is determined. For example, magnetometer angular velocity calculator 114 determines ωm_partial as a cross product between initial and final field measurements  and  that are normalized to unit length, where  and  are field measurements provided by magnetometer 108. In an embodiment, initial and final field measurements  and  may be filtered field measurements.
At step 304, an angular velocity component associated with the accelerometer is determined. For example, accelerometer angular velocity calculator 112 determines ωa_partial as a cross product between  and , where  and  are the beginning and ending measurements provided by accelerometer 106 that are normalized to unit length. In an embodiment, initial and final field measurements  and  may be filtered field measurements.
At step 306, unobservable components from the angular velocities components associated with an accelerometer and magnetometer are determined. For example, angular velocity calculator 116 determines ωm_afull using accelerometer 106 and magnetometer 108 measurements. Using ωm_afull, angular velocity estimator 120 determines ωm_fill which is a projection of ωm_afull to fill the unobservable component associated with magnetometer field measurements projected onto the magnetic field axis. In an embodiment,




  
    ω
    m_fill
  
  =
  
    
      
        (
        
          
            ω
            
              m_a
              full
            
          
          ⁢
          H
        
        )
      
      
        norm
        ⁢
        
          
        
        ⁢
        2
        ⁢
        
          (
          H
          )
        
      
    
    ⁢
    
      
        H
        ^
      
      .
    
  




Additionally, using ωm_afull, angular velocity estimator 120 determines ωa_fill, which is a projection of





  ω
  
    m
    
      a
      full
    
  




to fill the unobservable component associated with accelerometer measurements projected onto the gravitational field axis. In an embodiment, ωa_fill=





  
    
      (
      
        
          ω
          
            
              m
              a
            
            full
          
        
        ⁢
        A
      
      )
    
    
      norm
      ⁢
      
        
      
      ⁢
      2
      ⁢
      
        (
        A
        )
      
    
  
  ⁢
  
    
      A
      ^
    
    .
  




At step 308, angular velocities components associated with an accelerometer and magnetometer are compensated with the respective unobservable components. For example, angular velocity estimator 120 determines the magnetometer estimate for the angular velocity as ωm_full=ωm_partial+ωm_fill. Additionally, angular velocity estimator 120 determines the accelerometer estimate for the angular velocity ωa_full as ωa_partial+ωa_fill.
At step 310, angular velocity estimate is determined. For example, angular velocity estimate 122, ωest, may be the weighted sum of ωm_full and ωa_full. In another embodiment, weights may be based on the trustworthiness of A and H estimates for the current measurement sample.
Calibration of a Physical Gyroscope During Fast Motion
In an embodiment, angular velocity estimate 122 may be used to calibrate a physical rate gyroscope. The physical rate gyroscope may be calibrated in terms of the bias offset and sensitivity. A bias offset may be an offset by which a physical rate gyroscope outputs an angular velocity when the physical rate gyroscope does not experience rotation. Sensitivity may be a measure of how sensitive the gyroscope is to detecting changes in the angular velocity.
FIG. 4 is a block diagram 400 of a gyroscope calibration module for calibrating a gyroscope using an estimated angular velocity, according to an embodiment.
In one embodiment, gyroscope calibration module 402 determines sensitivity 406 of a calibrated gyroscope. To determine sensitivity 406 of the calibrated gyroscope, gyroscope calibration module 402 receives angular velocity estimate 122 and an angular velocity 404, where angular velocity 404 is generated by the gyroscope. Gyroscope calibration module 402 determines sensitivity 406 by dividing angular velocity estimate 122 by angular velocity 404.
In another embodiment, gyroscope calibration module 402 determines sensitivity 406 and bias offset 408 of a non-calibrated gyroscope. In an embodiment, when a physical rate gyroscope is not calibrated, then the angular velocity indicated by the physical rate gyroscope is related to the angular velocity estimate 122 by an affine function estimate ωma=S(ωphysical−biasphysical), where S is defined as sensitivity 406 of the gyroscope, biasphysical is the bias offset 408, ωma is angular velocity estimate 122 and ωphysical is the angular velocity 404. In an embodiment, S and biasphysical may be derived by performing a linear regression on the data points generated by angular velocity estimate 122 and angular velocity 404 generated by the physical rate gyroscope.
Detecting Magnetic Anomaly
In an embodiment, in the absence of linear acceleration or a magnetic anomaly, ωm_partial, ωa_partial, and ωm_afull may show agreement with each other. However, ωm_partial and ωa_partial may diverge in the presence of either linear acceleration or a magnetic anomaly.
In an embodiment, linear acceleration may be detected using a simple metric. For example, when the magnitude of the acceleration vector measured by the accelerometer varies from the long term average (which on Earth is typically 9.81 m/s^2 due to gravity alone), linear acceleration contributes to the acceleration vector. The bigger the difference in the magnitude, the more linear acceleration is present. However, when ωm_partial and ωa_partial diverge in the absence of linear acceleration, the divergence may be attributed to a magnetic anomaly. The anomaly detection may cause angular velocity estimator 120 to rely less weight kh during angular velocity estimate 122 calculations.
Source Code for Determining Angular Velocity Estimate
FIGS. 5A-B are diagrams 500A-B of source code that determines an angular velocity estimate according to the embodiments described above. In FIG. 5, “w_from_accel_mag” function receives H and A field measurements from magnetometer 108 and accelerometer 106 and determines angular velocity estimate 122.
Determining Angular Velocity Estimate Using Integrated Circuit
FIG. 6 is a block diagram 600 of an electronic device having a specialized processor, where the embodiment may be implemented.
In an embodiment, electronic device 102 may include a HUB processor 602. HUB processor 602 is a micro-processor within electronic device 102 that includes specialized integrated circuits and an internal memory 604. Internal memory 604 may be one of the memories described in detail in FIG. 7. Integrated circuits in HUB processor 602 execute instructions stored in internal memory 604. For instance, internal memory 604 may store instructions for generating angular velocity estimate 122, as described above.
In an embodiment, HUB processor 602 includes an angular velocity estimator integrated circuit 606. Angular velocity estimator integrated circuit 606 is a hardware circuit that performs the functionality of angular velocity processor 110 in hardware.
For instance, accelerometer 106 and magnetometer 108 may be connected to HUB processor 602. Accelerometer 106 and magnetometer 108 transmit field measurements to HUB processor 602. In an embodiment, before the field measurements generated by accelerometer 106 and magnetometer 108 are received by HUB processor, the field measurements pass through an analog to digital converter (not shown). The analog to digital converter transforms the field measurements from analog to digital form.
HUB processor 602 forwards the field measurements to angular velocity estimator integrated circuit 606. Because angular velocity estimator integrated circuit 606 is a circuit specialized for generating angular velocity estimate 122, the power and processing time for generating angular velocity estimate 122 using angular velocity estimator integrated circuit 606 may be superior to that of another processor executing on electronic device 102.
When angular velocity estimator integrated circuit 606 receives the field measurements, angular velocity estimator integrated circuit 606 retrieves instructions for processing the field measurements from internal memory 604 and processes the field measurements according to the retrieved instructions. Upon completion, angular velocity estimator integrated circuit 606 generates angular velocity estimate 122 that is then passed to applications 104 on electronic device 102.
In an embodiment, other processors, such as processors described in detail in FIG. 7, below, are also included in electronic device 102.
Computing Environment
Various embodiments can be implemented, for example, using one or more well-known computer systems, such as computer system 700 shown in FIG. 7. Computer system 700 can be any well-known computer capable of performing the functions described herein, such as computers available from International Business Machines, Apple, Sun, HP, Dell, Sony, Toshiba, to name a few examples.
Computer system 700 includes one or more processors (also called central processing units, or CPUs), such as a processor 704. Processor 704 is connected to a communication infrastructure or bus 706. In an embodiment, processor 704 may be a graphics processing unit (GPU). A GPU is a processor that is a specialized electronic circuit designed to rapidly process mathematically intensive applications on electronic devices. The GPU has a highly parallel structure that is efficient for parallel processing of large blocks of data, such as mathematically intensive data common to computer graphics applications, images and videos.
Computer system 700 also includes user input/output device(s) 703, such as monitors, keyboards, pointing devices, etc., which communicate with communication infrastructure 706 through user input/output interface(s) 702.
Computer system 700 also includes a main or primary memory 708, such as random access memory (RAM). Main memory 708 may include one or more levels of cache. Main memory 708 has stored therein control logic (i.e., computer software) and/or data.
Computer system 700 may also include one or more secondary storage devices or secondary memory 710. Secondary memory 710 may include, for example, a hard disk drive 712 and/or a removable storage device or drive 714. Removable storage drive 714 may be a floppy disk drive, a magnetic tape drive, a compact disk drive, an optical storage device, tape backup device, and/or any other storage device/drive.
Removable storage drive 714 may interact with a removable storage unit 718. Removable storage unit 718 includes a computer usable or readable storage device having stored thereon computer software (control logic) and/or data. Removable storage unit 718 may be a floppy disk, magnetic tape, compact disk, DVD, optical storage disk, or any other computer data storage device. Removable storage drive 714 reads from and/or writes to removable storage unit 718 in a well-known manner.
According to an exemplary embodiment, secondary memory 710 may include other means, instrumentalities or approaches for allowing computer programs and/or other instructions and/or data to be accessed by computer system 700. Such means, instrumentalities or approaches may include, for example, a removable storage unit 722 and an interface 720. Examples of the removable storage unit 722 and the interface 720 may include a program cartridge and cartridge interface (such as that found in video game devices), a removable memory chip (such as an EPROM or PROM) and associated socket, a memory stick and USB port, a memory card and associated memory card slot, and/or any other removable storage unit and associated interface.
Computer system 700 may further include a communication or network interface 724. Communication interface 724 enables computer system 700 to communicate and interact with any combination of remote devices, remote networks, remote entities, etc. (individually and collectively referenced by reference number 728). For example, communication interface 724 may allow computer system 700 to communicate with remote devices 728 over communications path 726, which may be wired and/or wireless, and which may include any combination of LANs, WANs, the Internet, etc. Control logic and/or data may be transmitted to and from computer system 700 via communication path 726.
In an embodiment, a tangible apparatus or article of manufacture comprising a tangible computer useable or readable medium having control logic (software) stored thereon is also referred to herein as a computer program product or program storage device. This includes, but is not limited to, computer system 700, main memory 708, secondary memory 710, and removable storage units 718 and 722, as well as tangible articles of manufacture embodying any combination of the foregoing. Such control logic, when executed by one or more data processing devices (such as computer system 700), causes such data processing devices to operate as described herein.
Based on the teachings contained in this disclosure, it will be apparent to persons skilled in the relevant art(s) how to make and use the embodiments using data processing devices, computer systems and/or computer architectures other than that shown in FIG. 7. In particular, embodiments may operate with software, hardware, and/or operating system implementations other than those described herein.
CONCLUSION
It is to be appreciated that the Detailed Description section, and not the Summary and Abstract sections (if any), is intended to be used to interpret the claims. The Summary and Abstract sections of any) may set forth one or more but not all exemplary embodiments as contemplated by the inventor(s), and thus, are not intended to limit the embodiments or the appended claims in any way.
While the embodiments have been described herein with reference to exemplary embodiments for exemplary fields and applications, it should be understood that the subject matter is not limited thereto. Other embodiments and modifications thereto are possible, and are within the scope and spirit of the subject matter. For example, and without limiting the generality of this paragraph, embodiments are not limited to the software, hardware, firmware, and/or entities illustrated in the figures and/or described herein. Further, embodiments (whether or not explicitly described herein) have significant utility to fields and applications beyond the examples described herein.
Embodiments have been described herein with the aid of functional building blocks illustrating the implementation of specified functions and relationships thereof. The boundaries of these functional building blocks have been arbitrarily defined herein for the convenience of the description. Alternate boundaries can be defined as long as the specified functions and relationships (or equivalents thereof) are appropriately performed. Also, alternative embodiments may perform functional blocks, steps, operations, methods, etc. using orderings different than those described herein.
References herein to “one embodiment,” “an embodiment,” “an example embodiment,” or similar phrases, indicate that the embodiment described may include a particular feature, structure, or characteristic, but every embodiment may not necessarily include the particular feature, structure, or characteristic. Moreover, such phrases are not necessarily referring to the same embodiment. Further, when a particular feature, structure, or characteristic is described in connection with an embodiment, it would be within the knowledge of persons skilled in the relevant art(s) to incorporate such feature, structure, or characteristic into other embodiments whether or not explicitly mentioned or described herein.
The breadth and scope of the embodiments should not be limited by any of the above-described exemplary embodiments, but should be defined only in accordance with the following claims and their equivalents.

## Claims

1. A system comprising:
an angular velocity processor stored in memory, executing on a processor, and causing the processor to:
determine a first angular velocity component from magnetic vector measurements provided by a magnetometer;
determine a second angular velocity component from gravitational vector measurements provided by an accelerometer;
determine a third angular velocity component from a combination of measurements provided by the magnetometer and accelerometer, wherein the third angular velocity component compensates for measurements that are not observed by the magnetic vector and the gravitational vector and are not included in determining the first angular velocity component and determining the second angular velocity component;
compensate the first angular velocity component with a projection of the third angular velocity component around a first unobservable axis;
compensate the second angular velocity component with a projection of the third angular velocity component around a second unobservable axis; and
generate an angular velocity estimate using the compensated first angular velocity component and the compensated second angular velocity component and without using a measurement from a gyroscope.

2. The system of claim 1, wherein the angular velocity processor further causes the processor to generate the angular velocity estimate using a first weight associated with the first angular velocity component and a second weight associated with the second angular velocity component.

3. The system of claim 2, wherein the first and second weights are based on a fusion angle noise.

4. The system of claim 2, wherein the first weight is based on calibration associated with the magnetometer.

5. The system of claim 1, wherein the angular velocity processor further causes the processor to determine the first angular velocity component using a cross product between the magnetic vector measurements of the magnetometer, wherein the magnetic vector measurements are normalized to unit length.

6. The system of claim 1, wherein the angular velocity processor further causes the processor to determine the second angular velocity component using a cross product between the gravitational vector measurements of the accelerometer, wherein the gravitational vector measurements are normalized to unit length.

7. The system of claim 1, wherein the angular velocity processor further causes the processor to determine the third angular velocity component using a rotation matrix comprising the magnetic vector measurements of the magnetometer and the gravitational vector measurements of the accelerometer.

8. The system of claim 1, wherein the angular velocity processor further causes the processor to calculate the projection of the third angular velocity component around the first unobservable axis using a dot product between the third angular velocity component and the magnetic vector measurements of the magnetometer normalized to unit length.

9. The system of claim 8, wherein the first unobservable axis is an axis in the direction of the magnetic field vector.

10. The system of claim 1, wherein the angular velocity processor further causes the processor to calculate the projection of the third angular velocity component around the second unobservable axis using a dot product between the third angular velocity component and the gravitational vector measurements of the accelerometer normalized to unit length.

11. The system of claim 10, wherein the second unobservable axis is in the direction of the gravitation field vector.

12. The system of claim 1, wherein the angular velocity processor causes the processor to determine the first and second angular velocity components using filtered magnetic vector and gravitational vector measurements.

13. The system of claim 12, further comprising a filter stored in the memory and wherein the filter causes the processor to filter the magnetic vector measurements provided by the magnetometer based on a function associated with the estimated angular velocity.

14. The system of claim 12, further comprising a filter stored in the memory and wherein the filter causes the processor to filter the gravitational vector measurements provided by the accelerometer based on a function associated with the estimated angular velocity.

15. The system of claim 12, further comprising a filter stored in the memory and wherein the filter causes the processor to filter the magnetic vector measurements provided by the magnetometer based on a function associated with an estimated angular acceleration.

16. The system of claim 12, further comprising a filter stored in the memory and wherein the filter causes the processor to filter the gravitational vector measurements provided by the accelerometer based on a function associated with an estimated angular acceleration.

17. The system of claim 1, further comprising a gyroscope calibration module stored in the memory and executing on the processor and causing the processor to:
determine sensitivity of a bias-calibrated gyroscope using the angular velocity estimate and an angular velocity generated using the calibrated gyroscope.

18. The system of claim 1, further comprising a gyroscope calibration module stored in the memory and executing on the processor and causing the processor to:
determine sensitivity and a bias offset of a non-calibrated gyroscope using the angular velocity estimate and an angular velocity generated using the non-calibrated gyroscope and linear regression.