Method for dynamic autocalibration of a multi-sensor tracking system and apparatus incorporating it therein

[0002] This invention relates to the calibration of real-time multi-sensor tracking systems. More particularly, the invention relates to systems that use information from complimentary sensors, filtered by a simultaneously running filter component, to calibrate sensor bias and to iteratively tune the bias estimate.

[0003] Magnetic compasses are subject to several outside influences that produce errors in the compass reading. One of these error sources is magnetic variation, which is the difference between the direction of the field lines of the earth's magnetic field and true north at any particular point on the earth, and which is a function of geographic position. Another source of error is magnetic deviation, which is the difference between magnetic north and the actual compass reading caused by local magnetic perturbations due to concentrations of metal or other magnetically active substances. Magnetic deviation is typically strong near large metal structures such as bridges, buildings with large steel components, and ships, or near structures that transport electricity such as power stations and electrical cables. Furthermore, in the case of ships or other large movable metal objects, the magnetic deviation may vary not only with direction, but may also vary over time in a particular direction.

[0004] Magnetic variation and static forms of magnetic deviation may be corrected in large part by modeling over the geographic region in question. Current approaches to bias determination rely either on a model of the bias, or on a number of external references and an involved calibration procedure. In the model-based approach, a mathematical model of how ambient magnetic fields modify compass output is used, and the calibration procedure uses a small number of external references to determine the parameters of the model. The heart of this method is the determination of a magnetic bias field that is fixed with respect to the compass, and thus moving with respect to the earth's magnetic field. It involves superimposing and subtracting the mapped magnetic bias field to leave only the earth's magnetic field in order to provide accurate compass output. Unfortunately, this approach has the disadvantage of being model-based, i.e. the bias must follow a general shape, which may not always be applicable to the measured bias. In the external reference approach, a large number of external references (e.g. landmarks at known headings) are used to build a distortion map. In this approach, known heading directions are used to calibrate the compass by comparing the compass reading with a known heading and subtracting the bias for that known heading to generate pure heading information. This approach requires many calibration points and a cumbersome, explicit calibration procedure. As a variation on this approach, prior art has also taught the use of a directional gyroscope to provide a second heading indicator to be compared with the compass heading to yield bias correction. This variation has the advantage of eliminating the need for predetermined, stored directions but is disadvantageous because the drift in the directional gyroscope causes it to become inaccurate over time. Existing systems utilizing this approach require an explicit calibration procedure, and drift out of calibration when the sensor distortion changes unless the calibration procedure is rerun. Thus, in the case of a ship, for example, although a mapped calibration procedure may be successful for a given ship configuration, any change in the structure of the ship or its contents may result in a need for re-calibration.

[0005] A method for dynamic autocalibration of a multi-sensor tracking system, having a system state x_iwithin a state space, including the steps of: dividing the state space into a plurality of patches p_j; providing a variable bias map including a plurality of bias entries {circumflex over (B)}_jwith each particular one of the plurality of bias entries {circumflex over (B)}_jassociated with a particular one of the plurality of patches p_j; providing a vector z_iof inputs from a plurality of sensors for a given time step i; determining the patch p_jto which the vector z_ifrom the plurality of sensors applies; combining the vector z_iwith the bias entry {circumflex over (B)}_jcorresponding to the patch p_jto which the input z_ifrom the plurality of sensors applies to provide a bias adjusted sensor input; providing a state estimator to receive the bias adjusted sensor input and to produce a system state estimation {circumflex over (x)}_icorresponding to the time step i; using a combination of the bias adjusted sensor input and the system state estimation {circumflex over (x)}_ito adjust the bias entry {circumflex over (B)}_jof the variable bias map corresponding to the patch p_jto which the input z_ifrom the plurality of sensors applies; and repeating a portion of the steps to provide a continual system update. In the case where the bias map includes a constant bias offset, the method further includes, the steps of: obtaining a calibration bias pair including an externally specified calibration bias {circumflex over (B)}* and a corresponding system state calibration value x *; and applying the calibration bias pair to the variable bias map to eliminate the constant bias offset. The error minimization utilized in the present invention may be chosen as desired for a particular application, but is preferably performed by means of gradient descent utilizing a learning rate γ′ and the combination of the bias adjusted sensor input and the system state estimate {circumflex over (x)}_i.

[0006] In addition to the use of a look-up table type bias map, the bias map may also be represented in the form of a parametric equation. An example of this type of bias map is provided through the use of Gaussian fuzzy sets, and is represented in a preferred embodiment including at least one gyroscope, at least one compass, and at least one tilt sensor incorporated as part of a head-mounted orientation tracker.

[0007] FIG. 1 is a flowchart presenting the steps of the present invention in a general embodiment, which utilizes gradient descent as the procedure used in the error minimizer;

[0008] FIG. 2 is a block diagram demonstrating the operation of the present invention in a general embodiment, which utilizes gradient descent as the function used in the error minimizer;

[0009] FIG. 3 is a flowchart presenting the steps of the present invention in a general embodiment, which utilizes a significantly simplified gradient descent function in the error minimizer;

[0010] FIG. 4 is a block diagram demonstrating the operation of the present invention in a general embodiment, which utilizes a significantly simplified gradient descent function in the error minimizer;

[0011] FIG. 5(a) presents results from a head-tracking embodiment in the form of a chart showing the actual and estimated compass bias versus the angle for an artificial bias of five degrees, centered at a heading of 100 degrees;

[0012] FIG. 5(b) provides results from a head-tracking embodiment in the form of a demonstration showing the actual and estimated compass bias versus angle for three data sets;

[0013] FIG. 6 presents results from a head-tracking embodiment in the form of a table displaying the root-mean-squared discrepancy between the sensed and estimated heading for three data sets, before and after the bias-estimation method is applied;

[0014] FIG. 7 is a flowchart presenting the steps of the present invention as utilized with Gaussian fuzzy sets to represent the bias map in a head-tracking embodiment;

[0015] FIG. 8 is a block diagram demonstrating the operation of the present invention as utilized with Gaussian fuzzy sets to represent the bias map in a head-tracking embodiment.

[0016] The present invention relates to a method for using information from complementary sensors to calibrate sensor bias, and apparatus incorporating it therein. The following description is presented to enable one of ordinary skill in the art to make and use the invention and to incorporate it in the context of particular applications. Various modifications to the preferred embodiment, as well as a variety of uses in different applications will be readily apparent to those skilled in the art, and the general principles defined herein may be applied to other embodiments. Thus, the present invention is not intended to be limited to the embodiments shown, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

[0017] In order to provide sensor bias calibration, the present invention utilizes information from a simultaneously running filter that combines sensor data into an estimation of the state of the monitored system in order to iteratively tune the bias estimate. To track a dynamic system, complementary, or redundant, sensors may be combined with a model of the system so that the uncertainty in the estimated state of the dynamic system is less than the noise in the individual sensors. In addition to noise, sensors may have bias, i.e. they may produce errors that vary with the system's state but that are repeatable (having the same value whenever the system is in a particular state). While estimating the actual value of the state, the present invention uses an estimator to determine the bias.

[0018] The primary application of the present invention to date is in head-mounted orientation tracking systems, which use a compass, a tilt sensor, and gyroscopes to track movement of the head and to estimate its orientation at any time. The particular system to which the present invention has been applied involves what has been termed “augmented reality.” Specifically, the system includes an information overlay for the natural environment, which provides visual cues regarding various landmarks and other environmental details. In order to provide cues in a meaningful manner, the system must track both the location of the user and the orientation of their head in terms of roll, pitch, and heading. The orientation of the user's head is tracked by inertial navigation combined with the absolute orientation provided by a compass and tilt sensor. The compasses utilized in the trackers are known to have orientation-specific distortions due to ambient magnetic fields. An advantage of utilizing the present auto-calibration method lies in its ability to allow the combined use of the gyroscopes and the compass to automatically map out the distortion in real-time. By utilizing sensor redundancy to perform the calibration, rather than standard modeling procedures, use of only one external landmark at a known heading is required. Thus, the present invention makes no “model-based” assumptions regarding the shape of the distortion map, which is especially important because distortion maps often obey no predictable pattern. This unpredictability is especially problematic when there is a need for a high degree of accuracy. Further, because the bias-determination process runs concurrently with the state estimator, changes in the distortion map are constantly monitored and corrected. Since existing systems require an explicit calibration procedure, they drift out of calibration in response to unpredictable sensor distortion changes unless the calibration procedure is rerun.

[0019] In the problem of estimating the unbiased state, the system herein may be mathematically modeled as a discrete time dynamic system that is subject to perturbing noise. Furthermore, the sensors that track the state of the system are inherently noisy. Using a combination of a model of the system and the sensor data, an estimate of the state of the system at any time may be calculated. The resulting estimate provides more accurate results than either the model or the sensors alone. Additionally, it is desirable to solve a parameter estimation problem. Specifically, the sensors have inherent biases that vary with the system state, but that are also repeatable. As mentioned previously, this means that each time the system passes through a specific state, the biases are the same. This is in contrast to the sensor and process noise, may vary even within the same specific state.

[0020] The system state at a particular time step i is represented by x_i, evolves according to linear state dynamics as follows:

[0021] x_i+1=A_ix_i+w_i,  (1)

[0022] where x_i+1represents the next system state (i.e. the system state at time step i+1), A_irepresents a state transition matrix, and w_iis a noise source, where {overscore (w)}_i=0. Higher order non-linear terms, including driving inputs, are modeled as noise, and are also included in w_i.

[0023] A sensor measures the system state x_i, producing output in the form of a vector z_iof inputs from the plurality of sensors. The dependence of the vector z_ion the system state x_iis given by:

[0024] z_i=Hx_i+B(x_i)+V_i  (2)

[0025] where v_iis a noise source, where {overscore (v)}_i=0, H is a constant matrix for mapping state values onto the system state x_i, and B(x_i) is the state dependent sensor bias.

[0026] The estimation process is subject both to process noise (which makes open loop modeling inaccurate) and to sensor noise and bias (which make sensing of the state inaccurate). The estimation approach combines the next system state estimation x_i+1based on the system dynamics as set forth in Equation (1) above, and the vector z_ifrom the sensors as set forth in Equation (2) above, resulting in:

[0027] {circumflex over (x)}_i+1=A_i{circumflex over (x)}_i+K(z_i+1−{circumflex over (B)}(A_i{circumflex over (x)}_i)−H A_i{circumflex over (x)}_i)  (3)

[0028] where {circumflex over (B)}(A_i{circumflex over (x)}_i) is the estimate of the sensor bias. Two elements are combined to give the updated next state estimate, {circumflex over (x)}_i+1as follows: A prior estimate from time step i, {circumflex over (x)}_i, is applied to the state transition matrix A_ito give the next system state estimate {circumflex over (x)}_i+1. This is modified by a correction term, z_i+1−{circumflex over (B)}(A_i{circumflex over (x)}_i)−H A_i{circumflex over (x)}_i, which represents the difference between what the sensors estimate and what the model predicts. K in the equation represents weight values chosen to add emphasis to either the model or the sensors based on their relative noisiness. The remaining problem is to determine the estimated bias {circumflex over (B)}. This determination is performed by the system depicted in its generic form in FIG. 1 and 2, as discussed below.

[0029] Before system operation, the state space is divided into a plurality n measurement regions, or patches p₁−>p_n, each of which has a single associated bias estimate. That is, when the estimated system state is in the measurement patch p_j, the bias estimate {circumflex over (B)}_jis applied. In other words, {circumflex over (B)}(A_i{circumflex over (x)}_i)={circumflex over (B)}_j, if A_i{circumflex over (x)}_iεp_j. In this case, the bias estimates {circumflex over (B)}_jare specific to their associated patches p_jso they may be stored in the form of a look-up table.

[0030] Next, the quantity E[z_i−Hx_i|x_i|], representing the difference between the sensed and true values for a state, is computed. Substituting Equation (2) for the vector z_ito determine this quantity, the following operation is performed:

[0031] E[z_i−Hx_i|x_i]=E[B(x_i)+v_i|x_i]=B(x_i)+{overscore (v)}_i=B(x_i).  (4)

[0032] This follows from the fact that the state-dependent sensor bias B(x_i) is constant for each system state x_i, and {overscore (v)}=0. By collecting statistics on this difference, the state dependent bias could be computed and used to adjust the sensor output. The problem would be feasible if the actual system state, x_i, were known. However, there is no direct access to this information and an indirect estimation method must be employed. Another way to cast the problem is to subtract a sensor bias estimate, then tune it so the expected value is zero. In other words,

[0033] E[z_i−{circumflex over (B)}(x_i)−Hx_i|x_i]=0,

[0034] where {circumflex over (B)}(x_i) is a good estimate of the sensor bias. This could be done for each patch p_j, by searching for an estimated bias, {circumflex over (B)}_j, where

[0035] E[z_i−{circumflex over (B)}_j−Hx_i|x_iεp_j]=0.

[0036] In order to determine the bias, various error minimization algorithms may be utilized. One particular method, gradient descent, has been found to be effective for the particular embodiments presented herein. Although gradient descent is presented herein to provide error minimization, various other algorithms may be equally effective depending on the particular application.

[0037] Gradient descent provides a straightforward approach to this parameter estimation problem: First, construct a function of the bias estimate {circumflex over (B)}_j, called an energy function, which is everywhere nonnegative, and is a minimum where the bias estimate {circumflex over (B)}_jis optimal. Then take the gradient of this function with respect to the bias estimate {circumflex over (B)}_jand incrementally change the bias estimate {circumflex over (B)}_jto reduce the value of the energy function in search of an optimum.

[0038] An example of an energy function useful for an embodiment of the invention is provided by E_j, which represents the statistical variance of d_iover all “visits” the system makes to particular patch p_j: $\begin{array}{cc}{E}_j=\frac{1}{2}\frac{1}{\left|p_j\right|}\sum _{{\hat{x}}_i{\mathrm{\epsilon p}}_j}{d_i}^T{d}_i,& \left(5\right)\end{array}$

[0039] where

[0040] d_i=z_i−{circumflex over (B)}_j−H{circumflex over (x)}_i  (6)

[0041] where |p_j| is the number of “visits” the system makes to particular patch p_j, and the “½” is an arbitrary positive constant added for convenience.

[0042] The gradient descent algorithm is expressed by the parameter update rule:

[0043] Δ{circumflex over (B)}_j=−γ(∇_{{circumflex over (B)}}E_j)^T  (7)

[0044] where Δ{circumflex over (B)}_jis the change to the sensor bias estimate {circumflex over (B)}_jfor a particular patch p_j, γ is a small positive constant controlling the learning rate, and ∇_{{circumflex over (B)}}E_jis the gradient of E_jwith respect to {circumflex over (B)}_j. The gradient of Equation (5) is determined with respect to {circumflex over (B)}_j $\begin{array}{cc}{\nabla }_{{\hat{B}}_j}{E}_j=\frac{1}{\left|p_j\right|}\sum _{{\hat{x}}_i\in p_j}{\left(\frac{\partial d_i}{\partial {\hat{B}}_j}\right)}^T{d}_i,\mathrm{where}\frac{\partial d_i}{\partial {\hat{B}}_j}=H\left(\sum _{k\le is.t.x_k\in p_j}\prod _{l=k}^{i-1}\left(I-\mathrm{KH}\right)A_i\right)K-I& \left(8\right)\end{array}$

[0045] where s.t. is an abbreviation for “such that”, I is an indexing variable, I represents the identity matrix, and K, A_i, and H are as previously defined relative to Equations (3), (1), and (2), respectively.

[0046] Since the energy function, Equation (8) is a sum; the incremental bias change Δ{circumflex over (B)}_jcan be modeled as a sequence of changes, one for each visit to a particular patch p_j, i.e. $\Delta {\hat{B}}_j=\sum _{{\hat{x}}_i\in p_j}\Delta {\hat{B}}_j^i,\Delta {\hat{B}}_j^i=-{\gamma }^{\prime }{\left(\frac{\partial d_i}{\partial {\hat{B}}_j}\right)}^T{d}_i^T,\mathrm{and}$ ${\gamma }^{\prime }=\frac{1}{\left|p_j\right|}\gamma .$

[0047] FIG. 1 provides a flow chart that summarizes the bias estimating procedure. As shown in the figure, it is first necessary to set a learning rate γ′ for the system, represented by box 100. Next, an initial system state estimate {circumflex over (x)}₀is obtained as represented by 102. Next, an externally specified calibration bias (x*, {circumflex over (B)}*) (e.g. a known landmark at a known heading in the case of a head-mounted orientation tracking system) is obtained, as represented by 104. The remaining boxes of FIG. 1 demonstrate the routine that is performed for each time step i during the operation of the system. The arrow 122 indicates repeated performance of this routine. Sensor sample data in the form of a vector z_iis first received in the system from a plurality of sensors 106. After receiving the sensor sample data vector z_iit is necessary to determine the current measurement patch p_jsuch that x_i−1εp_j, and to look up the associated bias estimate {circumflex over (B)}_jfrom the bias estimate table as shown by 108. The bias estimate table is preferably designed to perform the patch p_jdetermination. As shown by box 110, the estimated system state {circumflex over (x)}_iis developed through the following equation,

[0048] {circumflex over (x)}_i=A_i−1{circumflex over (x)}_i−1+K(z_i−{circumflex over (B)}_j−H A_i−1{circumflex over (x)}_i−1).

[0049] The difference between the bias-adjusted sensor input and the estimated system state are then calculated, as shown by box 112, by the following,

[0050] d_i=z_i−{circumflex over (B)}_j−H{circumflex over (x)}_i.

[0051] Next, as represented by box 114, the Jacobian, $\frac{\partial d_i}{\partial {\hat{B}}_j}=H\left(\sum _{k\le is.t.x_k\in p_j}\prod _{l=k}^{i-1}\left(I-\mathrm{KH}\right)A_i\right)K-I,$

[0052] is calculated. For summation via the Jacobian calculation of 114, this procedure requires the retention of past values of the state transition matrix A_iand information regarding the time step i and the patch p_j, as represented by box 116. Next, the update, {circumflex over (B)}_j={circumflex over (B)}_j+Δ{circumflex over (B)}ⁱ_j, is applied to the bias table where, $\Delta {\hat{B}}_j^i=-{\gamma }^{\prime }{d}_i^T\frac{\partial d_i}{\partial {\hat{B}}_j},$

[0053] as represented by box 118. Finally, all bias estimates are periodically refreshed by ∀j, {circumflex over (B)}_j={circumflex over (B)}_j−({circumflex over (B)}_j*−{circumflex over (B)}*), where ∀j is used as a convention for “for all j”, {circumflex over (B)}_j*represents the estimated calibration bias applied to the particular patch p_j, and {circumflex over (B)}* represents the bias provided by the external calibration bias(x*, {circumflex over (B)}*) to subtract the constant bias error from the entire bias map, as shown by 120. Finally, the externally specified calibration bias (x*, {circumflex over (B)}*) is obtained and applied to the bias table, as represented by 120. After the externally specified calibration bias (x*, {circumflex over (B)}*) has been applied to the bias table, the routine begins again with the receipt of a new sensor sample data vector z_i104, as represented by the arrow 122 between box 120 and 104.

[0054] FIG. 2 provides an overview of the dynamic auto-calibration estimated bias determination method of the present invention in the form of a block diagram. Prior to operation of the system, a learning rate γ′ is set and is represented by block 200. A bias table 202 is maintained, which is indexed by the estimated system state {circumflex over (x)}_iand whose output is the estimated bias {circumflex over (B)}_j. A single external input that provides the bias for one state, x*, is needed to offset the bias table in order to allow for calibration. Thus, an externally specified calibration bias (x*, {circumflex over (B)}*) 204 including a sensor value reference, x*, and an externally specified calibration bias {circumflex over (B)}* are provided. Furthermore, all bias estimates are periodically adjusted by ∀j, {circumflex over (B)}_j={circumflex over (B)}j−({circumflex over (B)}_j*−{circumflex over (B)}*), as represented by 206, which incorporates the externally specified calibration bias {circumflex over (B)}*. Biased input from a plurality of sensors 208 is aggregated in the form of a vector, z_i=Hx_i+B(x_i)+v_i, where x_iis a vector representation of the system state at time step i; H is a constant matrix for mapping state values onto sensor values; B is the state dependent sensor bias; and v_irepresents unbiased sensor noise. The estimated bias {circumflex over (B)}_jfor a given estimated state, is subtracted from the biased sensor reading vector z_iby a first comparator 210, to provide a bias-adjusted sensor reading z_i−{circumflex over (B)}_j. The bias-adjusted sensor reading z_i−{circumflex over (B)}_jis provided to an estimator 212, which then generates a system state estimate vector {circumflex over (x)}_i. The estimator 212 may also include memory to provide for the retention of past values of the state transition matrix A_iand information regarding the time step i and the patch p_j, as discussed relative to box 116 of FIG. 1. The system state estimate vector {circumflex over (x)}_iis then multiplied through the constant matrix H 214, providing a constant matrix-multiplied estimate H{circumflex over (x)}_i. The constant matrix-multiplied estimate H{circumflex over (x)}_iand the bias-adjusted sensor reading z_i−{circumflex over (B)}_jprovide inputs to a second comparator 216, with the bias-adjusted sensor input z_i−{circumflex over (B)}_jbeing subtracted from the constant matrix-multiplied estimate H{circumflex over (x)}_i. The Jacobian of the resulting difference d_i=(z_i−{circumflex over (B)}_j)−(H{circumflex over (x)}_i) is then calculated 218, and is then multiplied by the learning rate −γ 200. The learning rate preferably takes the form of a multiplicative scalar parameter between zero and one to provide a bias estimate adjustment figure Δ{circumflex over (B)}_j. An incrementor 220 combines the bias estimate {circumflex over (B)}_jgenerated from the bias table 202 with the bias estimate adjustment figure Δ{circumflex over (B)}_jto provide an adjusted bias estimate {circumflex over (B)}_j+Δ{circumflex over (B)}_j. The bias table 202 receives the adjusted bias estimate {circumflex over (B)}_j+Δ{circumflex over (B)}_jand the estimate of the system state {circumflex over (x)}_iin order to provide the next bias estimate {circumflex over (B)}_jto the first comparator 208. The change in the bias table can be computed from the difference between the estimated state and the corrected sensor reading. The rate of change is governed by the learning rate. Over time, the sequence of changes to the bias table drives the values it contains toward the correct values. Note that the incrementor 220, the learning rate −γ 200, and the Jacobian calculation 218 are all part of what is termed the error minimizer 222 of the present invention. As previously stated, there are many potential error minimization functions that may be employed.

[0055] The calculation of the Jacobian can be computationally intensive, as each term in the sum contains a product of a series of matrices. However, a significant simplification may be made. Note that the simplification is not critical to the present invention, but rather, is provided to demonstrate a preferred method by which its operation may be simplified. The mathematical steps in the simplification will be illustrated below in order to provide a clear explanation. As described above, the matrix K is designed to give partial weight to sensor input, with the sensor input as previously described in Equation (3). Similarly, (I−KH) gives partial weight to the model prediction. When these matrices are iteratively multiplied, they decay the terms in which they are contained. This is best illustrated with an example as follows:

[0056] First, rewrite Equation (8), $\frac{\partial d_i}{\partial {\hat{B}}_j}=H\left(\sum _{k\le is.t.x_k\in p_j}\prod _{l=k}^{i-1}\left(I-\mathrm{KH}\right)A_i\right)K-I,$

[0057] and, for simplicity set H=I and A_i=A. The equation then becomes $\frac{\partial d_i}{\partial {\hat{B}}_j}=I\left(\sum _{k\le is.t.x_k\in p_j}{\left(I-\mathrm{IK}\right)}^{i-1-k}{A}^{i-1-k}\right)K-I.$

[0058] Now set K=α·I; 0<α<1, and the equation becomes $\frac{\partial d_i}{\partial {\hat{B}}_j}=\left(\sum _{k\le is.t.x_k\in p_j}{\left(I-\mathrm{aI}\right)}^{i-1-k}{A}^{i-1-k}\right)\mathrm{aI}-I,\mathrm{or}$ $\frac{\partial d_i}{\partial {\hat{B}}_j}=a\left(\sum _{k\le is.t.x_k\in p_j}{\left(I-a\right)}^{i-1-k}{A}^{i-1-k}\right)-I$

[0059] Clearly, there exist terms in the last equation when k<<i. Since an approximate gradient often allows gradient descent to find an optimum, i.e. by proceeding ‘gradually downhill’ rather than ‘directly downhill’, it is sufficient to ignore all the terms that include K, and approximate Equation (8) by $\frac{\partial d_i}{\partial {\hat{B}}_j}=-I,$

[0060] which is much simpler computationally. As a result, the whole bias-update algorithm may be written compactly as follows:

[0061] Δ{circumflex over (B)}_jⁱ=−γ′(z_i−{circumflex over (B)}_j−H{circumflex over (x)}_i)(−I)

[0062] or

[0063] Δ{circumflex over (B)}_jⁱ=γ′(z_i−{circumflex over (B)}_j−H{circumflex over (x)}_i).  (9)

[0064] FIG. 3 provides a flow chart of the present invention incorporating the simplification process just described. Note that steps 100, 102, 104, 106, 108, 110, 112, and 120 are the same as were shown in FIG. 1. The main difference in FIG. 3 lies in the 318, which represents the simplification above and is equivalent to the Equation (9). After the update has been applied to the bias table, the routine begins again with the receipt of new sensor sample data, z_i, as represented by the arrow 322 between box 120 and 106.

[0065] FIG. 4 provides block diagram of the dynamic auto-calibration estimated bias determination method, incorporating the simplification discussed above. This figure is a modified version of FIG. 2, in which elements 202, 204, 206, 208, 210, 212, 214, 216, 220, and 222 remain the same. However, in FIG. 4 the resulting difference, d_i=(z_i−{circumflex over (B)})−(H{circumflex over (x)}_i), is directly multiplied by the positive learning rate scalar γ 400. Thus, the simplification avoids the need to calculate the computationally complex Jacobian of the resulting difference, d_i=(z_i−{circumflex over (B)})−(H{circumflex over (x)}_i). Below, an embodiment is described, which has been used to test the validity of the simplification, and to utilize the present invention in its preferred embodiment.

[0066] In order to test the validity of the simplification, the invention has been reduced to practice in the context of a head-mounted system that tracks head orientation using a compass, tilt sensor, and three rate gyroscopes. The compass is subject to unpredictable, but persistent distortions due to local magnetic field variations. The invention was applied, to sensor data collected from the head tracking system. It was also run on artificial data generated from a simulation of a distorted compass working in concert with gyroscopes. FIG. 5(a) presents the estimated compass bias versus the angle for an artificial bias of five degrees, centered at a heading of 100 degrees. The dotted line represents the actual bias and the solid line represents the estimated bias. FIG. 5(b) presents the estimated compass bias versus angle for three data sets, with the dotted line showing the bias estimate determined independently of the present invention. The shape of the distortion is accurately estimated, while the overall offset remains to be determined. To eliminate this offset in order to provide an accurate measure of the bias, the single external calibrating input, x*, is utilized. The externally specified calibration bias (x*,{circumflex over (B)}*) is provided for this purpose.

[0067] The implemented system uses inertial navigation to estimate the position of the head. The goal is to estimate the angular position of the head from the input from the compass/tilt sensor suite and the three orthogonally mounted gyroscopes. The compass/tilt sensor suite gives absolute orientation information, with respect to the earth's magnetic and gravitational fields. Two effects impact the compass information: magnetic declination and compass distortion. The magnetic declination adds a constant offset to the compass output. This may be determined either by referring to a magnetic declination map or by reading the compass while sighting a landmark at a known global positioning system (GPS) location, and then using the user-landmark offset to determine the true heading to the landmark. Compass distortion, typically only a few degrees, is a small but significant offset, which may vary with compass heading and with user location. If the mapping of compass heading to heading error followed a systematic and consistent pattern, e.g. simply changed in scale or orientation from day-to-day or from site-to-site, then we would simply create a parameterized corrective map and we would determine the parameters by sighting known landmarks. Such distortion is not systematic or consistent, so creation of a parameterized corrective map is not feasible. Thus, the autocalibration method described herein provides a means for finding the bias map.

[0068] For the head tracking system just described, the system state, x_iis six dimensional, consisting of the head's roll (r), pitch (p), and heading (h), in terms of the output of the compass/tilt sensor suite, and the rates of change of roll, pitch, and heading, as measured by the rate gyroscopes. Thus, this six-dimensional vector may be represented as:

[0069] x_i=[r_cp_ch_cr_gp_gh_g]^T

[0070] where the subscripts ‘C’ and ‘g’ refer to the compass/tilt sensor and gyroscopes, respectively. The system dynamics equation is: $x_{i+1}=[\begin{array}{cc}{I}_{3\times 3}& \Delta t{A}_{12i}\\ 0_{3\times 3}& I_{3\times 3}\end{array}]x_i+w_i,$

[0071] where A_12iis the 3×3 state transition matrix of derivatives (i.e. the Jacobian of compass/tilt sensor suite values with respect to gyroscopic rotations), and w_iis the noise term, lumping together process noise and acceleratory inputs. Because of the definition of the output of the particular compass/tilt sensor employed in the embodiment, and the orientations of the rate gyroscopes relative to the compass-tilt sensor, A₁₂was determined to be, $A_{12}\left(x\right)=[\begin{array}{ccc}{\mathrm{cpc}}^{2}r/a^2& \mathrm{asrcrsp}\left(t^{2}r+2/c^{2}p\right)& {\mathrm{atpc}}^{2}r/c^{2}p\\ 0& a/\mathrm{cp}& -\mathrm{atr}\\ 0& \mathrm{atr}/\mathrm{cp}& a/c^{2}p\end{array}],$

[0072] where $a=\frac{1}{\sqrt{1+t^{2}p+t^{2}r}},\mathrm{where}$

[0073] s=sin(θ), c=cos(θ), t=tan(θ), and where r, and p are the roll and pitch values in the state x, and Δt is the time step.

[0074] Because the state is defined in terms of the sensor outputs, H=I in Equation (2). The matrix of weights K in Equation (3) was chosen to be: $K=[\begin{array}{cc}{g}_c{I}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& g_g{I}_{3\times 3}\end{array}]$

[0075] where the values g_g=1.0, g_c=0.05 were determined empirically. This provides the state estimation procedure, i.e. Equation (3). The bias estimation must still be specified. Because the embodiment is designed only to detect and correct the compass heading bias, the bias estimation is as follows:

[0076] {circumflex over (B)}(x)=(0 0 {circumflex over (B)}^H(x) 0 0 0)^T  (10)

[0077] The simplified version of the gradient descent algorithm provided by Equation (9) was used, and γ′ was set to 0.05. Data sets used for the testing had on the order of 10,000 sensor inputs, collected at a frequency of 1 KHz. The testing procedure ran each data set 25 times to provide the results shown below. Instead of a simple “look-up table” of values of {circumflex over (B)}_j, it was desired to generate a function that smoothly interpolated the bias estimate for intermediate angles. As a result, an array of Gaussian fuzzy sets were employed which defined the bias estimate. The resulting general equation for the bias estimate {circumflex over (B)} is as follows: $\begin{array}{cc}\hat{B}\left({\hat{x}}_i\right)=\sum _{l=0}^N{W}_l{e}^{\frac{-\left|\right|{\hat{x}}_i-c_l{\left|\right|}^2}{{\sigma }_W^2}}& \left(11\right)\end{array}$

[0078] where N represents the total number of Gaussian fuzzy sets in the bias map, σ_W²defines the widths of the Gaussian fuzzy sets, l is an indexing variable, and c_ldefines the center of the lth Gaussian fuzzy set.

[0079] For purposes of the present invention, the following assignments were made: c_l=10l, σ=20, N=36∥{circumflex over (x)}_i−c_l∥²=({circumflex over (x)}_i−10l)². The resulting bias estimate {circumflex over (B)} equation is: $\begin{array}{cc}\hat{B}\left({\hat{x}}_i\right)=\sum _{l=0}^{35}{W}_l{e}^{-\frac{{\left(x-10l\right)}^2}{{\sigma }_W^2}},{\sigma }_W=20& \left(12\right)\end{array}$

[0080] thus, an array of 36 Gaussian fuzzy sets was generated, with centers 10 degrees apart and widths of 20 degrees. The use of Gaussian fuzzy sets necessitates a slight modification to the original gradient descent algorithm. This modification will be shown in detail below.

[0081] Instead of

[0082] Δ{circumflex over (B)}=−γ(∇_{{circumflex over (B)}}E)^T  (13)

[0083] we have

[0084] ΔWⁱ=−γ′(∇_WE)^T  (14)

[0085] where, ΔWⁱis the change in W at time step i, and by the chain rule, equals $\begin{array}{cc}\Delta W^i=-{\gamma }^{\prime }{\left({\nabla }_{\hat{B}}E\frac{\partial \hat{B}}{\partial W}\right)}^T& \left(15\right)\end{array}$

[0086] or, by substituting Equation (13), $\Delta W^i={\left(\frac{\partial \hat{B}}{\partial W}\right)}^T\Delta {\hat{B}}^i$

[0087] Next, substituting Equation (9), $\begin{array}{cc}\Delta W^i=-{\gamma }^{\prime }{\left(\frac{\partial \hat{B}}{\partial W}\right)}^T\left(z_i-{\hat{B}}^i-H{\hat{x}}_i\right)& \left(16\right)\end{array}$

[0088] Differentiating Equation (12) $\begin{array}{cc}\frac{\partial \hat{B}}{\partial W_l}=e^{-\frac{{\left(x-10l\right)}^2}{{\sigma }_W^2}}& \left(17\right)\end{array}$

[0089] Combining Equation (16) and Equation (17) generates the final parameter update formula: $\begin{array}{cc}\Delta W_l^i={\gamma }^{\prime }\left(z_i-{\hat{B}}^i-H{\hat{x}}_i\right)e^{-\frac{{\left(\widehat{x_i}-10l\right)}^2}{{\sigma }_W^2}}& \left(18\right)\end{array}$

[0090] The invention was first applied to artificial data with a local compass bias introduced. The data set mimicked a constant horizontal head rotation of 40 degrees per second, for 10 seconds, yielding a sequence of 10,000 sensor data points. As previously discussed for FIG. 5(a), an artificial distortion in the compass input was introduced, which was centered at a heading of 100 degrees, where it had a maximum of 5 degrees of error. As can be seen in the solid line in the same figure, after running through the data set 25 times, the bias estimation captured the actual bias, albeit with a constant offset. This offset was cured by the use of an externally specified calibration bias (x*,{circumflex over (B)}*). Below, in conjunction with the description of a specific embodiment of the present invention, detail regarding use of the externally specified calibration bias (x*, {circumflex over (B)}*) to subtract this constant offset from the estimated bias will be provided.

[0091] Next, the invention was applied to three real data sets, each of which was collected during horizontal rotation of the head for a duration of about 20 seconds, and yielding about 20,000 samples of sensor input. The data sets were collected in the same session, and at the same physical location, so the compass bias was assumed to be the same. As can be seen in FIG. 5(b), the bias estimation is about the same for each data set. Further, the bias estimate is comparable to that estimated by a separate procedure.

[0092] Another measure of the algorithm's performance is the root mean squared discrepancy between sensed and estimated compass heading, i.e. the standard deviation of d_i, defined by Equation (6):

[0093] d_i=z_i−{circumflex over (B)}_j−H{circumflex over (x)}_i

[0094] Because of sensor and process noise, this value will never decrease to zero. The table presented in FIG. 6 compares the root mean squared discrepancy, before and after tuning the bias estimate, for all three data sets. The results shown in the table clearly demonstrate that the calibration method of the present invention consistently drives this discrepancy to a small value.

[0095] One means for obtaining an externally specified calibration bias (x*, {circumflex over (B)}*) in the context of the head tracking system described here is to sight a landmark at a known heading, read the compass sensor value, and compute the true compass bias at that heading by subtracting the true heading from the compass output. The measurement patch at the known heading may be termed the landmark measurement patch, p_j*, and the computed compass bias may be termed the true bias {circumflex over (B)}_j*={circumflex over (B)}*. Next, the normal gradient descent algorithm is applied, but all bias estimates are periodically adjusted by

[0096] ∀j,{circumflex over (B)}_j={circumflex over (B)}_j−({circumflex over (B)}_j*−{circumflex over (B)}*).

[0097] This adjustment subtracts the constant bias error from the entire bias map.

[0098] Another method of adjusting the bias map in the context of the head-tracking embodiment involves the user during use of the system. When the head's estimated orientation is used in an augmented reality system, information is presented on a see-through, heads up display so that it aligns with visible objects in the user's vicinity. The locations on the computer screen at which information is displayed are determined using estimated head orientation. Since the bias-estimation error is constant for all head rotations, all the projected information will appear offset to the left or right by a constant angular displacement. It would be a simple matter to allow the user to control a single scalar input while using the system, e.g. by turning a knob, until the projected information is aligned with the world view. Thus, the system may also be manually aligned.

[0099] FIG. 7 provides a flowchart of the head-tracking embodiment of the present invention as a modification of FIG. 1. The steps represented by 100, 102, 104, 106, 110, 112, and 120 all perform the same function as those shown in FIG. 1. The step represented by 708 provides for the computation of the bias estimate {circumflex over (B)} as follows: ${\hat{B}}^i=\sum _{l=0}^{35}{W}_l{e}^{-\frac{{\left(x_{i-1}-10l\right)}^2}{{\sigma }^2}}$

[0100] In this case, the bias is represented in the form of an equation rather than by a look-up table. Thus, the time step i, the patch p_j, and the state transition matrix A_i, need not be saved.

[0101] The step represented by 714 provides the calculation, $\frac{\partial \hat{B}}{\partial W_l}=e^{-\frac{{\left(x-10l\right)}^2}{{\sigma }_W^2}},{\sigma }_W=20,$

[0102] also shown by Equation (17) above, where W is defined by Equation (13) and σ_W=20 for the particular embodiment. The step represented by 718 provides the update for this embodiment and is equivalent to step 318 of FIG. 3. After the update has been applied to the bias table, the routine begins again with the receipt of sensor data vector z_i106, as represented by the arrow 722 between box 120 and 106.

[0103] FIG. 8 provides an overview of the dynamic auto-calibration estimated bias determination method, representing the head-tracking embodiment discussed above in the form of a block diagram. This figure is a modified version of FIG. 2, in which elements 200, 202, 204, 206, 208, 210, 212, 214, 216, 220, and 222 are essentially unchanged. However, in FIG. 8 the resulting difference, d_i=(z_i−{circumflex over (B)})−(H{circumflex over (x)}_i), is multiplied by $\frac{\partial \hat{B}}{\partial W_l}=e^{-\frac{{\left(x-10l\right)}^2}{{\sigma }_W^2}},$

[0104] as represented by 800, and as also shown by Equation (17) above, where W as defined for Equation (11) and σ_W=20 for the particular embodiment. The product is then multiplied by the learning rate −γ 200, and the rest of the figure is analogous to FIG. 2.

[0105] Although the present invention has been discussed in the context of a head-tracking system embodiment, there are a wide variety of applications to a wide range of sensors and sensor data. The present invention provides a means to automatically estimate sensor bias, using information readily available from the concurrent sensor fusion/state estimation process. The bias estimation process described herein requires no assumption about the shape of the bias curve. In other words, it is model free. The method is computationally tractable, avoiding storing a long history of state and sensor information, and avoiding large amounts of matrix mathematics. In the head-tracking embodiment previously discussed, the bias is estimated modulo a constant offset, which can be determined by a single external input. This provides a major advantage over existing bias-estimation techniques that require a number of external inputs distributed along the range of possible headings.