`
`(12) United States Patent
`Foxlin
`
`(10) Patent N0.:
`(45) Date of Patent:
`
`US 7,725,253 B2
`*May 25, 2010
`
`(54)
`
`(75)
`
`(73)
`
`TRACKING, AUTO-CALIBRATION, AND
`MAP-BUILDING SYSTEM
`
`Inventor:
`
`Eric FoXlin, Arlington, MA (US)
`
`Assignee: InterSense, Inc., Billerica, MA (US)
`
`(*)
`
`Notice:
`
`Subject to any disclaimer, the term of this
`patent is extended or adjusted under 35
`U.S.C. 154(b) by 1293 days.
`
`This patent is subject to a terminal dis
`claimer.
`
`(21)
`
`(22)
`
`(65)
`
`(63)
`
`(60)
`
`(51)
`
`(52)
`
`(58)
`
`(56)
`
`Appl. No.:
`
`11/147,688
`
`Filed:
`
`Jun. 8, 2005
`
`Prior Publication Data
`
`US 2006/0027404 A1
`
`Feb. 9, 2006
`
`Related US. Application Data
`
`Continuation of application No. 10/639,242, ?led on
`Aug. 11, 2003, noW Pat. No. 6,922,632.
`Provisional application No. 60/402,178, ?led on Aug.
`9, 2002.
`
`Int. Cl.
`(2006.01)
`G01C 21/00
`US. Cl. ....................... .. 701/207; 701/36; 701/220;
`701/300; 342/357.07
`Field of Classi?cation Search ................. .. 701/33,
`701/36, 207, 220, 222, 225, 300; 340/357.01,
`340/357.08
`See application ?le for complete search history.
`
`References Cited
`
`U.S. PATENT DOCUMENTS
`
`5,615,132 A
`6,176,837 B1
`6,288,785 B1
`6,611,141 B1
`6,922,632 B2*
`
`3/1997
`1/2001
`9/2001
`8/2003
`7/2005
`
`Horton et al.
`FoXlin
`FrantZ et a1.
`Schultz et a1.
`FoXlin ...................... .. 701/207
`
`2002/0052674 A1
`
`5/2002 Chang et al.
`
`FOREIGN PATENT DOCUMENTS
`
`W0
`
`WO 01/80736
`
`11/2001
`
`OTHER PUBLICATIONS
`
`William Frey, Michael Zyda, Robert McGhee, Bill Cockayne, Off
`the-Shelf, Real-Time, Human Body Motion Capture for Synthetic
`Environments, 1995, Computer Science Department, Navel Post
`graduate School, Monterey, CA 93943-5118.
`“IEEE Standard for a Smart Transducer Interface for Sensors and
`ActuatorsiTransducer to Microprocessor Communication Proto
`cols and Transducer Electronic Data Sheet (TEDS) Format”. Institute
`of Electrical and Electronics Engineers, Inc., NeWYork, NY. Sep. 25,
`1998.
`
`(Continued)
`Primary ExamineriGertrude Arthur Jeanglaud
`(74) Attorney, Agent, or FirmiFish & Richardson PC.
`
`(57)
`
`ABSTRACT
`
`A navigation or motion tracking system includes components
`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 components indepen
`dently 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 com
`ponents may be dynamically incorporated into the system and
`the tracking component is then automatically con?gured to
`take advantage of measurements from the corresponding sen
`sors Without having to modify the tracking component.
`
`9 Claims, 12 Drawing Sheets
`
`110 §
`
`N-Frame
`
`META 1003
`META V. THALES
`
`
`
`US 7,725,253 B2
`Page 2
`
`OTHER PUBLICATIONS
`Neal A. Carlson. “Federated Filter for Fault-Tolerant Integrated
`Navigation”. NATO Advisory Group for Aerospace Research and
`Development (AGARD) book “Aerospace Navigation Systems”,
`AGARD-AG-33l, published Jun. 1995, pp. 265-280.
`V. A. Tupysev. “A Generalized Approach to the Problem of Distrib
`uted Kalman Filtering”. AIAA-98-4309, pp. 1097-1 1 16, Aug. 1998.
`
`Donald T. Knight. “Rapid Development of Tightly-Coupled GPS/
`INS Systems”. IEEE, 1977.
`Gudrun Klinker et al. “Distributed User Tracking Concepts for Aug
`mented Reality Applications”. IEEE, pp. 37-44, 2000.
`
`* cited by examiner
`
`META 1003
`META V. THALES
`
`
`
`US. Patent
`
`May 25, 2010
`
`Sheet 1 0f 12
`
`US 7,725,253 B2
`
`FIG. 1
`
`META 1003
`META V. THALES
`
`
`
`U.S. Patent
`
`May25, 2010
`
`Sheet 2 of 12
`
`US 7,725,253 B2
`
`O61
`
`JUSWUOIIAUZ||aJoIUs/
`dedew
`
`
`sinpoyyayepdyr)aye3S
`
`
`9J05uolsn-jJOSUaS
`
`
`WulfBuissaoo/gejeq
`
`SPl4sr
`
`9}E}S
`
`¢Olas
`
`-B]O\\
`
`J8AUQG
`
`s90INeq3Sd
`
`SOL
`
`NWI
`
`COL
`
`META 1003
`META V. THALES
`
`META 1003
`META V. THALES
`
`
`
`
`
`
`U.S. Patent
`
`May 25, 2010
`
`Sheet 3 of 12
`
`US 7,725,253 B2
`
`200
`
`214
`
`222
`
`VehicleLocation
`
`DRU
`
`IMU
`
`104
`
`FIG.3
`
`META 1003
`META V. THALES
`
`D
`oO
`> 2
`=L
`Oa
`Oa
`
` Environment Sensor
`VehicleState
`ProcessingModule
`105
`
`Meta-Driver
`
`309
`
`122
`
`120 |1308
`
`105
`
`META 1003
`META V. THALES
`
`
`
`US. Patent
`
`May 25, 2010
`
`Sheet 4 0f 12
`
`US 7,725,253 B2
`
`v .QE
`
`META 1003
`META V. THALES
`
`
`
`US. Patent
`
`May 25, 2010
`
`Sheet 5 0f 12
`
`US 7,725,253 B2
`
`
`
`Em :5 "a. 1 _ 128
`
`E 22 x2 “ “_ 62mm
`hung 2km XKm “ A 9K O u 2w mw w wow! u \
`
`xw 6.6 n
`
`m .mvE
`
`> £5 _
`x 3% A \ :_>__ n
`8&1 2:) u
`
`SE , v hwmmcmé 66 ,v 1" vEmEmSwmmé
`
`
`
`
`< 1,; x: " \Sm
`
`AEEAu 2 £614 hwiww
`
`\_
`
`08 _
`
`_
`
`........................................................ l.
`
`65 22a: 65 am 8?:
`
`81 En; m 5/ “ KNNF
`m?vmw hmmmcm? Qm_>_ At T vow m EmEcogcm m "E _
`
`
`
`
`r0; "
`
`................................................... i1---“
`
`, 8N
`
`
`
`m m m 65 m,wm,.€m&m§ 3_>__>_\l "2924682
`
`
`
`
`
`m5 /_
`
`_
`
`v A v E53
`
`META 1003
`META V. THALES
`
`
`
`US. Patent
`
`May 25, 2010
`
`Sheet 6 0f 12
`
`US 7,725,253 B2
`
`P_SE
`Drlver
`
`Meta-
`Drlver
`
`MMU
`
`Update
`Filter
`
`120
`/
`
`122
`/ ‘ “gym
`\N.\$“\\$\
`
`/304 306\
`600
`
`615
`
`,
`
`Z
`St’ R’ H11’ Hk , Hk
`S
`t [Compute
`\ ), ‘:58:
`A
`6207 185%
`t , S’
`622
`x send we)
`m], it
`enl
`place H
`easul'em
`X’
`request m
`H?
`625
`m stack
`with (Li)
`indicator
`
`,
`,
`/‘/<eg
`(pre - (S, t), Z’
`
`635
`
`VIOUS measst Urement)
`
`After a
`number
`of cycles
`
`—
`640] (-, ,
`Find (3’, t’):
`f; U’), 621-,’ -, R
`(i’,j’) on stack
`X’ Hmj', H] ’,
`compute
`?
`5Z=Z-/Z\
`
`\ (s, 0,2 M
`
`645
`
`FIG. 6
`
`META 1003
`META V. THALES
`
`
`
`U.S. Patent
`
`May25, 2010
`
`Sheet 7 of 12
`
`US 7,725,253 B2
`
`jeuoiBay
`
`JeINJONISeYU|
`
`
`
`(9x9")JOAUGMAH
`
`ByO"MMH
`
`CIS
`
`
`
`
`
`+ols(38d)[ss]frasdfasd[asd
`Olyae
`Byo'sasqdAxejesynejopNU,
`
`Bjo'sasqapaiyanyinejap
`
`zep-7LOHEIPeWMHjJeudIBay|}--(ax9")bpuoneoyddy
`
`
`oyL_2eienbios||[Beveasts|
`Bjo'saqsdAxejegojne+]|Byo'sqsqejoiyepojne
`
`
`P'NNN“I3SIP"LOOMASS
`~--------;IIP"NE8USYS
`!|
`IIP"EOIN}zz)
`
`
`
`[EaueZIP
`IP"COOMGSS
`||ByouegsAs
`"BSIANS
`IIP'SIODJS|}[IP'BIODIS
`
` oo,[IPLOOMASKZ,
`Sd]|3Sdh
`
`
`META 1003
`META V. THALES
`
`META 1003
`META V. THALES
`
`
`
`
`
`
`US. Patent
`
`May 25, 2010
`
`Sheet 8 0f 12
`
`US 7,725,253 B2
`
`Root
`
`Bin
`
`sfCore.d||.
`sfShelLdll
`sfMeta.dl|
`sfDrvO01.dll
`sfDrv002.d||
`
`sfDrvNNN.d||
`
`Cfg
`
`HW.cfg
`lastGood HW.cfg
`sysPar.cfg
`'gpeRulescfg
`river02.DLL
`
`Station1
`
`defaultVehiclePSEs.cfg
`defaultGalaxyPSEs.cfg
`autoVehiclePSEs.cfg
`autoGa|axyPSEs.cfg
`
`Stati0n2
`
`defaultVehiclePSEs.cfg
`defaultGalaxyPSEs.cfg
`autoVehiclePSEs.cfg
`autoGalaxyPSEs.cfg
`
`FIG. 8
`
`META 1003
`META V. THALES
`
`
`
`US. Patent
`
`May 25, 2010
`
`Sheet 9 0f 12
`
`US 7,725,253 B2
`
`C Start cfgMgr.exe km“
`432\
`i
`Has HW.cfg changed
`(from last GoodHW.cfg)?
`434\
`yes i
`Were any new lines added? no
`
`n0
`
`{430
`
`Do new lines match inactive yes
`PSE lines in either default file?
`440\
`no‘
`is there enough info in the new
`lines to automatically assign
`them to vehicle or galaxy?
`
`yes
`
`ll
`
`Re-active those line
`in default and auto files
`where they appear
`
`452
`
`HW and abort
`
`Open a GUI to_ ask user to
`assign new devices and fill in
`missing pose or other info.
`444\
`F
`Copy new lines to default
`VehiclePSEs.cfg or
`defaultGalaxyPSEs.cfg
`446\
`6‘
`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
`450\
`i‘
`_
`Mitigate’; 23:23.???’ no
`auto ?les to track?
`yes @
`454x
`Copy HW.cfg to lastgoodHWcfg
`456\
`F
`(Return contol to sfc.ex@
`
`FIG, 9
`
`META 1003
`META V. THALES
`
`
`
`U.S. Patent
`
`May 25, 2010
`
`Sheet 10 of 12
`
`US 7,725,253 B2
`
`uOIeIOUODAY
`deyjeuciboy
`Ja|npeyos
`jeucibay
`[BINJONISeIU|
`
`sJeEMpJeH
`
`JO}
`
`JOWQOV
`
`JOA
`
`SIBAUGQMHcBPIYaA
`
`SIOAUGMH|alyar
`
`OlSls
`
`META 1003
`META V. THALES
`
`META 1003
`META V. THALES
`
`
`
`
`
`
`
`US. Patent
`
`May 25, 2010
`
`Sheet 11 0112
`
`US 7,725,253 B2
`
`META 1003
`META V. THALES
`
`
`
`U.S. Patent
`
`May25, 2010
`
`Sheet 12 of 12
`
`US 7
`
`5725,253 B2
`
`XXchpesn4g
`
`ayepdy
`
`‘A
`
`Jayl4
`
`WyWy4qap‘xe|Jeul4aOVIS
`
`Ch
`JIS
`
`META 1003
`META V. THALES
`
`J0¥I4NVIS
`
`J9}yI4O71
`
`META 1003
`META V. THALES
`
`
`
`
`US 7,725,253 B2
`
`1
`TRACKING, AUTO-CALIBRATION, AND
`MAP-BUILDING SYSTEM
`
`CROSS-REFERENCE TO RELATED
`APPLICATIONS
`
`This application is a continuation of US. application Ser.
`No. 10/639,242, ?ledAug. 11,2003 now US. Pat. No. 6,922,
`632, Which claims the bene?t of US. Provisional Application
`No. 60/402,178, ?led Aug. 9, 2002.
`
`BACKGROUND
`
`This invention relates to tracking, navigation, pose estima
`tion, localiZation, auto-calibration, scene modeling, struc
`ture-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 (“lo
`caliZation”) 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 measure 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 characteristics that affect the mapping
`betWeen the relative pose of a sensor and a target and the
`measurement values provided by the sensor. These character
`istics 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, typi
`cally also using information about the dynamic characteris
`tics 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 com
`puter vision as structure-from-motion (SfM). The mapping
`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 navi
`gates 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 envi
`ronment. If a complete map of the terrain is not available in
`advance, the robot may ob serve 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 ultrasound
`receivers, laser range ?nders, cameras, or pattern recognition
`devices, for determining their positions relative to reference
`points in the factory environment. The locations of the refer
`
`20
`
`25
`
`30
`
`35
`
`40
`
`45
`
`50
`
`55
`
`60
`
`65
`
`2
`ence 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 dif
`ferent types of calibration parameters, such as measurement
`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 cali
`bration parameters using a common infrastructure that is used
`to determine the location of the vehicle. As With the localiZa
`tion and mapping approaches, the characteristics of the cali
`bration parameters are typically re?ected in the implementa
`tion of the Kalman Filter techniques. 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 component
`that takes advantage of information in the sensor measure
`ments. The architecture of this system enables development
`of sensor-speci?c components independently of the tracking
`component, and enables sensors and their associated compo
`nents 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 advan
`tage of measurements from the corresponding sensors With
`out 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 orienta
`tions 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, includ
`ing accepting measurement information from the sensor sub
`system, and updating the state estimate according to the
`accepted con?guration data and the accepted measurement
`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 interface
`for receiving information related to an expected scnsor mea
`surement 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 estima
`tion 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 1003
`META V. THALES
`
`
`
`US 7,725,253 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 accept
`ing information related to an actual sensor measurement.
`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 provid
`ing information characterizing a relative location of the sens
`ing elements.
`Accepting the information related to an actual sensor mea
`surement includes accepting information enabling the esti
`mation subsystem to calculate a difference betWeen the actual
`measurement and the expected measurement.
`Accepting the information related to an actual sensor mea
`surement includes accepting information for correlating mea
`surements and geometric relationships betWeen sensing ele
`ments.
`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 mea
`surement includes accepting information characterizing an
`uncertainty in the actual measurement.
`The information characterizing the uncertainty in the
`actual measurement includes parameters of a statistical dis
`tribution 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 sen
`sors.
`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.
`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 equip
`
`5
`
`20
`
`25
`
`30
`
`35
`
`40
`
`45
`
`50
`
`55
`
`60
`
`65
`
`4
`ment, a shoe, a boot, an article of clothing, a personal protec
`tive 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 track
`ing 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 informa
`tion to the estimation subsystem for localizing an object. The
`estimation subsystem is con?gured to update a location esti
`mate for the object based on con?guration data and measure
`ment 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 mod
`ules, each providing an interface for interacting With a corre
`sponding set of one or more sensing elements.
`The interface enables the sensor module to perform com
`putations independently of an implementation of the estima
`tion subsystem.
`The interface enables the estimation sub system to perform
`computations independently of an implementation of the sen
`sor 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 sen
`sor module that includes a sensor interface for communicat
`ing With a measurement sensor, and a communication inter
`face 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 inter
`face, 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 sub
`system to be con?gured based on the parameters 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.
`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
`
`META 1003
`META V. THALES
`
`
`
`US 7,725,253 B2
`
`5
`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 esti
`mate of a pose of a tracked object relative to an environment
`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 environ
`ment. The method also includes computing an estimate 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 measurement 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 observation
`matrix that characteriZes a lineariZed model of a function
`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 estimat
`ing a second value associated With a pose of the second
`sensing element relative to the ?rst sensing element, deter
`mining 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.
`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
`
`20
`
`25
`
`30
`
`35
`
`40
`
`45
`
`50
`
`55
`
`60
`
`65
`
`6
`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 ele
`ment is a target, and generating an innovation of a measure
`ment of the sensing element made by a sensorbased 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 sub
`system, each associated With a different set of one or more
`sensors. The method includes con?guring the tracking sys
`tem, 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 subsystem
`using the provided con?guration information. The method
`includes maintaining estimates of tracking parameters in the
`estimation subsystem, including repeatedly passing data
`based on the estimates of the tracking parameters from the
`estimation subsystem to one or more of the sensor modules,
`receiving from the one or more sensor modules at the estima
`tion subsystem data based on measurements 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 param
`eters in the estimation subsystem 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.
`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 a?ixed to a tracked object.
`
`META 1003
`META V. THALES
`
`
`
`US 7,725,253 B2
`
`7
`One of the components of the distributed Kalman ?lter is
`associated With a subset of sensor modules consisting of
`sensor modules Which are a?ixed 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 imple
`menting a Federated Kalman Filter.
`Providing con?guration information from the sensor mod
`ules includes providing information characterizing a type of a
`sensor associated With a sensor module.
`Providing con?guration information from the sensor mod
`ules includes providing information characterizing a position
`or an orientation of a sensor associated With a sensor module.
`Providing con?guration information from the sensor mod
`ules 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 track
`ing or navigation system, Where the sensing elements avail
`able 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 environment. 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 measure
`ment 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 naviga
`tion system include range sensors, and con?guring the esti
`mation module includes con?guring the estimation module
`so that the estimation module can process measurement infor
`mation 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 naviga
`tion system include inertial sensors, and con?guring the esti
`mation module includes con?guring the estimation module
`so that the estimation module can process measurement infor
`mation from either inside-out sensors, outside-in sensors,
`inertial sensors, or any combination of the above sensor