`Foxlin
`
`(10) Patent N0.:
`(45) Date of Patent:
`
`US 6,922,632 B2
`Jul. 26, 2005
`
`US006922632B2
`
`(54) TRACKING, AUTO-CALIBRATION, AND
`MAP-BUILDING SYSTEM
`
`(TEDS) Format”. Institute of Electrical and Electronics
`Engineers, Inc., NeW York, NY. Sep. 25, 1998.
`
`(75) Inventor: Eric FoXlin, Arlington, MA (US)
`
`(73)
`(*)
`
`(21)
`(22)
`(65)
`
`(60)
`
`(51)
`(52)
`
`(58)
`
`(56)
`
`Assignee: InterSense, Inc., Bedford, MA (US)
`
`Notice:
`
`Subject to any disclaimer, the term of this
`patent is extended or adjusted under 35
`U.S.C. 154(b) by 0 days.
`
`Appl. No.: 10/639,242
`Filed:
`Aug. 11, 2003
`
`Prior Publication Data
`
`US 2004/0073360 A1 Apr. 15, 2004
`
`Related US. Application Data
`Provisional application No. 60/402,178, ?led on Aug. 9,
`2002.
`
`Int. Cl.7 .............................................. .. G01C 21/26
`US. Cl. ....................... .. 701/207; 701/36; 701/220;
`701/300; 342/357.07
`Field of Search .......................... .. 701/33, 36, 207,
`701/220, 222, 225, 300, 342657.01, 357.07,
`357.08, 424
`
`References Cited
`
`U.S. PATENT DOCUMENTS
`
`1/2001 FoXlin ...................... .. 600/595
`6,176,837 B1
`6,288,785 B1 * 9/2001 FrantZ et al.
`356/614
`6,611,141 B1 * 8/2003 Schulz et al.
`324/226
`2002/0052674 A1 * 5/2002 Chang et al. ............. .. 700/300
`
`FOREIGN PATENT DOCUMENTS
`
`WO
`
`WO 01/80736
`
`11/2001
`
`OTHER PUBLICATIONS
`
`“IEEE Standard for a Smart Transducer Interface for Sen
`sors and Actuators—Transducer to Microprocessor Commu
`nication Protocols and Transducer Electronic Data Sheet
`
`Neal A. Carlson. “Federated Filter for Fault—Tolerant Inte
`grated Navigation”. NATO Advisory Group for Aerospace
`Research and Development (AGARD) book “Aerospace
`Navigation Systems”, AGARD—AG—331, published Jun.,
`1995, pp. 265—280.
`
`V. A. Tupysev. “A Generalized Approach to the Problem of
`Distributed Kalman Filtering”. AIAA—98—4309, pp.
`1097—1116, Aug., 1998.
`Donald T. Knight. “Rapid Development of Tightly—Coupled
`GPS/INS Systems”. IEEE, 1977.
`
`Gudrun Klinker et al. “Distributed User Tracking Concepts
`for Augmented Reality Applications”. IEEE, pp. 37—44,
`2000.
`
`* cited by examiner
`
`Primary Examiner—Gertrude A. Jeanglaude
`(74) Attorney, Agent, or Firm—Fish & Richardson PC.
`
`(57)
`
`ABSTRACT
`
`A navigation or motion tracking system includes compo
`nents associated With particular sensors, Which are
`decoupled from a tracking component that takes advantage
`of information in the sensor measurements. The architecture
`of this system enables development of sensor-speci?c com
`ponents independently of the tracking component, and
`enables sensors and their associated components to be added
`or removed Without having to re-implement the tracking
`component. In a softWare implementation of the system,
`sensor-speci?c softWare components may be dynamically
`incorporated into the system and the tracking component is
`then automatically con?gured to take advantage of measure
`ments from the corresponding sensors Without having to
`modify the tracking component.
`
`69 Claims, 12 Drawing Sheets
`
`B-Frame
`
`META 1001
`META V. THALES
`
`
`
`U.S. Patent
`
`Jul. 26,2005
`
`Sheet 1 0f 12
`
`US 6,922,632 B2
`
`B-Frame
`
`FIG. 1
`
`META 1001
`META V. THALES
`
`
`
`Jul. 26, 2005
`
`Sheet 2 of 12
`
`U.S. Patent
`
`O61
`
`ainpo-ayepdnya}e}S
`
`JUSWUOIIAUQ
`BjIYoA
`LAN
`
`dew
`
`dew
`
`9}EIS
`
`
`
`
`
`yup)Buissaoo0/gejeq
`
`-e}3IN
`
`JOA
`
`S99|A8qASd
`
`
`
`
`
`SOL
`
`
`
`
`
`9105)UOISN-4JOSUdS
`
`US 6,922,632 B2
`
`¢Old
`
`META 1001
`META V. THALES
`
`META 1001
`META V. THALES
`
`
`
`
`
`U.S. Patent
`
`Jul. 26,2005
`
`Sheet 3 0f 12
`
`US 6,922,632 B2
`
`META 1001
`META V. THALES
`
`
`
`U.S. Patent
`
`Jul. 26,2005
`
`Sheet 4 0f 12
`
`US 6,922,632 B2
`
`v. .QE
`
`META 1001
`META V. THALES
`
`
`
`U.S. Patent
`
`Jul. 26, 2005
`
`Sheet 5 of 12
`
`US 6,922,632 B2
`
`— ee ee ee ae ee ee ee Ow wee Oe eee Tee eee
`
`
`dA,WW,*Addd($29)WIWryOyxy‘ZQ
`
`ececeeeeaeiaeaaeaeeewewwwmewwwewreeeeeeeyrcct
`
`
`
`Joyi4ayepdn
`
`PqWa“Ma|IN“AQ“xg
`ddyWy¥Ay
` pucariq<
`“a‘|g
`Jebeueydew
`JUSWUOIAUQ
`BWaplyoA
`cay|[|
`JoBeueyj
`
`
`
`META 1001
`META V. THALES
`
`META 1001
`META V. THALES
`
`
`
`
`
`
`
`
`
`U.S. Patent
`
`Jul. 26,2005
`
`Sheet 6 0f 12
`
`US 6,922,632 B2
`
`PSE
`Driver
`
`Meta-
`Driver
`
`MMU
`
`Up_date
`Fllter
`
`/
`
`e30
`
`St’ R: H
`A’ Hk , H
`t) NI'COITIDU’EG
`28}
`6207 S’HX’
`e221"\\
`i (5e81,
`Hmj,H?;
`send st’ uremeni
`place HX,
`U651 meas
`- H
`Hmy f1
`req
`625
`‘4/
`initack
`W (Li)
`,
`(previogs ’ t’), Zét
`indicator
`635
`Sme Sur ement)
`6
`640] (1",
`Find (5', t’):
`H J > 21,1", R
`(M) on stack
`X’ Hmj', Hf,
`compute
`1
`
`Aftera
`
`number
`of cycles
`
`_
`
`650
`
`5Z=Z-'/Z\
`
`FIG. 6
`
`META 1001
`META V. THALES
`
`
`
`U.S. Patent
`
`Jul. 26,2005
`
`Sheet 7 0f 12
`
`US 6,922,632 B2
`
`N .OE
`
`META 1001
`META V. THALES
`
`
`
`U.S. Patent
`
`Jul. 26,2005
`
`Sheet 8 0f 12
`
`US 6,922,632 B2
`
`Root
`
`Bin
`
`sfCore.d|l
`sfSheILdlI
`sfMeta.dll
`sfDrvO01.dl|
`sfDrvOO2.dll
`sfDrvNNN.dll
`
`Cfg
`
`HW.cfg
`lastGoodHW.cfg
`sysPar.cfg
`'gpeRulescfg
`rive r02. DLL
`
`Station1
`
`defaultVehiclePSEs.cfg
`defaultGalaxyPSEs.cfg
`autoVehiclePSEs.cfg
`autoGalaxyPSEs.cfg
`
`Station2
`
`defau|tVehiclePSEs.cfg
`defaultGalaxyPSEs.cfg
`autoVehiclePSEs.cfg
`autoGalaxyPSEs.cfg
`
`FIG. 8
`
`META 1001
`META V. THALES
`
`
`
`U.S. Patent
`
`Jul. 26,2005
`
`Sheet 9 0f 12
`
`US 6,922,632 B2
`
`( Start cfgMgr.exe k4“
`
`{-430
`
`no
`
`Has HW.cfg changed
`(from last GoodHWcfg)?
`434-\
`yes‘
`Were any new lines added? no
`436\
`xesl
`438*
`.
`.
`DO new "."es. match inaCtiYe yes_ InRdBePaCJltIZnEQEISSttLIrrLIiS
`PSE llnes In elther default ?le?
`Where they appear
`44()\
`no{
`ls there enough info in the new
`lines to automatically assi n
`them to vehicle or galaxy.
`442\
`ng‘
`Open a GUI to ask user to
`assign new devloes and ?ll in
`missing pose or other info.
`444\
`F
`Copy new lines to default
`VehiclePSEs.cf or
`defaultGalaxyPS s.cfg
`446\
`f
`Have any lines been subtracted no
`from HW.cfg?
`yes #
`448\
`Set status inactive in default and
`auto files for an lines that were
`subtracte in HW.cfg
`
`Maserati tastes no
`auto files to track?
`454\
`yes l
`Copy HW.cfg to lastgoodHWcfg
`456\
`if
`@eturn contol to sfc.e@
`
`FIG, 9
`
`META 1001
`META V. THALES
`
`
`
`U.S. Patent
`
`Jul. 26, 2005
`
`Sheet 10 of 12
`
`US 6,922,632 B2
`
`jeuNjOnSedU|
`
`sJeMpleH
`
`JOAuq
`
`UONEIIOUONNY
`deyjeuoiBay
`Jainpayos
`jeuociBay
`
`JO}I4
`
`JOPUQUY
`
`SISAUGMAHZajolua/
`
`OLOld
`
`SPOLA
`
`Jainpayos
`SIOAUQMH|ajoua,
`
`META 1001
`META V. THALES
`
`META 1001
`META V. THALES
`
`
`
`
`
`U.S. Patent
`
`Jul. 26,2005
`
`Sheet 11 0f 12
`
`US 6,922,632 B2
`
`mom
`
`META 1001
`META V. THALES
`
`
`
`.XXydXQ
`
`pasn-j*’d‘A
`
`XX
`
`dXQ
`
`Jay14
`
`ayepdny
`uaSs—~—SSSOSCSOSSCSCSdSCSCGCS
`
`COLaPaapee]Jouwis
`WAYAX,>"wp“xg|JeHIdOWIS
`
`pesnsd‘h
`
`U.S. Patent
`
`Jul. 26, 2005
`
`Sheet 12 of 12
`
`US 6,922,632 B2
`
`cbOlas
`
`META 1001
`META V. THALES
`
`META 1001
`META V. THALES
`
`
`
`US 6,922,632 B2
`
`1
`TRACKING, AUTO-CALIBRATION, AND
`MAP-BUILDING SYSTEM
`
`CROSS-REFERENCE TO RELATED
`APPLICATIONS
`
`This application claims the bene?t of US. Provisional
`Application No. 60/402,178, ?led Aug. 9, 2002, titled
`“Localization, Auto-Calibration, and Map-Building,” the
`contents of Which are incorporated herein by reference.
`
`BACKGROUND
`This invention relates to tracking, navigation, pose
`estimation, localiZation, auto-calibration, scene modeling,
`structure-from-motion and/or map-building based on sensor
`inputs.
`Tracking or navigation systems often make use of mea
`surements from sensors to aid in determining a location
`(“localization”) or an orientation (attitude and heading) or a
`pose (position and orientation) of an object such as a person,
`a vehicle or a robot as it navigates in an environment, such
`as Within the bounds of a building. A variety of types of
`sensors are available for such systems, including sensors
`that measure a relative location betWeen a sensor and a
`target. An example of such a sensor/target combination is an
`acoustic emitter (target) and a microphone array (sensor)
`that can determine a direction of arrival of an acoustic signal
`broadcast from the emitter. Different types of sensors mea
`sure different aspects of the relative pose of a sensor and a
`target, such as a range, direction, or relative orientation.
`Different sensors may have different measurement charac
`teristics that affect the mapping betWeen the relative pose of
`a sensor and a target and the measurement values provided
`by the sensor. These characteristics can include uncertainty
`or noise characteristics of the measurement values.
`Systems have been developed that use Kalman Filtering
`techniques to incorporate information in sensor measure
`ments to track the position or orientation of an object,
`typically also using information about the dynamic charac
`teristics of the object. The implementation of such Kalman
`Filtering techniques is often complex, and typically requires
`detailed knoWledge of the measurement characteristics of
`the speci?c sensors used in tracking the object.
`Some navigation systems perform simultaneous localiZa
`tion and mapping (SLAM), also knoWn in the ?eld of
`computer vision as structure-from-motion
`The map
`ping aspect relates to determining the locations of ?xed
`landmarks or beacons in the environment While at the same
`time using sensor measurements from those ?xed landmarks
`to assist in localiZation of the object. As an example, When
`a robot navigates an uncharted territory, such as in a Mars
`rover mission, or in an underground mining or undersea
`operation, the robot may determine its location relative to
`the surrounding environment. If a complete map of the
`terrain is not available in advance, the robot may observe
`landmarks, build a map based on the landmark observations,
`and determine its location on the map that it has constructed
`so far. The landmarks may be man-made markers or natural
`features of the terrain.
`As another example, an automated factory may use robots
`to move materials and products among different locations.
`Beacons, such as ultrasound emitters, or graphic markers
`having special patterns, may be placed at various locations
`in the factory. The robots may have sensors, such as ultra
`sound receivers, laser range ?nders, cameras, or pattern
`recognition devices, for determining their positions relative
`to reference points in the factory environment. The locations
`
`10
`
`15
`
`20
`
`25
`
`30
`
`35
`
`40
`
`45
`
`55
`
`60
`
`65
`
`2
`of the reference points may not be knoWn in advance, so the
`robots may update their maps of the factory based on inputs
`from the sensors, and navigate through the factory based on
`their updated maps.
`It may be desirable to also perform automatic calibration
`of sensors during the ongoing process of localiZation of the
`object. For example, various types of sensors may have
`different types of calibration parameters, such as measure
`ment biases and scale factors. Examples of calibration
`parameters are focal lengths or distortion parameters of a
`camera lens, or alignment of a camera relative to the vehicle
`carrying it. The Kalman Filter implementation may estimate
`the sensor calibration parameters using a common infra
`structure that is used to determine the location of the vehicle.
`As With the localiZation and mapping approaches, the char
`acteristics of the calibration parameters are typically
`re?ected in the implementation of the Kalman Filter tech
`niques. Some systems combine localiZation, mapping, and
`auto-calibration.
`
`SUMMARY
`In a general aspect, the invention features a navigation or
`motion tracking system in Which components associated
`With particular sensors are decoupled from a tracking com
`ponent that takes advantage of information in the sensor
`measurements. The architecture of this system enables
`development of sensor-speci?c components independently
`of the tracking component, and enables sensors and their
`associated components to be added or removed Without
`having to re-implement the tracking component. In a soft
`Ware implementation of the system, sensor-speci?c softWare
`components may be dynamically incorporated into the sys
`tem and the tracking component is then automatically con
`?gured to take advantage of measurements from the corre
`sponding sensors Without having to modify the tracking
`component.
`In general, in one aspect, the invention features a method
`for tracking an object that includes coupling a sensor sub
`system to an estimation subsystem. The sensor subsystem
`enables measurement related to relative positions or orien
`tations of sensing elements. Con?guration data is accepted
`from the sensor subsystem, and the estimation system is
`con?gured according to the accepted con?guration data. The
`method includes repeatedly updating a state estimate,
`including accepting measurement information from the sen
`sor subsystem, and updating the state estimate according to
`the accepted con?guration data and the accepted measure
`ment data.
`This and other aspects of the invention may include one
`or more of the folloWing features.
`Coupling the sensor subsystem to the estimation sub
`system includes coupling softWare modules each associated
`With one or more of the sensing elements.
`Each of the softWare modules provides a softWare inter
`face for receiving information related to an expected sensor
`measurement and providing measurement information that
`depends on the received information.
`Each of the softWare modules implements calculations
`that are independent of a representation of the state in the
`estimation subsystem.
`The state estimate characteriZes an estimate of a location
`of the object.
`The state estimate characteriZes con?guration information
`for one or more sensing elements ?xed to the object.
`The con?guration information for the one or more sensing
`elements ?xed to the object includes information related to
`position or orientation of the sensing elements relative to the
`object.
`
`META 1001
`META V. THALES
`
`
`
`US 6,922,632 B2
`
`3
`The con?guration information for the one or more sensing
`elements ?xed to the object includes operational parameters
`for the one or more sensing elements.
`The state estimate characterizes con?guration information
`for one or more sensing elements ?xed in an environment of
`the object.
`The con?guration information for one or more sensing
`elements ?xed in the environment of the object includes a
`map of the locations of the sensing elements.
`Repeatedly updating the state further includes providing
`to the sensor subsystems information related to an eXpected
`sensor measurement, and Wherein accepting the measure
`ment information from the sensor subsystem includes
`accepting information related to an actual sensor measure
`ment.
`Providing the information related to an eXpected sensor
`measurement includes providing information related to a
`relative geometric con?guration of tWo of the sensing ele
`ments.
`Providing information related to a relative geometric
`con?guration of the tWo of the sensing elements includes
`providing information characteriZing a relative location of
`the sensing elements.
`Accepting the information related to an actual sensor
`measurement includes accepting information enabling the
`estimation subsystem to calculate a difference betWeen the
`actual measurement and the eXpected measurement.
`Accepting the information related to an actual sensor
`measurement includes accepting information for correlating
`measurements and geometric relationships betWeen sensing
`elements.
`The information for correlating measurements and geo
`metric relationships betWeen sensing elements includes a
`mapping betWeen a relative pose of the sensing elements and
`a sensor measurement.
`The mapping betWeen the relative pose of the sensing
`elements and the sensor measurement characteriZes a linear
`mapping.
`Accepting the information related to an actual sensor
`measurement includes accepting information characteriZing
`an uncertainty in the actual measurement.
`The information characteriZing the uncertainty in the
`actual measurement includes parameters of a statistical
`distribution of an error of the actual measurement.
`Repeatedly updating the state further includes selecting a
`pair of sensing elements for measurement, and providing an
`identi?cation of the selected pair to the sensing subsystem.
`Selecting the pair of sensing elements includes selecting
`the elements according to an eXpected utility of a measure
`ment associated With the elements to the updating of the
`state.
`Repeatedly updating the state further includes updating
`the state according to the accepted information related to an
`actual sensor measurement.
`Repeatedly updating the state further includes updating
`the state according to accepted measurements from inertial
`sensors.
`Updating the state estimate includes applying a Kalman
`Filter approach.
`Each of the sensing elements includes at least one of a
`sensor and a target.
`The target includes an active device that interacts With the
`sensor.
`The target includes at least one of a man-made signal
`re?ector and a natural feature of an environment.
`
`10
`
`15
`
`25
`
`35
`
`40
`
`45
`
`55
`
`65
`
`4
`The object is selected from a group consisting of a
`vehicle, a robot, a person, a part of a person, a ?ying object,
`a ?oating object, an underWater moving object, an animal, a
`camera, a sensing apparatus, a helmet, a tool, a piece of
`sports equipment, a shoe, a boot, an article of clothing, a
`personal protective equipment, and a rigid object having a
`dimension betWeen 1 nanometer to 109 meters.
`The state estimate includes information related to a posi
`tion or an orientation of the object relative to a reference
`coordinate frame.
`In general, in another aspect, the invention features a
`tracking system includes an estimation subsystem, and a
`sensor subsystem coupled to the estimation subsystem. The
`sensor subsystem is con?gured to provide con?guration data
`to the estimation subsystem and to provide measurement
`information to the estimation subsystem for localiZing an
`object. The estimation subsystem is con?gured to update a
`location estimate for the object based on con?guration data
`and measurement information accepted from the sensor
`subsystem.
`This and other aspects of the invention may include one
`or more of the folloWing features.
`The sensor subsystem includes one or more sensor
`modules, each providing an interface for interacting With a
`corresponding set of one or more sensing elements.
`The interface enables the sensor module to perform
`computations independently of an implementation of the
`estimation subsystem.
`The interface enables the estimation subsystem to perform
`computations independently of an implementation of the
`sensor modules.
`The tracking system also includes a navigation subsystem
`to navigate the object in an environment based on the
`location estimate for the object.
`In general, in another aspect, the invention features a
`sensor module that includes a sensor interface for commu
`nicating With a measurement sensor, and a communication
`interface for communication With an estimation system. The
`sensor module is con?gured to receive information related to
`an eXpected sensor measurement over the communication
`interface, receive a measurement signal over the sensor
`interface, and provide measurement information based on
`the measurement signal over the communication interface.
`This and other aspects of the invention may include one
`or more of the folloWing features.
`The sensor module is con?gured to provide information
`over the communication interface related to an uncertainty
`in the measurement information.
`The received information related to an eXpected sensor
`measurement includes a predicted pose of a sensing element
`relative to the measurement sensor.
`In general, in another aspect, the invention features a
`method that includes enumerating a set of sensing elements
`available to a tracking system that includes an estimation
`subsystem that estimates a position or orientation of an
`object, and providing parameters speci?c to the set of
`sensing elements to the tracking system to enable the
`estimation subsystem to be con?gured based on the param
`eters speci?c to the subset of sensing elements.
`This and other aspects of the invention may include one
`or more of the folloWing features.
`The method includes generating a sequence of candidates
`of pairs of sensing elements selected from the set of sensing
`elements, the sequence based on an eXpected utility of a
`measurement associated With the elements to the estimation
`subsystem.
`
`META 1001
`META V. THALES
`
`
`
`US 6,922,632 B2
`
`5
`The method includes selecting a pair of sensing elements
`from the sequence of candidates, the selected pair of sensing
`elements being ready to make a measurement at the time of
`selection of the pair or at a prede?ned time after the time of
`selection of the pair, the selected pair having a highest
`expected utility of a measurement among the sequence of
`candidates.
`The set of sensing elements includes at least one sensor
`and at least one target, the sensor making a measurement
`With respect to the target.
`The target includes a natural feature in an environment.
`In general, in another aspect, the invention features a
`method that includes computing an estimate of a pose of a
`target element relative to a sensor element based on an
`estimate of a pose of a tracked object relative to an envi
`ronment having af?xed thereto either the sensor element or
`the target element. The computing of the estimate of the pose
`of the target element relative to the sensor element is also
`based on an estimate of a pose of the af?xed element relative
`to the tracked object and the other element relative to the
`environment. The method also includes computing an esti
`mate of a measurement of the target made by the sensor
`based on the estimate of the pose of the target relative to the
`sensor, making an actual measurement of the target by using
`the sensor, computing a deviation betWeen the actual mea
`surement and the estimated measurement, and generating a
`neW estimate of the pose of the tracked object based on the
`deviation.
`This and other aspects of the invention may include one
`or more of the folloWing features.
`The method also includes computing a ?rst observation
`matrix that characteriZes a lineariZed model of a function
`relating the measurement made by the sensor to the pose of
`the target relative to the sensor.
`The method also includes computing a second observa
`tion matrix that characteriZes a lineariZed model of a func
`tion relating the pose of the target relative to the sensor to the
`estimate of the pose of the tracked object relative to the
`environment.
`The also includes computing an observation matrix that
`characteriZes a lineariZed model of a function relating the
`measurement made by the sensor to the pose of the tracked
`object relative to the environment by combining the ?rst
`observation matrix and the second observation matrix.
`In general, in another aspect, the invention features a
`method that includes estimating a ?rst value associated With
`a pose of a ?rst sensing element relative to a second sensing
`element. The ?rst sensing element is ?xed to an environment
`and the second sensing element is ?xed to an object being
`tracked, One of the ?rst and second sensing elements is a
`sensor and the other is a target. The method includes
`estimating a second value associated With a pose of the
`second sensing element relative to the ?rst sensing element,
`determining Which of the ?rst and second sensing elements
`is the sensor, and generating an innovation of a measurement
`of the target made by the sensor based on the ?rst value
`When the second sensing element is the sensor.
`This and other aspects of the invention may include one
`or more of the folloWing features.
`The method also includes generating the innovation based
`on the second value When the ?rst sensing element is the
`sensor.
`Estimating the ?rst value and estimating the second value
`are performed by a process ignorant of Which of the ?rst and
`second sensing elements is a sensor.
`
`10
`
`15
`
`25
`
`35
`
`40
`
`45
`
`55
`
`65
`
`6
`In general, in another aspect, the invention features a
`method that includes estimating a calibration parameter of a
`sensing element that is either a sensor or a target, the sensing
`element being ?xed either to an environment or to an object
`being tracked. The method includes determining Whether the
`sensing element is the sensor or the target, assigning the
`calibration parameter as a sensor calibration parameter When
`the sensing element is a sensor, and generating an innovation
`of a measurement of a target made by the sensing element
`based in part on the sensor calibration parameter.
`This and other aspects of the invention may include one
`or more of the folloWing features.
`The method also includes assigning the calibration param
`eter as a target calibration parameter When the sensing
`element is a target, and generating an innovation of a
`measurement of the sensing element made by a sensor based
`in part on the target calibration parameter.
`Estimating the calibration parameter is performed by a
`process ignorant of Whether the sensing element is a sensor
`or a target.
`In general, in another aspect, the invention features a
`method of using multiple sensors in a tracking system. The
`method includes providing an estimation subsystem, cou
`pling one or more sensor modules to the estimation
`subsystem, each associated With a different set of one or
`more sensors. The method includes con?guring the tracking
`system, Which includes providing con?guration information
`from each of the sensor modules to the estimation subsystem
`regarding the characteristics of the sensors associated With
`the sensor module, and con?guring the estimation sub
`system using the provided con?guration information. The
`method includes maintaining estimates of tracking param
`eters in the estimation subsystem, including repeatedly
`passing data based on the estimates of the tracking param
`eters from the estimation subsystem to one or more of the
`sensor modules, receiving from the one or more sensor
`modules at the estimation subsystem data based on mea
`surements obtained from the associated sensors, and the data
`passed to the sensor modules, and combining the data
`received from the one or more sensor modules and the
`estimates of the tracking parameters in the estimation sub
`system to update the tracking parameters.
`This and other aspects of the invention may include one
`or more of the folloWing features.
`The data passed from the estimation subsystem to one or
`more of the sensor modules includes an estimate of the pose
`of a target relative to a sensor that Was calculated by the
`estimation subsystem using an estimate of the pose of a
`tracked object relative to a frame of reference ?xed to an
`environment.
`The data passed from the estimation subsystem to one or
`more of the sensor modules does not include the estimate of
`the pose of the tracked object relative to the frame of
`reference ?xed to the environment.
`Providing the estimation subsystem includes providing a
`module that is con?gurable to use different sets of sensor
`modules coupled to it.
`Maintaining estimates of the tracking parameters in the
`estimation subsystem includes using a stochastic model in
`the estimation subsystem.
`Using a stochastic model includes implementing some or
`all of a Kalman ?lter in the estimation subsystem.
`Implementing some or all of the Kalman ?lter includes
`updating error estimates using lineariZed models of the
`sensor system.
`
`META 1001
`META V. THALES
`
`
`
`US 6,922,632 B2
`
`7
`Implementing some or all of the Kalman ?lter includes
`implementing a distributed Kalman ?lter, Wherein each of a
`plurality of components of the distributed Kalman ?lter is
`associated With a different subset of the sensor modules.
`One of the components of the distributed Kalman ?lter is
`associated With a subset of sensor modules consisting of
`sensor modules that are af?Xed to a tracked object.
`One of the components of the distributed Kalman ?lter is
`associated With a subset of sensor modules consisting of
`sensor modules Which are af?Xed to an environment.
`One of the components of the distributed Kalman ?lter is
`not associated With any sensor modules.
`Implementing the distributed Kalman ?lter includes
`implementing a Federated Kalman Filter.
`Providing con?guration information from the sensor
`modules includes providing information characteriZing a
`type of a sensor associated With a sensor module.
`Providing con?guration information from the sensor
`modules includes providing information characteriZing a
`position or an orientation of a sensor associated With a
`sensor module.
`Providing con?guration information from the sensor
`modules includes providing information characteriZing one
`or more calibration parameters of a sensor associated With a
`sensor module.
`In general, in another aspect, the invention features a
`machine-accessible medium, Which When accessed results in
`a tracking or navigation system that tracks or navigates,
`respectively, an object, performing operations that includes
`enumerating a set of sensing elements available to the
`tracking or navigation system, Where the sensing elements
`available to the tracking or navigation system includes at
`least one of an inside-out sensor and an outside-in sensor.
`The inside-out sensor is ?Xed to the object and makes
`measurements With respect to a target ?Xed to an environ
`ment. The outside-in sensor is ?Xed to the environment and
`makes measurements With respect to a target ?Xed to the
`object. The machine-accessible medium, When accessed,
`results in the tracking or navigation system con?guring an
`estimation module of the tracking or navigation system
`based on an enumeration of the set of sensing elements
`available to the tracking or navigation system so that the
`estimation module can process measurement information
`from either inside-out sensors, outside-in sensors, or a
`combination of inside-out and outside-in sensors depending
`on the sensors available. The machine-accessible medium,
`When accessed, results in the tracking or navigation system
`repeatedly updating an estimated pose of an object based on
`measurements from the set of sensing elements available to
`the tracking or navigation system.
`This and other aspects of the invention may include one
`or more of the folloWing features.
`The sensing elements available to the tracking or navi
`gation system include range sensors, and con?guring the
`estimation module includes con?guring the estimation mod
`ule so that the estimation module can process measurement
`information from either inside-out sensors, outside-in
`sensors, range sensors, or any combination of the above
`sensors.
`The sensing elements available to the tracking or navi
`gation system include inertial sensors, and con?guring the
`estimation module includes con?guring the estimation mod
`ule so that the estimation module can process measurement
`information from either inside-out sensors, outside-in
`sensors, inertial sensors, or any combination of the above
`sensors.
`
`10
`
`15
`
`25
`
`35
`
`40
`
`45
`
`55
`
`65
`
`8
`The sensing elements available to the tracking or navi
`gation system include dead reckoning sensors, and con?g
`uring the estimation module includes con?guring the esti
`mation module so that the estimation module can process
`measurement information from either inside-out sensors,
`outside-in sensors, dead reckoning sensors, or any combi
`nation of the above sensors.
`In general, in another aspect, the invention features a
`tracking or navigation method that includes receiving sensor
`con?guration information indicating a set of sensing ele
`ments available to a tracking or navigation system, and
`con?guring a data processing module of the tracking or
`navigation system based on the sensor con?guration infor
`mation to selectively perform one of (a) receiving data from
`at least one inside-out bearing sensor, and updating an
`estimated pose of an object based on data received from the
`inside-out bearing sensor, (b) receiving data from at least
`one outside-in bearing sensor, and updating an estimated
`pose of an object based on data received from the outside-in
`bearing sensor, and (c) receiving data from at least one
`inside-out bearing sensor and at least one outside-in bearing
`sensor, and updating an estimated pose of an object based on
`data received from the outside-in bearing sensor and the
`inside-out bearing sensor.
`This and other aspects of the invention may include one
`or more of the folloWing features.
`The tacking or navigation