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

• US 6,577,976 B1
• Filed: 09/17/1999
• Issued: 06/10/2003
• Est. Priority Date: 09/17/1999
• Status: Expired due to Fees

• Current Assignee: HRL Laboratories LLC (The Boeing Co.)
• Original Assignee: HRL Laboratories LLC (The Boeing Co.)
• Inventors: Azuma, Ronald T., Hoff, Bruce
• Primary Examiner(s): Barlow, John
• Assistant Examiner(s): VO, HIEN XUAN
• Application Number:
  US09/397,899
• Time in Patent Office: 1,362 Days
• Field of Search: 702/95, 702/94, 702/93, 702/150, 702/85, 702/86, 702/92, 701/221, 701/200, 333/56, 333/61, 333/52, 324/246, 324/245, 324/225, 73/17.6, 731/78.R
• US Class Current: 702/95
• CPC Class Codes: G01C 17/38 Testing, calibrating, or compensating of compasses
• CPC Class Codes: G01C 25/00 Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass testing, calibrating or compensating compasses G01C17/38

Abstract

A method for dynamic auto-calibration of a multi-sensor tracking system and apparatus incorporating it therein are presented. The method and apparatus utilize information from complementary sensors, filtered by a simultaneously running filter that combines the sensor data into an estimate of the state of the monitored system to iteratively tune the bias estimate. To track a dynamic system, complimentary 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, the sensors may have bias, which has the same value whenever the system is in a particular state. While estimating the actual value of the state, the present invention allows the estimator to determine the bias. The present invention, in its most general embodiment requires complex computations. However, significant simplifications have also been developed, which reduce the necessary computational power to a manageable level. The present invention has been reduced to practice in the context of a head-tracking system employing a plurality of sensors to determine the orientation of a human head.

Claims

1. A method for dynamic autocalibration of a multi-sensor tracking system, having a system state x_i within a state space, including the steps of:

a. dividing the state space into a plurality of patches p_j;

b. providing a variable bias map including a plurality of bias entries {circumflex over (B)}_j with each particular one of the plurality of bias entries {circumflex over (B)}_j associated with a particular one of the plurality of patches p_j;

c. providing a vector z_i of inputs from a plurality of sensors for a given time step i;

d. determining the patch p_j to which the vector z_i from the plurality of sensors applies;

e. combining the vector z_i with the bias entry {circumflex over (B)}_j corresponding to the patch p_j to which the input z_i from the plurality of sensors applies to provide a bias adjusted sensor input;

f. providing a state estimator to receive the bias adjusted sensor input and to produce a system state estimation {circumflex over (x)}_i corresponding to the time step i;

g. using a combination of the bias adjusted sensor input and the system state estimation {circumflex over (x)}_i to adjust the bias entry {circumflex over (B)}_j of the variable bias map corresponding to the patch p_j to which the input z_i from the plurality of sensors applies; and

h. repeating steps c through g for each time step i to provide a continual system update.

2. A method for dynamic autocalibration of a multi-sensor tracking system as set forth in claim 1, wherein the bias map includes a constant bias offset, and further including, before step c, the steps of:

a. obtaining a calibration bias pair including an externally specified calibration bias {circumflex over (B)} and a corresponding system state calibration value x*; and

b. applying the calibration bias pair to the variable bias map to eliminate the constant bias offset.

3. A method for dynamic autocalibration of a multi-sensor tracking system as set forth in claim 2, wherein a learning rate γ

′

is determined prior to the beginning of the repitition of steps c through g, and wherein the adjustment of the variable bias map is performed by means of gradient descent utilizing the learning rate γ

′ and

the combination of the bias adjusted sensor input and the system state estimate {circumflex over (x)}_i.

4. A method for dynamic autocalibration of a multi-sensor tracking system as set forth in claim 3, wherein the next state of the system x_i+1, corresponding to the next time step i+1, is modeled by

5. A method for dynamic autocalibration of a multi-sensor tracking system as set forth in claim 3, wherein the gradient descent is performed parametrically.

6. A method for dynamic autocalibration of a multi-sensor tracking system as set forth in claim 4, wherein the vector z_i from the plurality of sensors depends on the system state x_i corresponding to the time step i by the following relationship,

7. A method for dynamic autocalibration of a multi-sensor tracking system as set forth in claim 6, wherein the calculation of the Jacobian is simplified into the following relationship, $\frac{\partial }{}$

di∂

B^j=-I,

and is used in the adjustment of the bias entries {circumflex over (B)}_j.

8. A method for dynamic autocalibration of a multi-sensor tracking system including the steps of:

a. providing a variable bias map parameterized by a plurality of weights;

b. providing a vector z_i of inputs from a plurality of sensors for a given time step i;

c. combining the vector z_i with a bias estimate {circumflex over (B)} derived from the variable bias map to provide a bias adjusted sensor input;

d. providing a state estimator to receive the bias adjusted sensor input and to produce a system state estimation {circumflex over (x)}_i corresponding to the time step i;

e. using a combination of the bias adjusted sensor input and the system state estimation {circumflex over (x)}_i to adjust the plurality of weights of the variable bias map; and

f. repeating steps b through e for each time step i to provide a continual system update.

9. A method for dynamic autocalibration of a multi-sensor tracking system as set forth in claim 8, wherein the variable bias map includes a constant bias offset, and further including, before step b, the steps of:

a. obtaining a calibration bias pair including an externally specified calibration bias {circumflex over (B)}* and a corresponding system state calibration value x*; and

b. applying the calibration bias pair to the variable bias map to eliminate the constant bias offset.

10. A method for dynamic autocalibration of a multi-sensor tracking system as set forth in claim 9, wherein a learning rate γ

′

is determined prior to the beginning of the repitition of steps c through g, and wherein the adjustment of the variable bias map is performed by means of gradient descent utilizing the learning rate γ

′ and

the combination of the bias adjusted sensor input and the system state estimate {circumflex over (x)}_i.

11. A method for dynamic autocalibration of a multi-sensor tracking system as set forth in claim 10, wherein the variable bias map includes a plurality l of Gaussian fuzzy sets W_l, and the bias estimate {circumflex over (B)} is defined by $\hat{B}$

(x^l)=∑

l=1N

 



Wl



-

x^i-cl

2σ

W2

where N represents the total number of Gaussian fuzzy sets W_l in the bias map, σ

_W² defines the widths of the Gaussian fuzzy sets W_l, l is an indexing variable, and c_l defines the center of the lth Gaussian fuzzy set W_l.

12. A method for dynamic autocalibration of a multi-sensor tracking system as set forth in claim 11, wherein c_l=10l, σ

=20, N=36, and ∥

{circumflex over (x)}_i−

c_l∥

²=({circumflex over (x)}_i−

10l)².

13. A method for dynamic autocalibration of a multi-sensor tracking system as set forth in claim 12 wherein the vector z_i of inputs from a plurality of sensors is from a sensor system including at least one gyroscope, at least one compass, and at least one tilt sensor.

14. A dynamic autocalibration system for a multi-sensor tracking system as set forth in claim 13, wherein the at least one gyroscope, at least one compass, and at least one tilt sensor are utilized as part of a head-mounted orientation tracker.

15. A dynamic autocalibration system for a multi-sensor tracking system operating in a system'"'"'s state space which is divided into a plurality of patches p_j, and operative to receive sensor input in the form of a vector z_i from a plurality of sensors where i represents a particular time step, the dynamic autocalibration system including:

a. a variable bias map including a plurality of bias entries {circumflex over (B)}_j with each particular one of the plurality of bias entries {circumflex over (B)}_j associated with a particular one of the plurality of patches p_j, said variable bias map operative to receive a system state estimation {circumflex over (x)}_i and to determine an associated patch p_j and the corresponding bias entry {circumflex over (B)}_j;

b. a first comparator operative to receive the vector z_i from the plurality of sensors and a bias entry {circumflex over (B)}_j from the bias map and to subtract the bias entry {circumflex over (B)}_j from the vector z₁, resulting in the difference z_i−

{circumflex over (B)}_j;

c. a system state estimator operatively connected with the first comparator to receive the difference z_i−

{circumflex over (B)}_j to provide a system state estimation {circumflex over (x)}_i to the bias map;

d. a constant matrix H multiplier including a constant matrix H for mapping state values onto the system state x_i, which is connected to receive the system state estimation {circumflex over (x)}_i and multiply it by the constant matrix H, resulting in a constant matrix H multiplied system state estimate H{circumflex over (x)}_i;

e. a second comparator connected to receive the constant matrix H multiplied system state estimate H{circumflex over (x)}_i from the constant matrix H multiplier and the difference z_i−

{circumflex over (B)}_j from the first comparator and to subtract the constant matrix H multiplied system state estimate H{circumflex over (x)}_i from the difference z_i−

{circumflex over (B)}_j, resulting in d_i=(z_i−

{circumflex over (B)}_j)−

(H{circumflex over (x)}_i);

f. an error minimizer connected to receive d_i=(z_i−

{circumflex over (B)}_j)−

(H{circumflex over (x)}_i) from the second comparator and the particular bias entry {circumflex over (B)}_j associated with the particular patch p_j to which the current system state estimation {circumflex over (x)}_i applies and to adjust the particular bias entry {circumflex over (B)}_j.

16. A dynamic autocalibration system for a multi-sensor tracking system as set forth in claim 15, wherein the bias map includes a constant bias offset and is operative to receive a calibration bias pair including an externally including an externally specified calibration bias {circumflex over (B)} and a corresponding system state calibration value x*, and to utilize calibration bias pair to eliminate the constant bias offset.

17. A dynamic autocalibration system for a multi-sensor tracking system as set forth in claim 16 wherein the error minimizer includes a learning rate γ

′ and

adjusts the variable bias map by means of gradient descent, utilizing the learning rate γ

′

.

18. A dynamic autocalibration system for a multi-sensor tracking system as set forth in claim 17, wherein the estimator further includes a memory accessible by the error minimizer, and the next state of the system x_i+1, corresponding to the next time step i+1, is represented by

19. A dynamic autocalibration system for a multi-sensor tracking system as set forth in claim 17, wherein the gradient descent is performed parametrically.

20. A dynamic autocalibration system for a multi-sensor tracking system as set forth in claim 18, wherein the vector z_i from the plurality of sensors depends on the system state x_i corresponding to the time step i by the following relationship,

21. A dynamic autocalibration system for a multi-sensor tracking system as set forth in claim 18, wherein the calculation of the Jacobian in the error minimizer is simplified into the following relationship, $\frac{\partial }{}$

di∂

B^j=-I,

and is used in the adjustment of the bias entries {circumflex over (B)}_j.

22. A dynamic autocalibration system for a multi-sensor tracking system operating in a system'"'"'s state space, and operative to receive sensor input in the form of a vector z_i from a plurality of sensors where i represents a particular time step, the dynamic autocalibration system including:

a. a variable bias map parameterized by a plurality of weights;

b. a first comparator operative to receive the vector z_i from the plurality of sensors and a bias entry {circumflex over (B)}_i for the time step i from the bias map and to subtract the bias entry {circumflex over (B)}_i from the vector z_i, resulting in the difference z_i−

{circumflex over (B)}_i;

c. a system state estimator operatively connected first comparator to receive the difference z_i−

{circumflex over (B)}_i to provide a system state estimation {circumflex over (x)}_i to the bias map;

d. a constant matrix H multiplier including a constant matrix H for mapping state values onto the system state x_i, which is connected to receive the system state estimation {circumflex over (x)}_i and multiply it by the constant matrix H, resulting in a constant matrix H multiplied system state estimate H{circumflex over (x)}_i;

e. a second comparator connected to receive the constant matrix H multiplied system state estimate H{circumflex over (x)}_i from the constant matrix H multiplier and the difference z_i−

{circumflex over (B)}_i from the first comparator and to subtract the constant matrix H multiplied system state estimate H{circumflex over (x)}_i from the difference z_i−

{circumflex over (B)}_i, resulting in d_i=(z_i−

{circumflex over (B)}_i)−

(H{circumflex over (x)}_i);

f. an error minimizer connected to receive d_i=(z_i−

{circumflex over (B)}_i)−

(H{circumflex over (x)}_i) from the second comparator and the particular bias entry {circumflex over (B)}_i and to adjust the particular bias entry {circumflex over (B)}_j.

23. A dynamic autocalibration system for a multi-sensor tracking system as set forth in claim 22, wherein the bias map includes a constant bias offset and is operative to receive a calibration bias pair including an externally including an externally specified calibration bias {circumflex over (B)}* and a corresponding system state calibration value x*, and to utilize the calibration bias pair eliminate the constant bias offset.

24. A dynamic autocalibration system for a multi-sensor tracking system as set forth in claim 23 wherein the error minimizer includes a learning rate γ

′ and

adjusts the variable bias map by means of gradient descent, utilizing the learning rate γ

′

.

25. A dynamic autocalibration system for a multi-sensor tracking system as set forth in claim 24, wherein the variable bias map includes a plurality l of Gaussian fuzzy sets, and the bias estimate {circumflex over (B)} is defined by $\hat{B}$

(x^i)=∑

l=0N

 



Wl



-





x^i-cl





2σ

W2

where N represents the total number of Gaussian fuzzy sets W_l in the bias map, σ

_W² defines the widths of the Gaussian fuzzy sets W_l, l is an indexing variable, and c_l defines the center of the lth Gaussian fuzzy set W_l.

26. A dynamic autocalibration system for a multi-sensor tracking system as set forth in claim 25, wherein c_l=10l, σ

=20, N=36, and ∥

{circumflex over (x)}_i−

c_l∥

²=({circumflex over (x)}_i−

10l)².

27. A dynamic autocalibration system for a multi-sensor tracking system as set forth in claim 26 wherein the vector z_i of inputs from a plurality of sensors is from a sensor system including at least one gyroscope, at least one compass, and at least one tilt sensor.

28. A dynamic autocalibration system for a multi-sensor tracking system as set forth in claim 27, wherein the at least one gyroscope, at least one compass, and at least one tilt sensor are utilized as part of a head-mounted orientation tracker.