Patent Publication Number: US-9846040-B2

Title: System and method for determining the orientation of an inertial measurement unit (IMU)

Description:
BACKGROUND OF THE INVENTION 
     1. Field of the Invention 
     This invention generally relates to location and movement determination and, more particularly, to systems and methods for determining the spatial orientation of an inertial measurement unit (IMU). 
     2. Description of the Related Art 
     Inertial measurement units, also called inertial motion units, are devices that facilitate continuous computation of the orientation of the objects to which they are attached. By way of example, IMUs may be attached to human body limbs, robotic appendages, or aerial drones. IMUs typically have multiple single- or multi-axis sensors, such as gyroscopes, accelerometers, and magnetometers. Ideally, the accelerometer and magnetometer respectively measure the field strength of the local gravitational and magnetic fields produced by a reference object, such as Earth, and in turn produce sensor field strength readings. The gyroscope measures the rotational speed of the sensor relative to itself, producing a sensor rotational speed reading. The sensor field strength and rotational speed readings may be used singularly or in combination to compute the orientation of the IMU and its associated objects relative to the reference object. This object orientation is typically applied in an object motion tracking application. 
     Even well calibrated IMU sensors can exhibit measurement bias and measurement jitter, potentially causing noisy results in computed orientations. Further, gravitational and inertial accelerations both affect accelerometer output readings, causing an undeterminable offset in the sensor&#39;s reading from the actual direction of gravity, and therefore an uncertainty in the sensor&#39;s orientation relative to local gravitational up and down. Methods already exist for fusing IMU sensor values together to determine object orientation and mitigate sensor offsets and jitters, but they typically employ Kalman filters, which require matrix inversion and are computationally expensive. As a result, IMUs employing these methodologies typically require more expensive and current consuming processors. The larger current consumption results in the need for larger batteries or reduced operational time. 
     It would be advantageous if a very low processing power method existed for continuously estimating the orientation of an IMU, enabling the IMU to operate on lower performing processors, and using less electrical power to extend the battery life of the system. 
     It would be advantageous if this low processing power method could compensate for sensor errors such as accelerometers deviating from pointing in the direction of gravity during inertial accelerations, magnetometers that tend to have high jitter noise levels, and gyroscopes that jitter and can cause orientation estimation drift. 
     SUMMARY OF THE INVENTION 
     Disclosed herein are machine-specific algorithms modified to receive rotational velocity measurements from a 3-axis gyroscope, gravitational measurements from a 3-axis accelerometer, and (optionally) magnetic field measurements from a magnetometer, all contained in an inertial measurement unit (IMU). Two estimates of the IMU&#39;s orientation are calculated in quaternion form. The first estimate, called the gyroscopic quaternion, is based upon the previous orientation estimate of the IMU and the current gyroscope readings. The second estimate, called the field quaternion, is based upon the accelerometer and magnetometer readings. The field quaternion estimate error is calculated from the readings of all three sensor types. The gyroscopic and field quaternion estimates are combined together to form a current sample quaternion. The relative weighting of gyroscopic and field quaternions used to form the current sample quaternion is based upon the field quaternion estimate error and the angular distance between the gyroscopic and field quaternions. If the desired quaternion output rate is less than the sampled quaternion rate, then multiple consecutive sample quaternions are filtered together and decimated to produce the output quaternion. Various types of decimation filters are possible, including quaternion median and low-pass filters. 
     Some unique aspects of the methodology disclosed herein include the ability to estimate when the IMU is relatively still and then updating the magnetometer jitter and the accelerometer bias offset estimates. Likewise, the gyroscope bias estimate can be measured when the gyroscope readings are relatively small, to reduce gyroscope drift. The angular error can be estimated between accelerometer readings and Earth&#39;s gravitational field due to sensor inertial motion. The angular error can be estimated between magnetometer readings and Earth&#39;s magnetic field due to jitter in magnetometer readings, and the combined angular error of the accelerometer and magnetometer readings can also be estimated. Another feature is the ability to calculate a stable quaternion from two vectors, even when the two vectors approach an anti-parallel orientation. Further, a stable field quaternion can be calculated from acceleration and gyroscopic sensors, and the gyroscopic and field quaternions can be combined using a weighting based upon estimated field quaternion angular error, and the angular difference between the gyroscopic and field quaternions. The resulting quaternion output can be decimated using a quaternion median filter. 
     Accordingly, a method is provided for determining the orientation of an IMU. The method calculates a gyroscopic quaternion, and when the IMU accelerometer reading is about equal to gravity (1 G), the method calculates a field quaternion using IMU accelerometer readings. Estimates are made of angular orientation errors due to IMU angular velocity and linear acceleration, and these angular orientation errors are used to selectively mix the gyroscopic quaternion and field quaternion to supply a current sample quaternion. Alternatively, if the accelerometer reading is not about equal to 1 G, the gyroscopic quaternion is used as the current sample quaternion. 
     In one aspect, IMU gyroscope readings and IMU accelerometer readings are calibrated in response to determining a lack of IMU movement. Near-zero gyroscope reading jitter is removed by setting the IMU gyroscopic reading to zero when the gyroscopic reading is near zero. When a magnetometer is used, IMU magnetometer jitter errors are also estimated, in one aspect, in response to determining a lack of movement. Then, the field quaternion is calculated using the IMU accelerometer readings and IMU magnetometer readings. The gyroscopic quaternion and field quaternion are selectively mixed in response to the angular orientation errors and the estimated IMU magnetometer jitter errors. 
     Prior to calculating the field quaternion, a total angular orientation error is calculated by combining the orientation angular error due to centripetal acceleration, the orientation angular error due to linear acceleration, and (optionally) the orientation angular error due to estimated IMU magnetometer jitter. 
     The gyroscopic quaternion and field quaternion are selectively mixed to supply the current sample quaternion by calculating an angular difference between the gyroscopic quaternion and field quaternion, comparing the angular difference to the total angular orientation error, and adjusting the mixture in response to the comparison. 
     In one aspect, the field quaternion is calculated by initially calculating a gravitational quaternion able to rotate an accelerometer reading so that the normalized Z-axis component of the accelerometer reading is equal to 1. Optionally, the gravitational quaternion is used to rotate a magnetometer reading, and the rotated magnetometer reading&#39;s Z-axis component is set to zero, creating an adjusted magnetometer reading. A magnetometer quaternion is calculated that is able to further rotate the adjusted magnetometer reading so that the normalized X-axis component is equal to −1. Then, the gravitational quaternion and the magnetometer quaternion are combined. 
     Additional details of the above-described method and an IMU device are provided below. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         FIG. 1  is a schematic block diagram of an inertial measurement unit (IMU). 
         FIG. 2  is an overall flowchart map for an orientation calculation algorithm depicted in  FIGS. 2A through 2C , specifically tailored for use in the IMU of  FIG. 1 . 
         FIGS. 3A and 3B  are a flowchart illustrating a summarized method for determining the orientation of an inertial measurement unit. 
         FIG. 4  is a flowchart illustrating a method for an inertial measurement unit to calculate an orientation-related quaternion. 
         FIG. 5  is a diagram depicting a quaternion coordinate system (prior art). 
         FIG. 6  is a diagram depicting the most error producing direction of centripetal acceleration relative to gravitational acceleration. 
         FIG. 7  is a diagram depicting the most error producing direction of inertial acceleration relative to gravitational acceleration. 
         FIG. 8  is a diagram depicting a method for calculating a stable quaternion from two obtuse vectors. 
     
    
    
     DETAILED DESCRIPTION 
       FIG. 1  is a schematic block diagram of an inertial measurement unit (IMU). The IMU  100  comprises a gyroscope  102  having an output on line  104  to supply gyroscopic readings. An accelerometer  106  has an output on line  108  to supply accelerometer readings. The IMU  100  further comprises a controller  110 , with a processor  112 , a non-transitory memory  114 , and an IMU application  116  residing in the non-transitory memory. The IMU application  116  comprises a sequence of processor executable instructions for calculating a gyroscopic quaternion in response to gyroscopic readings, and calculating a field quaternion using accelerometer readings when an accelerometer reading is about equal to gravity (1 G). The IMU application  116  estimates angular orientation errors due to IMU angular velocity and linear acceleration, and uses the angular orientation errors to selectively mix the gyroscopic quaternion and field quaternion to supply a current sample quaternion via interface  118 . Alternatively, the IMU application  116  uses the gyroscopic quaternion as the current sample quaternion when the accelerometer reading is not about equal to 1 G. The term “about equal to gravity” in this context typically means a reading magnitude that is within 1% of 1 G. 
     Optionally, the IMU  100  further comprises a magnetometer  120  (shown in phantom) having an output on line  122  to supply magnetometer readings. In this case, the IMU application  116  estimates magnetometer jitter errors in response to magnetometer readings, and calculates the field quaternion using the accelerometer readings and magnetometer readings. Then, the IMU application  116  selectively mixes the gyroscopic quaternion and field quaternion includes in response to the angular orientation errors and the estimated magnetometer jitter errors. 
     The controller  110  typically employs a bus  124  or other communication mechanism for communicating information between the sensors  102  and  106  (and optionally  120 ), processor  112 , memory  114 , and interface  118 . The communication bus  124  may, for example, be a Serial Peripheral Interface (SPI), an Inter-Integrated Circuit (I 2 C), a Universal Asynchronous Receiver/Transmitter (UART), and/or any other suitable bus or network. Although the drawing implies that the components of the controller  110  are collocated in the same device, in some aspects various components may be located outside the IMU device, communicating with other components of the IMU via a hardwire or wireless connection. 
     The memory  114  may include a main memory, a random access memory (RAM), or other dynamic storage devices. These memories may also be referred to as a computer-readable medium. Such a medium may take many forms, including but not limited to, non-volatile media, volatile media, and transmission media. Non-volatile media includes, for example, optical or magnetic disks. Volatile media includes dynamic memory. Common forms of computer-readable media include, for example, a floppy disk, a flexible disk, hard disk, magnetic tape, or any other magnetic medium, a CD-ROM, any other optical medium, punch cards, paper tape, any other physical medium with patterns of holes, a RAM, a PROM, and EPROM, a FLASH-EPROM, any other memory chip or cartridge, or any other medium from which a computer can read. The execution of the sequences of instructions contained in a computer-readable medium (i.e. IMU application  116 ) may cause the processor  112  to perform some of the steps of estimating readings and calculating quaternions. Alternately, some of these functions may be performed in hardware (not shown). The practical implementation of such a computer system would be well known to one with skill in the art. In one aspect, the processor  112  is an ARM processor using a reduced instruction set computing (RISC) architecture. 
     The interface  118  may be more than one interface. The interface may be a display, a modem, an Ethernet card, or any other appropriate data communications device such as USB. The physical communication links may be optical, wired, or wireless. The interface  118  may incorporate appropriate input/output (IO) ports for user interface interconnection, such as a keypad or a cursor control device such as a mouse, touchpad, touchscreen, trackball, stylus, or cursor direction keys. 
     Application Ser. No. 14/707,194, filed May 8, 2015, entitled METHOD AND SYSTEM FOR WIRELESS TRANSMISSION OF QUATERNIONS, invented by Bryan Hallberg, which is incorporated herein by reference, describes a method of efficiently transmitting quaternions over a wireless network from multiple sensors. 
     The IMU  100  may be considered a type of special purpose computing system, and as such, can be programmed, configured, and/or otherwise designed to comply with one or more networking protocols. According to certain embodiments, the IMU  100  may be designed to work with protocols of one or more layers of the Open Systems Interconnection (OSI) reference model, such as a physical layer protocol, a link layer protocol, a network layer protocol, a transport layer protocol, a session layer protocol, a presentation layer protocol, and/or an application layer protocol. For example, interface  118  may include a network device configured according to a Universal Serial Bus (USB) protocol, an Institute of Electrical and Electronics Engineers (IEEE) 1394 protocol, an Ethernet protocol, a T1 protocol, a Synchronous Optical Networking (SONET) protocol, a Synchronous Digital Hierarchy (SDH) protocol, an Integrated Services Digital Network (ISDN) protocol, an Asynchronous Transfer Mode (ATM) protocol, a Point-to-Point Protocol (PPP), a Point-to-Point Protocol over Ethernet (PPPoE), a Point-to-Point Protocol over ATM (PPPoA), a Bluetooth protocol, an IEEE 802.XX protocol, a frame relay protocol, a token ring protocol, a spanning tree protocol, and/or any other suitable protocol. 
     Examples of interface  118  include, without limitation, a link adapter, a wired network interface (such as a network interface card), a wireless network interface (such as a wireless network interface card), and any other suitable interface. In at least one embodiment, interface  118  may provide a direct connection to a remote server via a direct link to a network, such as the Internet. Interface  118  may also indirectly provide such a connection through, for example, a local area network (such as an Ethernet network), a personal area network, a wide area network, a private network (e.g., a virtual private network), a telephone or cable network, a cellular telephone connection, a satellite data connection, or any other suitable connection. 
     In certain embodiments, the interface  118  may also represent a host adapter configured to facilitate communication between IMU  100  and one or more network or storage devices via an external bus or communications channel. Examples of host adapters include, without limitation, Small Computer System Interface (SCSI) host adapters, Universal Serial Bus (USB) host adapters, IEEE 1394 host adapters, Advanced Technology Attachment (ATA), Parallel ATA (PATA), Serial ATA (SATA), and External SATA (eSATA) host adapters, Fibre Channel interface adapters, Ethernet adapters, or the like. 
     A conventional gyroscope measures angular rotation speed, and has non-zero bias offset that jitters and varies with sensor temperature. Over time, the bias offset creates integration errors. A conventional accelerometer measures the direction of sensor acceleration, but it cannot distinguish between gravity and inertial acceleration (i.e., an axiom of General Relativity). A conventional magnetometer measures the direction of a local magnetic field, and it cannot distinguish between Earth&#39;s field and any other nearby fields. Unfortunately, magnetometer readings also typically include significant noise. 
     Ideally, an IMU&#39;s orientation could be calculated by knowing its previous location and integrating the angular velocity reported by a 3-axis gyroscope. However, gyroscope bias, and especially jittering bias, cause integration errors over time and subsequently cause the orientation estimate to drift away from the IMU&#39;s actual orientation. 
     Alternatively, an IMU&#39;s orientation could be calculated from its accelerometer and magnetometer readings. However, accelerometers cannot distinguish between gravity and inertial acceleration. So, gravity&#39;s direction cannot be accurately determined when the IMU is either rotating or linearly accelerating. Also, magnetometers produce readings that jitter substantially. So, the calculated IMU&#39;s orientation may also jitter when based upon a jittering magnetometer reading. To compensate for the above issues, a sensor fusion algorithm can be used that estimates and mitigates for these errors. Such sensor fusion algorithms are most often based on Kalman filters that require extensive processing power. 
     The IMU application  116  uses an orientation algorithm to convert the readings from the three types of 3-axis sensors (accelerometer, magnetometer (optionally), gyroscope) into a single spatial orientation vector represented as a quaternion. Quaternions are selected because they are mathematically efficient and stable relative to other options, such as Euler angles. A common property of Euler Angles, Tait-Bryan Angles, and quaternions is that they describe the orientation of one spatial coordinate system relative to another spatial coordinate system. Quaternions uses a 4-element rotation vector (r, x, y, z), which requires no trigonometric functions to calculate results. Concatenating rotations are computationally fast and numerically stable. Euler angles and Tait-Bryan angles, on the other hand, uses a 3-element rotation vector (α, β, γ), which requires trigonometric functions to calculate results. In addition, these methods suffer from gimbal lock, an inability to distinguish between two rotational axes, such as when one axis is rotated parallel to a second axis, and they can be numerically unstable. 
       FIG. 5  is a diagram depicting a quaternion coordinate system (prior art). The quaternion 4-element vector (r, x, y, z) is based on a rotation angle θ and unit length rotation axis  . A quaternion that rotates an object from spatial coordinate system A to spatial coordinate system B is defined as the normalized vector  A   B Q. 
     
       
         
           
             
               
                   
                 A 
                 B 
               
               ⁢ 
               Q 
             
             = 
             
               
                 ( 
                 
                   r 
                   , 
                   x 
                   , 
                   y 
                   , 
                   z 
                 
                 ) 
               
               = 
               
                 r 
                 + 
                 
                   
                     i 
                     ^ 
                   
                   ⁢ 
                   x 
                 
                 + 
                 
                   
                     j 
                     ^ 
                   
                   ⁢ 
                   y 
                 
                 + 
                 
                   
                     k 
                     ^ 
                   
                   ⁢ 
                   z 
                 
               
             
           
         
       
       
         
           
             
               
                 i 
                 ^ 
               
               2 
             
             = 
             
               
                 
                   j 
                   ^ 
                 
                 2 
               
               = 
               
                 
                   
                     k 
                     ^ 
                   
                   2 
                 
                 = 
                 
                   
                     
                       i 
                       ^ 
                     
                     ⁢ 
                     
                       j 
                       ^ 
                     
                     ⁢ 
                     
                       k 
                       ^ 
                     
                   
                   = 
                   
                     - 
                     1 
                   
                 
               
             
           
         
       
       
         
           
             
               r 
               = 
               
                 cos 
                 ⁡ 
                 
                   ( 
                   
                     θ 
                     2 
                   
                   ) 
                 
               
             
             ; 
             
               x 
               = 
               
                 
                   u 
                   x 
                 
                 ⁢ 
                 
                   sin 
                   ⁡ 
                   
                     ( 
                     
                       θ 
                       2 
                     
                     ) 
                   
                 
               
             
             ; 
             
               y 
               = 
               
                 
                   u 
                   y 
                 
                 ⁢ 
                 
                   sin 
                   ⁡ 
                   
                     ( 
                     
                       θ 
                       2 
                     
                     ) 
                   
                 
               
             
             ; 
             
               z 
               = 
               
                 
                   u 
                   z 
                 
                 ⁢ 
                 
                   sin 
                   ⁡ 
                   
                     ( 
                     
                       θ 
                       2 
                     
                     ) 
                   
                 
               
             
           
         
       
       
         
           
             
               
                 r 
                 2 
               
               + 
               
                 x 
                 2 
               
               + 
               
                 y 
                 2 
               
               + 
               
                 z 
                 2 
               
             
             = 
             1 
           
         
       
     
     Null Rotation
 
θ=0
 
 Q =(1,0,0,0)
 
     Inverse of a Given Quaternion
 
 A   B   Q′=   B   A   Q  
 
θ→−θ;( r,x,y,z )→( r,−x,−y,−z )
 
     Multiplication
 
 Q   1   Q   2 =( r   1   +îx   1   +ĵy   1   +{circumflex over (k)}z   1 )( r   2   +îx   2   +ĵy   2   +{circumflex over (k)}z   2 )
 
 Q   1   Q   2 =( r   1   r   2   −x   1   x   2   −y   1   y   2   −z   1   z   2 )+{circumflex over ( i )}( r   1   x   2   +x   1   r   2   +y   1   z   2   −z   1   y   2 )+{circumflex over ( j )}( r   1   y   2   −x   1   z   2   +y   1   r   2   +z   1   x   2 )+{circumflex over ( k )}( r   1   z   2   +x   1   y   2   −y   1   x   2   +z   1   r   2 )
 
     Transpose Multiplication (Quaternion Transpose Includes Conjugation)
 
 R ( Q   1   Q′   2 )= Q   1   T   Q   2   =r   1   r   2   +x   1   x   2   +y   1   y   2   +z   1   z   2  
 
     Concatenated Rotations 
     Concatenated rotations are calculated by multiplying quaternions in order of rotations
 
 A   C   Q=   B   C   Q   A   B   Q  
 
     Rotating a Vector from Frame a to Frame B
 
 V   B = A   B   QV   AA   B   Q′=   A   B   QV   AB   A   Q  
 
     The application-specific algorithm disclosed herein calculates the orientation of an IMU in quaternion form and mitigates errors due to sensor bias offsets, sensor jitter, and the indistinguishability of gravity and inertial accelerations. This application-specific algorithm runs on low computational power processors to reduce overall system cost and battery power consumption. Each sensor typically has a non-zero bias that may be removed from the raw reading to obtain an estimated bias-free reading. Application Ser. No. 14/742,852, filed Jun. 18, 2015, entitled SENSOR CALIBRATION METHOD AND SYSTEM, invented by Bryan Hallberg, which is incorporated herein by reference, describes a method of estimating and removing the estimated bias of the accelerometer and magnetometer readings. A method for removing estimated gyroscope bias from the gyroscope&#39;s raw reading is described herein. 
     The IMU application  116  estimates the IMU magnetometer jitter errors (if present), and calibrates the gyroscope readings and accelerometer readings in response to determining a lack of IMU movement. 
       FIG. 2  is an overall flowchart map for an orientation calculation algorithm depicted in  FIGS. 2A through 2C , specifically tailored for use in the IMU of  FIG. 1 . In  FIG. 2A , the “Bias &amp; Jitter Estimation” block ( 200 ) first calculates estimates of the relative stillness of the IMU. Initially, the current and previous raw gyroscope readings are compared to each other ( 202 ) and the magnitude of the estimated bias-free accelerometer reading is compared to gravity ( 204 ). If the square of the magnitude of the gyroscope difference is less than a predetermined amount (typically the square of 1 degree per second) and the square of the magnitude of the accelerometer is different from the square of gravity by less than a predetermined amount (typically the square of 1% of gravity), then the IMU is considered to be instantaneously relatively still ( 206 ). 
     If the IMU is not instantaneously relatively still, then the still counter is reset to zero ( 208 ). Otherwise, it is incremented ( 210 ). In  FIG. 2B , if the IMU has been relatively still for at least a predetermined consecutive number of measurements (e.g., 4) ( 212 ), then the IMU is considered sufficiently still to update bias and jitter estimates ( 214 ). 
     
       
         
           
             If 
             ⁢ 
             
                 
             
             ⁢ 
             
               
                 ∏ 
                 0 
                 3 
               
               ⁢ 
               
                   
               
               ⁢ 
               
                 ( 
                 
                   
                     ( 
                     
                       
                         
                            
                           
                             
                               ω 
                               n 
                             
                             - 
                             
                               ω 
                               
                                 n 
                                 - 
                                 1 
                               
                             
                           
                            
                         
                         2 
                       
                       &lt; 
                       
                         ω 
                         Threshold 
                         2 
                       
                     
                     ) 
                   
                   &amp;&amp; 
                   
                     ( 
                     
                       
                          
                         
                           
                             
                                
                               
                                 a 
                                 n 
                               
                                
                             
                             2 
                           
                           - 
                           
                             g 
                             2 
                           
                         
                          
                       
                       &lt; 
                       
                         a 
                         Threshold 
                         2 
                       
                     
                     ) 
                   
                 
                 ) 
               
             
           
         
       
     
     Then IMU is Relatively Still 
     The gyroscope bias is updated by inputting the previous gyroscope reading into a 3-dimensional low-pass filter. The filter independently filters each of the three axes. The previous reading is used because it may be more still than the current reading, since it is known to lie between two relatively still readings. The output of the filter is the updated estimated gyroscope bias. 
     Accelerometer bias estimates can also be updated, by calling its particular bias update routine. An example bias update routine is disclosed in parent application Ser. No. 14/742,852. The magnetometer jitter estimate is also updated when the IMU is relatively still. The magnitude of the instantaneous jitter is measured by calculating the magnitude of the difference between the current and the immediately previous estimated bias-free magnetometer readings. This instantaneous magnitude is input into a one-dimensional low-pass filter. The output of the filter is the updated estimated magnetometer jitter.
 
   J   m   = f   lpf (∥ m   n   −m   n-1 ∥)
 
     Returning to  FIG. 2A , after exiting the “Bias &amp; Jitter Estimation” block ( 200 ), the current accelerometer and magnetometer estimated bias-free readings are each stored in a history buffer (e.g., memory  114  of  FIG. 1 ) used later for sensor jitter mitigation. The length of these buffers is equal to the maximum length of the jitter mitigation filter, e.g., 15 samples. The current raw gyroscope reading is stored in a history buffer that only needs to be one sample long ( 216 ). That value is used in the relative stillness estimation described above. 
     After storing the current values into the history buffers, processing continues with the “Gyroscopic Quaternion Calculation” block ( 218 ). This block calculates an updated estimate for the IMU orientation based upon the previous IMU orientation estimate and the current estimated bias-free gyroscope reading. First, the estimated bias-free reading is compared to zero ( 220 ). If the value is near zero (e.g., within two for a gyroscope that produces values between +/−32767 for rotational speeds between +/−2000 degrees per second), then the reading is set to zero ( 222 ). This removes errors caused by small gyroscope readings that contain only jitter information when the IMU is still. 
     Next, the updated estimated gyroscopic quaternion is calculated using the estimated bias-free gyroscope reading ( 224 ). The actual calculation is performed as follows (prior art). 
     
       
         
           
             
                 
             
             ⁢ 
             
               
                 Q 
                 i 
                   
                   
                 E 
                 S 
               
               = 
               
                 previous 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 quaternion 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 G 
                 ω 
               
               = 
               
                 
                   ( 
                   
                     0 
                     , 
                     
                       ω 
                       x 
                     
                     , 
                     
                       ω 
                       y 
                     
                     , 
                     
                       ω 
                       z 
                     
                   
                   ) 
                 
                 = 
                 
                   gyroscope 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   reading 
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 
                   Q 
                   . 
                 
                 ω 
                   
                   
                 E 
                 S 
               
               = 
               
                 
                   
                     1 
                     2 
                   
                   ⁢ 
                   
                     Q 
                     i 
                       
                       
                     E 
                     S 
                   
                   ⁢ 
                   
                     G 
                     ω 
                   
                 
                 = 
                 
                   quaternion 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   rate 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   of 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   change 
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 Δ 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   Q 
                   ω 
                 
               
               = 
               
                 
                   
                     Q 
                     . 
                   
                   ω 
                     
                     
                   E 
                   S 
                 
                 ⁢ 
                 Δ 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 t 
               
             
           
         
       
       
         
           
             
               Q 
               
                 ω 
                 , 
                 
                   i 
                   + 
                   1 
                 
               
                 
                 
               E 
               S 
             
             = 
             
               
                 
                   Q 
                   i 
                     
                     
                   E 
                   S 
                 
                 + 
                 
                   Δ 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     Q 
                     ω 
                   
                 
               
               = 
               
                 
                   
                     Q 
                     i 
                       
                       
                     E 
                     S 
                   
                   + 
                   
                     
                       1 
                       2 
                     
                     ⁢ 
                     
                       Q 
                       i 
                         
                         
                       E 
                       S 
                     
                     ⁢ 
                     
                       G 
                       ω 
                     
                     ⁢ 
                     Δ 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     t 
                   
                 
                 = 
                 
                   new 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   quaternion 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   after 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   time 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   Δ 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     t 
                     . 
                   
                 
               
             
           
         
       
     
     After exiting the “Gyroscopic Quaternion Calculation” block ( 218 ), processing continues with the “Field Sensor Error Measurement” block ( 226 ). This block estimates the amount of angular error that a quaternion would have if it were calculated from only accelerometer and magnetometer estimated bias-free readings. 
     If the accelerometer estimated bias-free reading is not near 1 G ( 228 ), then the reading has definitely been significantly affected by inertial motion of the IMU and, ideally, should not be used to calculate the IMUs orientation. 
     a m =a i +g=accelerometer measurement, sum of inertial acceleration and gravity. The greatest angular error occurs when the inertial acceleration is perpendicular to either gravity or the measured acceleration. The estimated error separates into two cases: 
     
       
         
           
             
                
               
                 
                    
                   
                     
                        
                       
                         a 
                         m 
                       
                        
                     
                     - 
                     
                        
                       g 
                        
                     
                   
                    
                 
                 
                    
                   g 
                    
                 
               
                
             
             &gt; 
             
               
                 a 
                 Threshold 
               
               ⁢ 
               
                   
               
               ⁢ 
               case 
               ⁢ 
               
                 : 
               
             
           
         
       
     
     The accelerometer measurement is outside acceptable limits and the accelerometer measurement is considered not valid. If the magnitude of the accelerometer estimated bias-free reading differs from 1 G by more than a predetermined value (e.g., 0.25 G), then the gyroscopic quaternion is selected as the current sample quaternion ( 229 ) and processing continues with the “Output Quaternion Filter” block ( 230 ). 
     
       
         
           
             
                
               
                 
                    
                   
                     
                        
                       
                         a 
                         m 
                       
                        
                     
                     - 
                     
                        
                       g 
                        
                     
                   
                    
                 
                 
                    
                   g 
                    
                 
               
                
             
             ≤ 
             
               
                 a 
                 Threshold 
               
               ⁢ 
               
                   
               
               ⁢ 
               case 
               ⁢ 
               
                 : 
               
             
           
         
       
     
     The accelerometer measurement is within acceptable limits and is a i  assumed sufficiently near 0 to not adversely influence the estimate of the IMU&#39;s orientation relative to gravity. The measurement might still be not valid if ∥a i +g∥≈∥g∥. 
     Processing continues with estimating the centripetal acceleration acting upon the IMU ( 232 ), as IMU rotation affects accelerometer measurements. Referring briefly to  FIG. 6 , for an IMU placed a distance l from the rotational center, the centripetal acceleration is given by
 
 a   r   =l∥ω∥   2 =centripetal acceleration measured by sensor accelerometer;
 
ω=angular velocity;
 
l=distance from rotation center.
 
     In the measurement error calculation, the greatest angular error occurs when centripetal acceleration is perpendicular to gravity&#39;s direction and is given by: 
     
       
         
           
             
               θ 
               c 
             
             = 
             
               
                 arctan 
                 ⁡ 
                 
                   ( 
                   
                     
                       a 
                       r 
                     
                     
                        
                       g 
                        
                     
                   
                   ) 
                 
               
               = 
               
                 arctan 
                 ( 
                 
                   l 
                   ⁢ 
                   
                     
                       
                          
                         ω 
                          
                       
                       2 
                     
                     
                        
                       g 
                        
                     
                   
                 
                 ) 
               
             
           
         
       
     
     The angular velocity required to reach a particular angular error threshold can be calculated from: 
     
       
         
           
             
                
               ω 
                
             
             = 
             
               
                 
                   
                      
                     g 
                      
                   
                   l 
                 
                 ⁢ 
                 
                   tan 
                   ⁡ 
                   
                     ( 
                     
                       θ 
                       Threshold 
                     
                     ) 
                   
                 
               
             
           
         
       
       
         
           
             
               
                  
                 ω 
                  
               
               2 
             
             = 
             
               
                 
                    
                   g 
                    
                 
                 l 
               
               ⁢ 
               
                 tan 
                 ⁡ 
                 
                   ( 
                   
                     θ 
                     Threshold 
                   
                   ) 
                 
               
             
           
         
       
     
     If the centripetal acceleration calculated from the estimated bias-free gyroscope reading is significant relative to gravity ( 234 ), then the accelerometer reading has been significantly affected by rotational motion of the IMU and, ideally, should not be used to calculate the IMU&#39;s orientation. The square of magnitude of the estimated bias-free gyroscope reading is compared to a maximum allowed value dependent upon an allowed maximum angle error (e.g., 30 degrees) and an assumed IMU member length l (e.g., if mounted on a human limb−1 meter). If the reading&#39;s magnitude is larger than this threshold value, then the gyroscopic quaternion is selected as the current sample quaternion ( 229 ) and processing continues with the “Output Quaternion Filter” block ( 230 ). Otherwise, processing continues with estimating the angular error caused by centripetal acceleration acting upon the IMU ( 236 ). 
     The centripetal angular error estimate is performed with the following equations using the previously assumed maximum member length. Since the centripetal angular error is small, the arctangent function can be estimated simply using the computed value to reduce processing power requirements. This is possible because the first term of the Taylor expansion for arctan(x) is simply x. 
     
       
         
           
             
               θ 
               c 
             
             = 
             
               
                 arctan 
                 ⁡ 
                 
                   ( 
                   
                     
                       a 
                       r 
                     
                     
                        
                       g 
                        
                     
                   
                   ) 
                 
               
               = 
               
                 
                   arctan 
                   ( 
                   
                     l 
                     ⁢ 
                     
                       
                         
                            
                           ω 
                            
                         
                         2 
                       
                       
                          
                         g 
                          
                       
                     
                   
                   ) 
                 
                 ≅ 
                 
                   l 
                   ⁢ 
                   
                     
                       
                          
                         ω 
                          
                       
                       2 
                     
                     
                        
                       g 
                        
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   for 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   small 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     c 
                   
                 
               
             
           
         
       
     
     Referring to  FIGS. 7 and 2B , the angular error caused by the IMU&#39;s linear acceleration ( 238 ) is estimated by assuming that any difference between the magnitude of the estimated bias-free accelerometer reading and gravity is caused by an acceleration perpendicular to gravity or to the measured acceleration, as acceleration in these directions would cause the greatest angular error. If the estimated bias-free acceleration is larger than gravity, then the angular error is estimated by the following equation. Since the angular error is small, the sine function can be estimated using simply the computed value to reduce processing power requirements. This is possible because the first term of the Taylor expansion for sin(x) is simply x. 
     
       
         
           
             
               θ 
               l 
             
             = 
             
               
                 arcsin 
                 ( 
                 
                   
                     
                       
                         
                            
                           a 
                            
                         
                         2 
                       
                       - 
                       
                         g 
                         2 
                       
                     
                   
                   
                      
                     a 
                      
                   
                 
                 ) 
               
               ≅ 
               
                 
                   
                     
                       
                         
                            
                           a 
                            
                         
                         2 
                       
                       - 
                       
                         g 
                         2 
                       
                     
                   
                   
                      
                     a 
                      
                   
                 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 for 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 small 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   θ 
                   l 
                 
               
             
           
         
       
     
     If the estimated bias-free acceleration is less than gravity, then the angular error is estimated by 
     
       
         
           
             
               θ 
               l 
             
             = 
             
               
                 arcsin 
                 ( 
                 
                   
                     
                       
                         g 
                         2 
                       
                       - 
                       
                         
                            
                           a 
                            
                         
                         2 
                       
                     
                   
                   
                     g 
                     2 
                   
                 
                 ) 
               
               ≅ 
               
                 
                   
                     
                       
                         g 
                         2 
                       
                       - 
                       
                         
                            
                           a 
                            
                         
                         2 
                       
                     
                   
                   
                     g 
                     2 
                   
                 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 for 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 small 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   θ 
                   l 
                 
               
             
           
         
       
     
     Magnetometers typically jitter much more than accelerometers, so field quaternion jitter mitigation is governed by magnetometer jitter measurements. The set of estimated bias-free magnetometer readings just previous to the current reading, and within a predetermined distance from the current reading, is located starting at the reading just previous in time to the current reading and moving backward in time from there. The set has a predetermined number of maximum readings (e.g., 16). The magnitude of the squared distance between the current reading and a previous reading is calculated using the squared magnitude of the difference between the current estimated bias-free magnetometer reading and the previous reading. The set ends when the squared magnitude distance (i.e., angle) to a previous reading is greater than or equal a multiple (e.g., 4) of the squared magnitude of the estimated magnetometer jitter calculated by the “Bias &amp; Jitter Estimation” block ( 200 ). The average magnetometer reading is calculated by averaging together the current estimated bias-free magnetometer reading and all magnetometer readings in the set. The average accelerometer reading is calculated using the current estimated bias-free accelerometer reading and the set of accelerometer readings temporally co-located with the set of located magnetometer readings ( 240 ). The magnitude of the current magnetometer reading&#39;s jitter is estimated by dividing the estimated magnetometer jitter by the number of readings used to calculate the average magnetometer reading. The jitter angular error is estimated by dividing the magnitude of the current reading&#39;s jitter by the magnitude of the average magnetometer reading. 
     Measure the jitter between current magnetometer reading and previous readings:
 
 N =min( N   max ,(∥ m   0   −m   0-n ∥ 2 &lt;α i     j   m     2 ))=total previous readings;
 
N max =16;
 
α j =4=jitter distance threshold multiplier.
 
     Measure the magnetometer and accelerometer average reading over previous low jitter readings and estimate jitter: 
     
       
         
           
             
                 
             
             ⁢ 
             
               
                 
                   m 
                   _ 
                 
                 = 
                 
                   
                     
                       1 
                       
                         N 
                         + 
                         1 
                       
                     
                     ⁢ 
                     
                       
                         
                           ∑ 
                           0 
                         
                         N 
                       
                       ⁢ 
                       
                         ( 
                         
                           m 
                           
                             0 
                             - 
                             n 
                           
                         
                         ) 
                       
                     
                   
                   = 
                   
                     average 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     magnetometer 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     reading 
                   
                 
               
               ; 
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 
                   a 
                   _ 
                 
                 = 
                 
                   
                     
                       1 
                       
                         N 
                         + 
                         1 
                       
                     
                     ⁢ 
                     
                       
                         
                           ∑ 
                           0 
                         
                         N 
                       
                       ⁢ 
                       
                         ( 
                         
                           a 
                           
                             0 
                             - 
                             n 
                           
                         
                         ) 
                       
                     
                   
                   = 
                   
                     average 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     accelerometer 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     reading 
                   
                 
               
               ; 
             
           
         
       
       
         
           
             
               JmN 
               _ 
             
             = 
             
               
                 
                   Jm 
                   _ 
                 
                 
                   N 
                   + 
                   1 
                 
               
               = 
               
                 estimated 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 jitter 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 in 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 average 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 magnetometer 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   reading 
                   . 
                 
               
             
           
         
       
     
     Convert jitter distance to jitter angle: 
     
       
         
           
             
               
                 θ 
                 jm 
               
               _ 
             
             = 
             
               
                 arcsin 
                 ⁡ 
                 
                   ( 
                   
                     
                       JmN 
                       _ 
                     
                     
                        
                       
                         m 
                         _ 
                       
                        
                     
                   
                   ) 
                 
               
               ≅ 
               
                 
                   
                     JmN 
                     _ 
                   
                   
                      
                     
                       m 
                       _ 
                     
                      
                   
                 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 for 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 small 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   
                     
                       θ 
                       jm 
                     
                     _ 
                   
                   . 
                 
               
             
           
         
       
     
     The total field sensor angular error is estimated by summing the maximum of the centripetal acceleration and linear acceleration angular errors with a multiple (e.g., 4) of the jitter angular error ( 242 ). The multiple is used to include a few standard deviations of jitter in the angular error measurement.
 
θ fe =max(θ c ,θ l )+α jitter   θ jm   
 
     Returning to  FIG. 1 , the IMU application  116 , prior to calculating the field quaternion, calculates a total angular orientation error by combining an orientation angular error due to centripetal acceleration, an orientation angular error due to linear acceleration, and (optionally) an orientation angular error due to estimated magnetometer jitter. Next, the IMU application  116  calculates an angular difference between the gyroscopic quaternion and the field quaternion, compares this angular difference to the total angular orientation error, and selectively mixes the gyroscopic quaternion and field quaternion in response to the comparison. 
     The IMU application  116  calculates the field quaternion by calculating a gravitational quaternion able to rotate an accelerometer reading so that a normalized Z-axis component of the accelerometer reading is equal to 1. The IMU application uses the gravitational quaternion to rotate a magnetometer reading, and sets the rotated magnetometer reading&#39;s Z-axis component to zero, creating an adjusted magnetometer reading. The IMU application calculates a magnetometer quaternion able to further rotate the adjusted magnetometer reading so that a normalized X-axis component is equal to −1, and then combines the gravitational quaternion and the magnetometer quaternion. 
     In  FIG. 2C , after completing the field angular error estimate ( 242  of  FIG. 2B ), processing exits the “Field Sensor Error Measurement” block ( 226 ) and proceeds to the “Field Quaternion Calculation” block ( 244 ) to calculate the field quaternion. The field quaternion is calculated from the average accelerometer and magnetometer readings. Calculating a quaternion from two vectors is known, and is described as follows. 
     A quaternion that rotates vector A to vector B is calculated using the following method: 
                     ⁢       H   =         A   +   B            A   +   B            =     half   ⁢           ⁢   vector   ⁢           ⁢     (     vector   ⁢           ⁢   halfway   ⁢           ⁢   between   ⁢           ⁢   A   ⁢           ⁢   and   ⁢           ⁢   B     )           ;                   R   =         H   ×   B            H   ×   B            =     normalized   ⁢           ⁢   cross   ⁢           ⁢   product   ⁢           ⁢   of   ⁢           ⁢   H   ⁢           ⁢   and   ⁢           ⁢   B   ⁢           ⁢     (     gives   ⁢           ⁢   rotation   ⁢           ⁢   axis     )           ;           r=H·B=dot  product of  H  and  B  (gives  r  value of  Q );   îx+ĵy+{circumflex over (k)}z=R √{square root over (1− r   2 )} (gives  x,y,z  value of  Q );
 
 Q=r+îx+ĵy+{circumflex over (k)}z;  
 
 A   B   Q =( H·B )+[ R √{square root over (1−( H·B ) 2 )}].
 
     However, prior art methods suffer from stability and erroneous results when the two vectors are relatively anti-parallel, and are particularly unstable when using integer math as opposed to floating point. Integer math is preferred over floating point to reduce computational processing power requirements. A stable method for calculating a quaternion is presented below. 
     Half vector is equal to or nearly equal to a null vector: 
     
       
         
           
             
               
                 A 
                 + 
                 
                   A 
                   ′ 
                 
               
               = 
               0 
             
             ; 
             
               
                  
                 
                   A 
                   + 
                   
                     A 
                     ′ 
                   
                 
                  
               
               = 
               0 
             
             ; 
             
               
                 
                   A 
                   + 
                   
                     A 
                     ′ 
                   
                 
                 
                    
                   
                     A 
                     + 
                     
                       A 
                       ′ 
                     
                   
                    
                 
               
               = 
               
                 undefined 
                 ⁢ 
                 
                     
                 
                 . 
               
             
           
         
       
     
     When two vectors spatially differ by more than 90°, then calculating the half vector between one of the vectors and the other vector&#39;s inverse produces a more accurate result: 
     If A·B&lt;0, then Use B′ to Calculate Quaternion. 
     Referring to  FIG. 8 , a quaternion that rotates vector A to vector B is calculated using the following method: 
     
       
         
           
             
               H 
               
                 AB 
                 ′ 
               
             
             = 
             
               
                 
                   A 
                   + 
                   
                     B 
                     ′ 
                   
                 
                 
                    
                   
                     A 
                     + 
                     
                       B 
                       ′ 
                     
                   
                    
                 
               
               = 
               
                 half 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 vector 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   
                     ( 
                     
                       vector 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       halfway 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       between 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       A 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       and 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         B 
                         ′ 
                       
                     
                     ) 
                   
                   . 
                 
               
             
           
         
       
     
     Quaternion element r is calculated using the following method: 
     
       
         
           
             
                 
             
             ⁢ 
             
               
                 
                   H 
                   
                     AB 
                     ′ 
                   
                 
                 · 
                 
                   B 
                   ′ 
                 
               
               = 
               
                 cos 
                 ⁡ 
                 
                   ( 
                   
                     
                       ( 
                       
                         π 
                         - 
                         θ 
                       
                       ) 
                     
                     2 
                   
                   ) 
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 
                   cos 
                   ⁡ 
                   
                     ( 
                     
                       a 
                       + 
                       b 
                     
                     ) 
                   
                 
                 = 
                 
                   
                     
                       cos 
                       ⁡ 
                       
                         ( 
                         a 
                         ) 
                       
                     
                     ⁢ 
                     
                       cos 
                       ⁡ 
                       
                         ( 
                         b 
                         ) 
                       
                     
                   
                   - 
                   
                     
                       sin 
                       ⁡ 
                       
                         ( 
                         a 
                         ) 
                       
                     
                     ⁢ 
                     
                       sin 
                       ⁡ 
                       
                         ( 
                         b 
                         ) 
                       
                     
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       ( 
                       
                         cosine 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         addition 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         law 
                       
                       ) 
                     
                   
                 
               
               ; 
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 cos 
                 ⁡ 
                 
                   ( 
                   
                     
                       ( 
                       
                         π 
                         - 
                         θ 
                       
                       ) 
                     
                     2 
                   
                   ) 
                 
               
               = 
               
                 
                   
                     
                       cos 
                       ⁡ 
                       
                         ( 
                         
                           π 
                           2 
                         
                         ) 
                       
                     
                     ⁢ 
                     
                       cos 
                       ⁡ 
                       
                         ( 
                         
                           - 
                           
                             θ 
                             2 
                           
                         
                         ) 
                       
                     
                   
                   - 
                   
                     
                       sin 
                       ⁡ 
                       
                         ( 
                         
                           π 
                           2 
                         
                         ) 
                       
                     
                     ⁢ 
                     
                       sin 
                       ⁡ 
                       
                         ( 
                         
                           - 
                           
                             θ 
                             2 
                           
                         
                         ) 
                       
                     
                   
                 
                 = 
                 
                   sin 
                   ⁡ 
                   
                     ( 
                     
                       θ 
                       2 
                     
                     ) 
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 
                   
                     
                       sin 
                       2 
                     
                     ⁡ 
                     
                       ( 
                       θ 
                       ) 
                     
                   
                   + 
                   
                     
                       cos 
                       2 
                     
                     ⁡ 
                     
                       ( 
                       θ 
                       ) 
                     
                   
                 
                 = 
                 
                   1 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ( 
                     
                       Pythagorean 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       Trigonometric 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       Identity 
                     
                     ) 
                   
                 
               
               ; 
             
           
         
       
       
         
           
             
               
                 cos 
                 ⁡ 
                 
                   ( 
                   
                     θ 
                     2 
                   
                   ) 
                 
               
               = 
               
                 
                   
                     1 
                     - 
                     
                       
                         sin 
                         2 
                       
                       ⁡ 
                       
                         ( 
                         
                           θ 
                           2 
                         
                         ) 
                       
                     
                   
                 
                 = 
                 
                   
                     
                       1 
                       - 
                       
                         
                           cos 
                           2 
                         
                         ⁡ 
                         
                           ( 
                           
                             
                               ( 
                               
                                 π 
                                 - 
                                 θ 
                               
                               ) 
                             
                             2 
                           
                           ) 
                         
                       
                     
                   
                   = 
                   
                     
                       1 
                       - 
                       
                         
                           ( 
                           
                             
                               H 
                               
                                 AB 
                                 ′ 
                               
                             
                             · 
                             
                               B 
                               ′ 
                             
                           
                           ) 
                         
                         2 
                       
                     
                   
                 
               
             
             ; 
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               r 
               = 
               
                 
                   cos 
                   ⁡ 
                   
                     ( 
                     
                       θ 
                       2 
                     
                     ) 
                   
                 
                 = 
                 
                   
                     
                       1 
                       - 
                       
                         
                           ( 
                           
                             
                               H 
                               
                                 AB 
                                 ′ 
                               
                             
                             · 
                             
                               B 
                               ′ 
                             
                           
                           ) 
                         
                         2 
                       
                     
                   
                   . 
                 
               
             
           
         
       
     
     Quaternion elements (x, y, z) are calculated using the following method: 
     
       
         
           
             
               R 
               = 
               
                 
                   
                     
                       H 
                       
                         AB 
                         ′ 
                       
                     
                     × 
                     B 
                   
                   
                      
                     
                       
                         H 
                         
                           AB 
                           ′ 
                         
                       
                       × 
                       B 
                     
                      
                   
                 
                 = 
                 
                   normalized 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   cross 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   product 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   of 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     H 
                     
                       AB 
                       ′ 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   and 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   B 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ( 
                     
                       gives 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       rotation 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       axis 
                     
                     ) 
                   
                 
               
             
             ; 
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 
                   sin 
                   ⁡ 
                   
                     ( 
                     
                       θ 
                       2 
                     
                     ) 
                   
                 
                 = 
                 
                   
                     
                       H 
                       
                         AB 
                         ′ 
                       
                     
                     · 
                     
                       B 
                       ′ 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ( 
                     
                       From 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       above 
                     
                     ) 
                   
                 
               
               ; 
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 
                   
                     
                       i 
                       ^ 
                     
                     ⁢ 
                     x 
                   
                   + 
                   
                     
                       j 
                       ^ 
                     
                     ⁢ 
                     y 
                   
                   + 
                   
                     
                       k 
                       ^ 
                     
                     ⁢ 
                     z 
                   
                 
                 = 
                 
                   
                     R 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       sin 
                       ⁡ 
                       
                         ( 
                         
                           θ 
                           2 
                         
                         ) 
                       
                     
                   
                   = 
                   
                     R 
                     ⁡ 
                     
                       ( 
                       
                         
                           H 
                           
                             AB 
                             ′ 
                           
                         
                         · 
                         
                           B 
                           ′ 
                         
                       
                       ) 
                     
                   
                 
               
               ; 
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 
                     
                   A 
                   B 
                 
                 ⁢ 
                 Q 
               
               = 
               
                 
                   
                     1 
                     - 
                     
                       
                         ( 
                         
                           
                             H 
                             
                               AB 
                               ′ 
                             
                           
                           · 
                           
                             B 
                             ′ 
                           
                         
                         ) 
                       
                       2 
                     
                   
                 
                 + 
                 
                   
                     [ 
                     
                       R 
                       ⁡ 
                       
                         ( 
                         
                           
                             H 
                             
                               AB 
                               ′ 
                             
                           
                           · 
                           
                             B 
                             ′ 
                           
                         
                         ) 
                       
                     
                     ] 
                   
                   . 
                 
               
             
           
         
       
     
     A field quaternion is calculated by using the average accelerometer reading to first calculate a gravitational quaternion, which rotates the IMU accelerometer reading to align with gravity as described below. 
     Calculate a quaternion to rotate measured acceleration vector to Earth&#39;s gravitational vector: 
     
       
         
           
             
               g 
               = 
               
                 
                   ( 
                   
                     0 
                     , 
                     0 
                     , 
                     1 
                   
                   ) 
                 
                 = 
                 
                   Earth 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   Z 
                   ⁢ 
                   
                     - 
                   
                   ⁢ 
                   axis 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   vector 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   aligns 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   with 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   gravity 
                 
               
             
             ; 
           
         
       
       
         
           
             a 
             = 
             
               
                 
                   a 
                   m 
                 
                 
                    
                   
                     a 
                     m 
                   
                    
                 
               
               = 
               
                 normalized 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 acceleration 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   
                     vector 
                     ⁢ 
                     
                       
 
                     
                     ( 
                     
                       
                         a 
                         m 
                       
                       = 
                       
                         measure 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         acceleration 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         vector 
                       
                     
                     ) 
                   
                   . 
                 
               
             
           
         
       
     
     If a z =1, then ∠ag is 0 degrees:
         a g Q=(1,0,0,0).       

     If a z =−1, then ∠ag is 180 degrees: 
     Set the quaternion to the normalized value of the previous gravitational quaternion with its r component set to zero; 
     
       
         
           
             
               Q 
               n 
                 
                 
               a 
               g 
             
             = 
             
               
                 
                   
                     Q 
                     
                       n 
                       - 
                       1 
                     
                       
                       
                     a 
                     g 
                   
                   ⁡ 
                   
                     ( 
                     
                       0 
                       , 
                       1 
                       , 
                       1 
                       , 
                       1 
                     
                     ) 
                   
                 
                 
                    
                   
                     
                       Q 
                       
                         n 
                         - 
                         1 
                       
                         
                         
                       a 
                       g 
                     
                     ⁡ 
                     
                       ( 
                       
                         0 
                         , 
                         1 
                         , 
                         1 
                         , 
                         1 
                       
                       ) 
                     
                   
                    
                 
               
               = 
               
                 
                   
                     ( 
                     
                       0 
                       , 
                       
                         x 
                         
                           n 
                           - 
                           1 
                         
                       
                       , 
                       
                         y 
                         
                           n 
                           - 
                           1 
                         
                       
                       , 
                       
                         z 
                         
                           n 
                           - 
                           1 
                         
                       
                     
                     ) 
                   
                   
                      
                     
                       ( 
                       
                         0 
                         , 
                         
                           x 
                           
                             n 
                             - 
                             1 
                           
                         
                         , 
                         
                           y 
                           
                             n 
                             - 
                             1 
                           
                         
                         , 
                         
                           z 
                           
                             n 
                             - 
                             1 
                           
                         
                       
                       ) 
                     
                      
                   
                 
                 . 
               
             
           
         
       
     
     If 0≦a z &lt;1, then ∠ag is 90 degrees or acute: 
     
       
         
           
             
               H 
               ag 
             
             = 
             
               
                 
                   a 
                   + 
                   g 
                 
                 
                    
                   
                     a 
                     + 
                     g 
                   
                    
                 
               
               = 
               
                 
                   
                     
                       
                         i 
                         ^ 
                       
                       ⁢ 
                       
                         a 
                         x 
                       
                     
                     + 
                     
                       
                         j 
                         ^ 
                       
                       ⁢ 
                       
                         a 
                         y 
                       
                     
                     + 
                     
                       
                         k 
                         ^ 
                       
                       ⁡ 
                       
                         ( 
                         
                           
                             a 
                             z 
                           
                           + 
                           1 
                         
                         ) 
                       
                     
                   
                   
                      
                     
                       
                         
                           i 
                           ^ 
                         
                         ⁢ 
                         
                           a 
                           x 
                         
                       
                       + 
                       
                         
                           j 
                           ^ 
                         
                         ⁢ 
                         
                           a 
                           y 
                         
                       
                       + 
                       
                         
                           k 
                           ^ 
                         
                         ⁡ 
                         
                           ( 
                           
                             
                               a 
                               z 
                             
                             + 
                             1 
                           
                           ) 
                         
                       
                     
                      
                   
                 
                 = 
                 
                   
                     
                       
                         i 
                         ^ 
                       
                       ⁢ 
                       
                         a 
                         x 
                       
                     
                     + 
                     
                       
                         j 
                         ^ 
                       
                       ⁢ 
                       
                         a 
                         y 
                       
                     
                     + 
                     
                       
                         k 
                         ^ 
                       
                       ⁡ 
                       
                         ( 
                         
                           
                             a 
                             z 
                           
                           + 
                           1 
                         
                         ) 
                       
                     
                   
                   
                     
                       
                         a 
                         x 
                         2 
                       
                       + 
                       
                         a 
                         y 
                         2 
                       
                       + 
                       
                         
                           ( 
                           
                             
                               a 
                               z 
                             
                             + 
                             1 
                           
                           ) 
                         
                         2 
                       
                     
                   
                 
               
             
           
         
       
       
         
           
             
               R 
               = 
               
                 
                   
                     
                       H 
                       ag 
                     
                     × 
                     g 
                   
                   
                      
                     
                       
                         H 
                         ag 
                       
                       × 
                       g 
                     
                      
                   
                 
                 = 
                 
                   
                     
                       
                         i 
                         ^ 
                       
                       ⁢ 
                       
                         a 
                         y 
                       
                     
                     - 
                     
                       
                         j 
                         ^ 
                       
                       ⁢ 
                       
                         a 
                         x 
                       
                     
                   
                   
                     
                       
                         a 
                         y 
                         2 
                       
                       + 
                       
                         a 
                         x 
                         2 
                       
                     
                   
                 
               
             
             ; 
           
         
       
       
         
           
             
               
                 H 
                 ag 
               
               · 
               g 
             
             = 
             
               
                 
                   a 
                   z 
                 
                 + 
                 1 
               
               
                 
                   
                     a 
                     x 
                     2 
                   
                   + 
                   
                     a 
                     y 
                     2 
                   
                   + 
                   
                     
                       ( 
                       
                         
                           a 
                           z 
                         
                         + 
                         1 
                       
                       ) 
                     
                     2 
                   
                 
               
             
           
         
       
       
         
           
             
               
                   
                 a 
                 g 
               
               ⁢ 
               Q 
             
             = 
             
               
                 
                   ( 
                   
                     
                       H 
                       ag 
                     
                     · 
                     g 
                   
                   ) 
                 
                 + 
                 
                   [ 
                   
                     R 
                     ⁢ 
                     
                       
                         1 
                         - 
                         
                           
                             ( 
                             
                               
                                 H 
                                 ag 
                               
                               · 
                               g 
                             
                             ) 
                           
                           2 
                         
                       
                     
                   
                   ] 
                 
               
               = 
               
                 
                   
                     ( 
                     
                       
                         a 
                         z 
                       
                       + 
                       1 
                     
                     ) 
                   
                   + 
                   
                     
                       i 
                       ^ 
                     
                     ⁡ 
                     
                       ( 
                       
                         a 
                         y 
                       
                       ) 
                     
                   
                   + 
                   
                     
                       j 
                       ^ 
                     
                     ⁡ 
                     
                       ( 
                       
                         - 
                         
                           a 
                           x 
                         
                       
                       ) 
                     
                   
                 
                 
                   
                     
                       a 
                       x 
                       2 
                     
                     + 
                     
                       a 
                       y 
                       2 
                     
                     + 
                     
                       
                         ( 
                         
                           
                             a 
                             z 
                           
                           + 
                           1 
                         
                         ) 
                       
                       2 
                     
                   
                 
               
             
           
         
       
     
     If −1&lt;a z &lt;0, then ∠ag is obtuse: 
     
       
         
           
             
               H 
               
                 ag 
                 ′ 
               
             
             = 
             
               
                 
                   a 
                   + 
                   
                     g 
                     ′ 
                   
                 
                 
                    
                   
                     a 
                     + 
                     
                       g 
                       ′ 
                     
                   
                    
                 
               
               = 
               
                 
                   
                     
                       
                         i 
                         ^ 
                       
                       ⁢ 
                       
                         a 
                         x 
                       
                     
                     + 
                     
                       
                         j 
                         ^ 
                       
                       ⁢ 
                       
                         a 
                         y 
                       
                     
                     + 
                     
                       
                         k 
                         ^ 
                       
                       ⁡ 
                       
                         ( 
                         
                           
                             a 
                             z 
                           
                           - 
                           1 
                         
                         ) 
                       
                     
                   
                   
                      
                     
                       
                         
                           i 
                           ^ 
                         
                         ⁢ 
                         
                           a 
                           x 
                         
                       
                       + 
                       
                         
                           j 
                           ^ 
                         
                         ⁢ 
                         
                           a 
                           y 
                         
                       
                       + 
                       
                         
                           k 
                           ^ 
                         
                         ⁡ 
                         
                           ( 
                           
                             
                               a 
                               z 
                             
                             - 
                             1 
                           
                           ) 
                         
                       
                     
                      
                   
                 
                 = 
                 
                   
                     
                       
                         i 
                         ^ 
                       
                       ⁢ 
                       
                         a 
                         x 
                       
                     
                     + 
                     
                       
                         j 
                         ^ 
                       
                       ⁢ 
                       
                         a 
                         y 
                       
                     
                     + 
                     
                       
                         k 
                         ^ 
                       
                       ⁡ 
                       
                         ( 
                         
                           
                             a 
                             z 
                           
                           - 
                           1 
                         
                         ) 
                       
                     
                   
                   
                     
                       
                         a 
                         x 
                         2 
                       
                       + 
                       
                         a 
                         y 
                         2 
                       
                       + 
                       
                         
                           ( 
                           
                             
                               a 
                               z 
                             
                             - 
                             1 
                           
                           ) 
                         
                         2 
                       
                     
                   
                 
               
             
           
         
       
       
         
           
             r 
             = 
             
               
                 
                   1 
                   - 
                   
                     
                       ( 
                       
                         
                           H 
                           
                             ag 
                             ′ 
                           
                         
                         · 
                         
                           g 
                           ′ 
                         
                       
                       ) 
                     
                     2 
                   
                 
               
               = 
               
                 
                   
                     1 
                     - 
                     
                       
                         
                           ( 
                           
                             - 
                             
                               ( 
                               
                                 
                                   a 
                                   z 
                                 
                                 - 
                                 1 
                               
                               ) 
                             
                           
                           ) 
                         
                         2 
                       
                       
                         
                           a 
                           x 
                           2 
                         
                         + 
                         
                           a 
                           y 
                           2 
                         
                         + 
                         
                           
                             ( 
                             
                               
                                 a 
                                 z 
                               
                               - 
                               1 
                             
                             ) 
                           
                           2 
                         
                       
                     
                   
                 
                 = 
                 
                   
                     1 
                     - 
                     
                       
                         
                           ( 
                           
                             
                               a 
                               z 
                             
                             - 
                             1 
                           
                           ) 
                         
                         2 
                       
                       
                         
                           a 
                           x 
                           2 
                         
                         + 
                         
                           a 
                           y 
                           2 
                         
                         + 
                         
                           
                             ( 
                             
                               
                                 a 
                                 z 
                               
                               - 
                               1 
                             
                             ) 
                           
                           2 
                         
                       
                     
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               R 
               = 
               
                 
                   
                     
                       H 
                       
                         ag 
                         ′ 
                       
                     
                     × 
                     g 
                   
                   
                      
                     
                       
                         H 
                         
                           ag 
                           ′ 
                         
                       
                       × 
                       g 
                     
                      
                   
                 
                 = 
                 
                   
                     
                       
                         i 
                         ^ 
                       
                       ⁢ 
                       
                         a 
                         y 
                       
                     
                     - 
                     
                       
                         j 
                         ^ 
                       
                       ⁢ 
                       
                         a 
                         x 
                       
                     
                   
                   
                     
                       
                         a 
                         y 
                         2 
                       
                       + 
                       
                         a 
                         x 
                         2 
                       
                     
                   
                 
               
             
           
         
       
       
         
           
             
               
                 
                   i 
                   ^ 
                 
                 ⁢ 
                 x 
               
               + 
               
                 
                   j 
                   ^ 
                 
                 ⁢ 
                 y 
               
               + 
               
                 
                   k 
                   ^ 
                 
                 ⁢ 
                 z 
               
             
             = 
             
               
                 R 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   sin 
                   ⁡ 
                   
                     ( 
                     
                       θ 
                       2 
                     
                     ) 
                   
                 
               
               = 
               
                 
                   R 
                   ⁡ 
                   
                     ( 
                     
                       
                         H 
                         
                           ag 
                           ′ 
                         
                       
                       · 
                       
                         g 
                         ′ 
                       
                     
                     ) 
                   
                 
                 = 
                 
                   
                     
                       
                         
                           i 
                           ^ 
                         
                         ⁢ 
                         
                           a 
                           y 
                         
                       
                       - 
                       
                         
                           j 
                           ^ 
                         
                         ⁢ 
                         
                           a 
                           x 
                         
                       
                     
                     
                       
                         
                           a 
                           y 
                           2 
                         
                         + 
                         
                           a 
                           x 
                           2 
                         
                       
                     
                   
                   ⁢ 
                   
                     
                       
                         
                           ( 
                           
                             
                               a 
                               z 
                             
                             - 
                             1 
                           
                           ) 
                         
                         2 
                       
                       
                         
                           a 
                           x 
                           2 
                         
                         + 
                         
                           a 
                           y 
                           2 
                         
                         + 
                         
                           
                             ( 
                             
                               
                                 a 
                                 z 
                               
                               - 
                               1 
                             
                             ) 
                           
                           2 
                         
                       
                     
                   
                 
               
             
           
         
       
       
         
           
             
               
                   
                 a 
                 g 
               
               ⁢ 
               Q 
             
             = 
             
               
                 
                   
                     1 
                     - 
                     
                       
                         ( 
                         
                           
                             H 
                             ag 
                             ′ 
                           
                           · 
                           
                             g 
                             ′ 
                           
                         
                         ) 
                       
                       2 
                     
                   
                 
                 + 
                 
                   [ 
                   
                     R 
                     ⁡ 
                     
                       ( 
                       
                         
                           H 
                           ag 
                         
                         · 
                         g 
                       
                       ) 
                     
                   
                   ] 
                 
               
               = 
               
                 
                   
                     1 
                     - 
                     
                       
                         
                           ( 
                           
                             
                               a 
                               z 
                             
                             - 
                             1 
                           
                           ) 
                         
                         2 
                       
                       
                         
                           a 
                           x 
                           2 
                         
                         + 
                         
                           a 
                           y 
                           2 
                         
                         + 
                         
                           
                             ( 
                             
                               
                                 a 
                                 z 
                               
                               - 
                               1 
                             
                             ) 
                           
                           2 
                         
                       
                     
                   
                 
                 + 
                 
                   
                     
                       
                         
                           i 
                           ^ 
                         
                         ⁢ 
                         
                           a 
                           y 
                         
                       
                       - 
                       
                         
                           j 
                           ^ 
                         
                         ⁢ 
                         
                           a 
                           x 
                         
                       
                     
                     
                       
                         
                           a 
                           y 
                           2 
                         
                         + 
                         
                           a 
                           x 
                           2 
                         
                       
                     
                   
                   ⁢ 
                   
                     
                       
                         
                           ( 
                           
                             
                               a 
                               z 
                             
                             - 
                             1 
                           
                           ) 
                         
                         2 
                       
                       
                         
                           a 
                           x 
                           2 
                         
                         + 
                         
                           a 
                           y 
                           2 
                         
                         + 
                         
                           
                             ( 
                             
                               
                                 a 
                                 z 
                               
                               - 
                               1 
                             
                             ) 
                           
                           2 
                         
                       
                     
                   
                 
               
             
           
         
       
     
     The gravitational quaternion is then used to rotate the average magnetometer reading so that it aligns with gravity. The rotated average magnetometer reading is then projected onto the XY-plane that parallels Earth&#39;s surface and that projected reading is used to calculate a magnetic quaternion aligned purely with the Z-axis, as described below. 
     Rotate the measured magnetometer reading using gravitational quaternion:
 
 m   g = a   g   Qm   mg   a   Q.  
 
     Calculate the normalized magnetic field vector perpendicular to the (e.g., Earth&#39;s) gravitational field: 
     
       
         
           
             m 
             = 
             
               
                 
                   
                     i 
                     ^ 
                   
                   ⁢ 
                   
                     m 
                     x 
                   
                 
                 + 
                 
                   
                     j 
                     ^ 
                   
                   ⁢ 
                   
                     m 
                     y 
                   
                 
               
               = 
               
                 
                   
                     
                       
                         i 
                         ^ 
                       
                       ⁢ 
                       
                         m 
                         gx 
                       
                     
                     + 
                     
                       
                         j 
                         ^ 
                       
                       ⁢ 
                       
                         m 
                         gy 
                       
                     
                   
                   
                     
                       
                         m 
                         gx 
                         2 
                       
                       + 
                       
                         m 
                         gy 
                         2 
                       
                     
                   
                 
                 . 
               
             
           
         
       
     
     Calculate Z-axis quaternion to rotate magnetic field vector to the magnetic pole (e.g., North): 
     n=(−1, 0, 0)=Earth negative X-axis vector aligns with North; 
     If m x =−1, then ∠mn is 0 degrees:
 
 m   n   Q =(1,0,0,0).
 
If m x =1, then ∠mn is 180 degrees:
 
Set the quaternion to the normalized value of the previous magnetic quaternion with its r component set to zero;
 
               Q   n           m   n     =           Q     n   -   1             m   n     ⁡     (     0   ,   1   ,   1   ,   1     )                Q     n   -   1             m   n     ⁡     (     0   ,   1   ,   1   ,   1     )              =         (     0   ,     x     n   -   1       ,     y     n   -   1       ,     z     n   -   1         )            (     0   ,     x     n   -   1       ,     y     n   -   1       ,     z     n   -   1         )            .             
If −1&lt;m x ≦0, then ∠mn is 90 degrees or acute:
 
     
       
         
           
             
               H 
               mn 
             
             = 
             
               
                 
                   m 
                   + 
                   n 
                 
                 
                    
                   
                     m 
                     + 
                     n 
                   
                    
                 
               
               = 
               
                 
                   
                     
                       
                         i 
                         ^ 
                       
                       ⁡ 
                       
                         ( 
                         
                           
                             m 
                             x 
                           
                           - 
                           1 
                         
                         ) 
                       
                     
                     + 
                     
                       
                         j 
                         ^ 
                       
                       ⁢ 
                       
                         m 
                         y 
                       
                     
                   
                   
                      
                     
                       
                         
                           i 
                           ^ 
                         
                         ⁡ 
                         
                           ( 
                           
                             
                               m 
                               x 
                             
                             - 
                             1 
                           
                           ) 
                         
                       
                       + 
                       
                         
                           j 
                           ^ 
                         
                         ⁢ 
                         
                           m 
                           y 
                         
                       
                     
                      
                   
                 
                 = 
                 
                   
                     
                       
                         i 
                         ^ 
                       
                       ⁡ 
                       
                         ( 
                         
                           
                             m 
                             x 
                           
                           - 
                           1 
                         
                         ) 
                       
                     
                     + 
                     
                       
                         j 
                         ^ 
                       
                       ⁢ 
                       
                         m 
                         y 
                       
                     
                   
                   
                     
                       
                         
                           ( 
                           
                             
                               m 
                               x 
                             
                             - 
                             1 
                           
                           ) 
                         
                         2 
                       
                       + 
                       
                         m 
                         y 
                         2 
                       
                     
                   
                 
               
             
           
         
       
       
         
           
             R 
             = 
             
               
                 
                   
                     H 
                     mn 
                   
                   × 
                   n 
                 
                 
                    
                   
                     
                       H 
                       mn 
                     
                     × 
                     n 
                   
                    
                 
               
               = 
               
                 
                   
                     k 
                     ^ 
                   
                   ⁢ 
                   
                     m 
                     y 
                   
                 
                 
                   
                     m 
                     y 
                     2 
                   
                 
               
             
           
         
       
       
         
           
             
               
                 H 
                 mn 
               
               · 
               n 
             
             = 
             
               
                 1 
                 - 
                 
                   m 
                   x 
                 
               
               
                 
                   
                     
                       ( 
                       
                         
                           m 
                           x 
                         
                         - 
                         1 
                       
                       ) 
                     
                     2 
                   
                   + 
                   
                     m 
                     y 
                     2 
                   
                 
               
             
           
         
       
       
         
           
             
               
                   
                 m 
                 n 
               
               ⁢ 
               Q 
             
             = 
             
               
                 
                   ( 
                   
                     
                       H 
                       mn 
                     
                     · 
                     n 
                   
                   ) 
                 
                 + 
                 
                   [ 
                   
                     R 
                     ⁢ 
                     
                       
                         1 
                         - 
                         
                           
                             ( 
                             
                               
                                 H 
                                 mn 
                               
                               · 
                               n 
                             
                             ) 
                           
                           2 
                         
                       
                     
                   
                   ] 
                 
               
               = 
               
                 
                   
                     ( 
                     
                       1 
                       - 
                       
                         m 
                         x 
                       
                     
                     ) 
                   
                   + 
                   
                     
                       k 
                       ^ 
                     
                     ⁡ 
                     
                       ( 
                       
                         m 
                         y 
                       
                       ) 
                     
                   
                 
                 
                   
                     
                       
                         ( 
                         
                           
                             m 
                             x 
                           
                           - 
                           1 
                         
                         ) 
                       
                       2 
                     
                     + 
                     
                       m 
                       y 
                       2 
                     
                   
                 
               
             
           
         
       
     
     If 0&lt;m x &lt;1, then ∠mn is obtuse: 
     
       
         
           
             
                 
             
             ⁢ 
             
               
                 H 
                 
                   mn 
                   ′ 
                 
               
               = 
               
                 
                   
                     m 
                     + 
                     
                       n 
                       ′ 
                     
                   
                   
                      
                     
                       m 
                       + 
                       
                         n 
                         ′ 
                       
                     
                      
                   
                 
                 = 
                 
                   
                     
                       
                         
                           i 
                           ^ 
                         
                         ⁡ 
                         
                           ( 
                           
                             
                               m 
                               x 
                             
                             + 
                             1 
                           
                           ) 
                         
                       
                       + 
                       
                         
                           j 
                           ^ 
                         
                         ⁢ 
                         
                           m 
                           y 
                         
                       
                     
                     
                        
                       
                         
                           
                             i 
                             ^ 
                           
                           ⁡ 
                           
                             ( 
                             
                               
                                 m 
                                 x 
                               
                               + 
                               1 
                             
                             ) 
                           
                         
                         + 
                         
                           
                             j 
                             ^ 
                           
                           ⁢ 
                           
                             m 
                             y 
                           
                         
                       
                        
                     
                   
                   = 
                   
                     
                       
                         
                           i 
                           ^ 
                         
                         ⁡ 
                         
                           ( 
                           
                             
                               m 
                               x 
                             
                             + 
                             1 
                           
                           ) 
                         
                       
                       + 
                       
                         
                           j 
                           ^ 
                         
                         ⁢ 
                         
                           m 
                           y 
                         
                       
                     
                     
                       
                         
                           
                             ( 
                             
                               
                                 m 
                                 x 
                               
                               + 
                               1 
                             
                             ) 
                           
                           2 
                         
                         + 
                         
                           m 
                           y 
                           2 
                         
                       
                     
                   
                 
               
             
           
         
       
       
         
           
             r 
             = 
             
               
                 
                   1 
                   - 
                   
                     
                       ( 
                       
                         
                           H 
                           
                             mn 
                             ′ 
                           
                         
                         · 
                         
                           n 
                           ′ 
                         
                       
                       ) 
                     
                     2 
                   
                 
               
               = 
               
                 
                   
                     1 
                     - 
                     
                       
                         
                           ( 
                           
                             
                               m 
                               x 
                             
                             + 
                             1 
                           
                           ) 
                         
                         2 
                       
                       
                         
                           
                             ( 
                             
                               
                                 m 
                                 x 
                               
                               + 
                               1 
                             
                             ) 
                           
                           2 
                         
                         + 
                         
                           m 
                           y 
                           2 
                         
                       
                     
                   
                 
                 = 
                 
                   
                      
                     
                       m 
                       y 
                     
                      
                   
                   
                     
                       
                         
                           ( 
                           
                             
                               m 
                               x 
                             
                             + 
                             1 
                           
                           ) 
                         
                         2 
                       
                       + 
                       
                         m 
                         y 
                         2 
                       
                     
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               R 
               = 
               
                 
                   
                     
                       H 
                       
                         mn 
                         ′ 
                       
                     
                     × 
                     n 
                   
                   
                      
                     
                       
                         H 
                         
                           mn 
                           ′ 
                         
                       
                       × 
                       n 
                     
                      
                   
                 
                 = 
                 
                   
                     
                       
                         k 
                         ^ 
                       
                       ⁢ 
                       
                         m 
                         y 
                       
                     
                     
                       
                         m 
                         y 
                         2 
                       
                     
                   
                   = 
                   
                     
                       k 
                       ^ 
                     
                     ⁡ 
                     
                       ( 
                       
                         
                           
                             
                               ( 
                               
                                 
                                   m 
                                   y 
                                 
                                 &gt; 
                                 0 
                               
                               ) 
                             
                             ? 
                             1 
                           
                           ⁢ 
                           
                             : 
                           
                         
                         ⁢ 
                         
                             
                         
                         - 
                         1 
                       
                       ) 
                     
                   
                 
               
             
           
         
       
       
         
           
             
               
                 
                   i 
                   ^ 
                 
                 ⁢ 
                 x 
               
               + 
               
                 
                   j 
                   ^ 
                 
                 ⁢ 
                 y 
               
               + 
               
                 
                   k 
                   ^ 
                 
                 ⁢ 
                 z 
               
             
             = 
             
               
                 R 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   sin 
                   ⁡ 
                   
                     ( 
                     
                       θ 
                       2 
                     
                     ) 
                   
                 
               
               = 
               
                 
                   R 
                   ⁡ 
                   
                     ( 
                     
                       
                         H 
                         
                           mn 
                           ′ 
                         
                       
                       · 
                       
                         n 
                         ′ 
                       
                     
                     ) 
                   
                 
                 = 
                 
                   
                     
                       
                         k 
                         ^ 
                       
                       ⁡ 
                       
                         ( 
                         
                           
                             
                               
                                 ( 
                                 
                                   
                                     m 
                                     y 
                                   
                                   &gt; 
                                   0 
                                 
                                 ) 
                               
                               ? 
                               1 
                             
                             ⁢ 
                             
                               : 
                             
                           
                           ⁢ 
                           
                               
                           
                           - 
                           1 
                         
                         ) 
                       
                     
                     ⁢ 
                     
                       ( 
                       
                         
                           m 
                           x 
                         
                         + 
                         1 
                       
                       ) 
                     
                   
                   
                     
                       
                         
                           ( 
                           
                             
                               m 
                               x 
                             
                             + 
                             1 
                           
                           ) 
                         
                         2 
                       
                       + 
                       
                         m 
                         y 
                         2 
                       
                     
                   
                 
               
             
           
         
       
       
         
           
             
               
                   
                 m 
                 n 
               
               ⁢ 
               Q 
             
             = 
             
               
                 
                   
                     1 
                     - 
                     
                       
                         ( 
                         
                           
                             H 
                             
                               mn 
                               ′ 
                             
                           
                           · 
                           
                             n 
                             ′ 
                           
                         
                         ) 
                       
                       2 
                     
                   
                 
                 + 
                 
                   [ 
                   
                     R 
                     ⁡ 
                     
                       ( 
                       
                         
                           H 
                           
                             mn 
                             ′ 
                           
                         
                         · 
                         
                           n 
                           ′ 
                         
                       
                       ) 
                     
                   
                   ] 
                 
               
               = 
               
                 
                   
                      
                     
                       m 
                       y 
                     
                      
                   
                   + 
                   
                     
                       
                         k 
                         ^ 
                       
                       ⁡ 
                       
                         ( 
                         
                           
                             
                               
                                 ( 
                                 
                                   
                                     m 
                                     y 
                                   
                                   &gt; 
                                   0 
                                 
                                 ) 
                               
                               ? 
                               1 
                             
                             ⁢ 
                             
                               : 
                             
                           
                           ⁢ 
                           
                               
                           
                           - 
                           1 
                         
                         ) 
                       
                     
                     ⁢ 
                     
                       ( 
                       
                         
                           m 
                           x 
                         
                         + 
                         1 
                       
                       ) 
                     
                   
                 
                 
                   
                     
                       
                         ( 
                         
                           
                             m 
                             x 
                           
                           + 
                           1 
                         
                         ) 
                       
                       2 
                     
                     + 
                     
                       m 
                       y 
                       2 
                     
                   
                 
               
             
           
         
       
     
     The gravitational and magnetic quaternions are combined to calculate the complete field quaternion that equals the rotation of the sensor relative to Earth&#39;s coordinate system.
 
 E   S   Q   f =( m   n   Q   a   g   Q   m   n   Q ′) m   n   Q=   m   n   Q   a   g   Q  
 
     For any rotation, there exist two possible quaternions that can represent the rotation. They differ by the sign of each element in the quaternion vector. To produce the smoothest possible mathematical transition between time sequential quaternions, the distance between the two possible field quaternions and the previous sample quaternion is calculated, and the field quaternion closest to the previous sample quaternion is selected as the current field quaternion. The two distances are calculated by summing together the absolute values of the difference between each vector element of the current field quaternion candidate and the previous sample quaternion.
 
distance=Σ rxyz   |Q   f,i   −Q   p,i |
 
     Returning to  FIG. 1 , the IMU application  116  may calculate the field quaternion by combining a plurality of accelerometer readings to create an average accelerometer reading, and using the average accelerometer reading to calculate an accelerometer quaternion. In another aspect, the IMU application  116  calculates the field quaternion by combining a plurality of magnetometer readings to create an average magnetometer reading, and using the average magnetometer reading to calculate a magnetometer quaternion. For example, the IMU application  116  may select a number of readings in the plurality of magnetometers readings in response to a distance between a current reading and past readings, and the estimated magnetometer jitter. 
     Returning to  FIG. 2C , after completing the field quaternion calculation, processing exits the “Field Quaternion Calculation” block ( 244 ) and proceeds to the “Gyroscopic &amp; Field Quaternion Mixing” block ( 246 ) to calculate the current sample quaternion for the current input readings. The two quaternions are combined using a weighted average as shown below. 
     Use the weighted average to calculate final quaternion:
 
 Q   f = E   S   Q   f =field quaternion;
 
 Q   ω = E   S   Q   ω =gyroscopic quaternion;
 
 w   f =weight of  Q   f ;
 
 w   ω =weight of  Q   ω ;
 
 w   f   +w   ω =1;
 
     
       
         
           
             
               Q 
               
                 f 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 ω 
               
             
             = 
             
               
                 
                   
                     w 
                     f 
                   
                   ⁢ 
                   
                     Q 
                     f 
                   
                 
                 + 
                 
                   
                     w 
                     ω 
                   
                   ⁢ 
                   
                     Q 
                     ω 
                   
                 
               
               
                  
                 
                   
                     
                       w 
                       f 
                     
                     ⁢ 
                     
                       Q 
                       f 
                     
                   
                   + 
                   
                     
                       w 
                       ω 
                     
                     ⁢ 
                     
                       Q 
                       ω 
                     
                   
                 
                  
               
             
           
         
       
     
     The field quaternion weighting is based upon the estimated accuracy of the field measurements (θ fe ), calculated earlier, and the angular distance between the field and gyroscopic quaternions ( 248 ). The angular distance between the two quaternions is calculated from the r-element of the quaternion that would rotate one quaternion onto the other. 
     
       
         
           
             
                 
             
             ⁢ 
             
               
                 r 
                 
                   f 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   ω 
                 
               
               = 
               
                 
                   Real 
                   ⁡ 
                   
                     ( 
                     
                       
                           
                         f 
                         ω 
                       
                       ⁢ 
                       Q 
                     
                     ) 
                   
                 
                 = 
                 
                   
                     Real 
                     ⁡ 
                     
                       ( 
                       
                         
                           Q 
                           ω 
                         
                         ⁢ 
                         
                           Q 
                           f 
                           ′ 
                         
                       
                       ) 
                     
                   
                   = 
                   
                     
                       
                         r 
                         ω 
                       
                       ⁢ 
                       
                         r 
                         f 
                       
                     
                     + 
                     
                       
                         x 
                         ω 
                       
                       ⁢ 
                       
                         x 
                         f 
                       
                     
                     + 
                     
                       
                         y 
                         ω 
                       
                       ⁢ 
                       
                         y 
                         f 
                       
                     
                     + 
                     
                       
                         z 
                         ω 
                       
                       ⁢ 
                       
                         z 
                         f 
                       
                     
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 r 
                 
                   f 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   ω 
                 
               
               = 
               
                 cos 
                 ⁡ 
                 
                   ( 
                   
                     
                       θ 
                       
                         f 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         ω 
                       
                     
                     2 
                   
                   ) 
                 
               
             
           
         
       
       
         
           
             
               θ 
               
                 f 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 ω 
               
             
             = 
             
               
                 2 
                 · 
                 
                   arcsin 
                   ⁡ 
                   
                     ( 
                     
                       sin 
                       ⁡ 
                       
                         ( 
                         
                           
                             θ 
                             
                               f 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               ω 
                             
                           
                           2 
                         
                         ) 
                       
                     
                     ) 
                   
                 
               
               = 
               
                 
                   2 
                   · 
                   
                     arcsin 
                     ⁡ 
                     
                       ( 
                       
                         
                           1 
                           - 
                           
                             
                               cos 
                               2 
                             
                             ⁡ 
                             
                               ( 
                               
                                 
                                   θ 
                                   
                                     f 
                                     ⁢ 
                                     
                                         
                                     
                                     ⁢ 
                                     ω 
                                   
                                 
                                 2 
                               
                               ) 
                             
                           
                         
                       
                       ) 
                     
                   
                 
                 = 
                 
                   
                     2 
                     · 
                     
                       arcsin 
                       ⁡ 
                       
                         ( 
                         
                           
                             1 
                             - 
                             
                               r 
                               
                                 f 
                                 ⁢ 
                                 
                                     
                                 
                                 ⁢ 
                                 ω 
                               
                               2 
                             
                           
                         
                         ) 
                       
                     
                   
                   ≅ 
                   
                     2 
                     ⁢ 
                     
                       
                         1 
                         - 
                         
                           r 
                           
                             f 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             ω 
                           
                           2 
                         
                       
                     
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     for 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       θ 
                       
                         f 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         ω 
                       
                     
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     small 
                   
                 
               
             
           
         
       
     
     If the angular distance is less than or equal to the field angular error measurement ( 250 ), then the gyroscopic quaternion is used as the current sample quaternion ( 251 ), because the field quaternion has relatively too much error to be useful. 
     If (θ fω ≦ƒ fe ), then Use Gyroscopic Quaternion 
     If the angular distance is larger than the field angular error measurement, then the gyroscopic and field quaternions are mixed to obtain the current sample quaternion using the following equations to calculate the relative quaternion weights, which favor the field quaternion as its error becomes small relative to the angular distance ( 252 ). 
     
       
         
           
             
               w 
               ω 
             
             = 
             
               
                 θ 
                 fe 
               
               
                 θ 
                 
                   f 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   ω 
                 
               
             
           
         
       
       
         
           
             
               w 
               f 
             
             = 
             
               1 
               - 
               
                 w 
                 ω 
               
             
           
         
       
     
     Returning to  FIG. 1 , in one aspect the IMU application  116  low-pass filters four vector elements of the current sample quaternion to provide an output quaternion. Alternatively, the IMU application  116  compares a sum of angular commonality between each current sample quaternion in a group of current sample quaternions, with other current sample quaternions from the group, and selects the current sample quaternion associated with the largest sum as an output quaternion. 
     Returning to  FIG. 2C , after completing the field quaternion calculation, processing exits the “Gyroscopic &amp; Field Quaternion Mixing” block ( 246 ) and proceeds to the “Output Quaternion Filter” block ( 230 ) to reduce the quaternion sample rate to a rate required by the application. Using a higher quaternion sample rate than the system requires allows the algorithm to track the IMU orientation with more accuracy not limited to the application data rate. The IMU sample rate may be determined by the rate the IMU changes orientation rather than the rate that the application consumes the data. 
     Many types of filters can be used to perform the down conversion from the sample rate to the output rate. For example, the sample quaternions can be low-pass filtered by filtering all four vector elements in four separate paths of a four input filter. Alternatively, a median filter can be used. The median value of a set of quaternions is calculated by summing the angular commonality between each candidate quaternion and all other candidate quaternions, and selecting the quaternion with the largest total angular commonality. 
     
       
         
           
             
               total_angular 
               ⁢ 
               _commonality 
               ⁢ 
               
                   
               
               ⁢ 
               
                 ( 
                 
                   Q 
                   j 
                 
                 ) 
               
             
             = 
             
               
                 ∑ 
                 
                   i 
                   ≠ 
                   j 
                 
                 N 
               
               ⁢ 
               
                   
               
               ⁢ 
               
                 Real 
                 ⁡ 
                 
                   ( 
                   
                     
                         
                       i 
                       j 
                     
                     ⁢ 
                     Q 
                   
                   ) 
                 
               
             
           
         
       
     
     Other filters may also be used for the output quaternion filter. 
     As noted above, the IMU and associated algorithm need not necessarily use a magnetometer. In this case, only the Z-axis orientation is relative to the reference field (e.g., Earth). The X and Y axis orientation are not relative to Earth, and may drift due to gyroscope sensor drift. 
     Summarizing  FIGS. 1 and 2A-2C , it is noted that estimating can be performed when the IMU is relatively still, for the purpose of updating the magnetometer jitter and the accelerometer bias offset estimates. The gyroscope bias estimate can be updated when the gyroscope readings are relatively small to reduce gyroscope drift. The angular error between accelerometer readings and Earth&#39;s gravitational field due to sensor inertial motion can also be estimated. Estimations are possible of the angular error between magnetometer readings and Earth&#39;s magnetic field due to jitter in magnetometer readings. Further, the combined angular error of the accelerometer and magnetometer readings can be estimated. Advantageously, a stable quaternion can be calculated from two vectors even when the two vectors approach an anti-parallel orientation. It is possible to calculate a stable field quaternion from acceleration and gyroscopic sensors. Gyroscopic and field quaternions are combined using a weighting based upon estimated field quaternion angular error and the angular difference between the gyroscopic and field quaternions, and the quaternion output can be decimated using a quaternion median filter. 
       FIGS. 3A and 3B  are a flowchart illustrating a summarized method for determining the orientation of an inertial measurement unit. Although the method is depicted as a sequence of numbered steps for clarity, the numbering does not necessarily dictate the order of the steps. It should be understood that some of these steps may be skipped, performed in parallel, or performed without the requirement of maintaining a strict order of sequence. Generally however, the method follows the numeric order of the depicted steps. The method starts at Step  300 . 
     Step  306  calculates a gyroscopic quaternion. Step  308  estimates angular orientation errors due to IMU angular velocity and linear acceleration. When an IMU accelerometer reading is about equal to gravity (1 G), Step  310  calculates a field quaternion using IMU accelerometer readings. In one aspect, Step  302  estimates IMU bias and jitter when an IMU is relatively still. Step  304  stores sensor readings in a history buffer for future use. When the accelerometer reading is not about equal to 1 G, Step  312  uses the gyroscopic quaternion as the current sample quaternion. Step  314  uses the angular orientation errors to selectively mix the gyroscopic quaternion and field quaternion to supply a current sample quaternion. Step  316  decimates the current sample rate to the output sample rate. Step  318  outputs the resultant quaternion. 
     In one aspect, Step  302   b  estimates IMU gyroscope reading bias and Step  302   c  estimates IMU accelerometer reading bias in response to determining a lack of IMU movement. In a related aspect, the method optionally estimates IMU magnetometer jitter errors in Step  302   a.    
     In another aspect, Step  306   a  adjusts the IMU gyroscopic reading to zero, when the gyroscopic reading is near zero, to remove near-zero gyroscope reading jitter. Step  306   b  calculates the current gyroscopic quaternion from the previous sample quaternion and the adjusted gyroscopic reading. 
     In another aspect, Step  308  may calculate a total angular orientation error by combining an orientation angular error due to centripetal acceleration and an orientation angular error due to linear acceleration. When the IMU uses a magnetometer, Step  308  may calculate the total angular orientation error by additionally combining an orientation angular error due to estimated IMU magnetometer jitter. Then, Step  310  calculates the field quaternion using the IMU accelerometer readings and IMU magnetometer readings, and Step  314  selectively mixes the gyroscopic quaternion and field quaternion in response to the angular orientation errors and the estimated IMU magnetometer jitter errors. 
     In one aspect, calculating the field quaternion in Step  310  includes calculating a gravitational quaternion able to rotate an accelerometer reading so that a normalized Z-axis component of the accelerometer reading is equal to 1. Then, calculating the field quaternion may include the following sub-steps. Step  310   d  uses the gravitational quaternion to rotate a magnetometer reading. Step  310   e  sets the rotated magnetometer reading&#39;s Z-axis component to zero, creating an adjusted magnetometer reading. Step  310   f  calculates a magnetometer quaternion able to further rotate the adjusted magnetometer reading so that a normalized X-axis component is equal to −1. Step  310   g  combines the gravitational quaternion and the magnetometer quaternion. 
     In a different aspect, Step  310  calculates the field quaternion further using the following sub-steps. Step  310   a  combines a plurality of accelerometer readings to create a combined accelerometer reading, and Step  310   b  uses the combined accelerometer reading to calculate a gravitational quaternion. For example, the combined plurality of accelerometer readings may create an average accelerometer reading. Likewise, Step  310   c  may combine a plurality of magnetometer readings to create a combined (e.g., average) magnetometer reading, and Step  310   f  uses the combined magnetometer reading to calculate a magnetometer quaternion. Further, Step  310   c  may include selecting a number of readings in the plurality of magnetometers readings in response to a distance (angle) between a current reading and past readings, and the estimated IMU magnetometer jitter. 
     In one aspect, selectively mixing the gyroscopic quaternion and field quaternion to supply the current sample quaternion in Step  314  includes sub-steps. Step  314   a  calculates an angular difference between the gyroscopic quaternion and field quaternion. Step  314   b  compares the angular difference to the total angular orientation error, and Step  314   c  adjusts the mixture of the gyroscopic quaternion and field quaternion in response to the comparison. 
     Step  316   c  low-pass filters four vector elements of the current sample quaternion to provide an output quaternion. Alternatively, Step  316   a  compares a sum of angular commonalties between each current sample quaternion in a group of current sample quaternions, with other current sample quaternions from the group. Step  316   b  selects the current sample quaternion associated with the largest sum as an output quaternion. 
       FIG. 4  is a flowchart illustrating a method for an inertial measurement unit to calculate an orientation-related quaternion. The method begins at Step  400 . In Step  402  the IMU measures a first orientation-related vector and a second orientation-related vector. When the angle between the first orientation-related vector and the second orientation-related vectors is obtuse, Step  404  inverts the first orientation-related vector. Step  406  calculates an orientation-related quaternion using the inverted first orientation-related vector and the second orientation-related vector. When the angle between the first orientation-related vector and the second orientation vector is 90 degrees or acute, Step  408  calculates the orientation-related quaternion using the first orientation-related vector and second orientation-related vector. When the angle between the first orientation-related vector and the second orientation vector is 180 degrees, Step  410  sets the orientation-related quaternion to a normalized value of a previous orientation-related quaternion with the previous orientation-related quaternion r component set to zero. 
     An IMU system and method have been provided. Examples of particular process steps and formulas been presented to illustrate the invention. However, the invention is not limited to merely these examples. Other variations and embodiments of the invention will occur to those skilled in the art.