Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 1 of 40
`
` Exhibit 4
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 2 of 40
`
`11111111111111011111111111111111111111111111111111111111111111110111111
`
`US006922632B2
`
`(12) United States Patent
`Foxlin
`
`(to) Patent No.: US 6,922,632 B2
`(45) Date of Patent:
`Jul. 26, 2005
`
`(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)
`
`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.
`
`(21)
`
`Appl. No.: 10/639,242
`
`(22)
`
`Filed:
`
`Aug. 11, 2003
`
`(65)
`
`Prior Publication Data
`
`US 2004/0073360 Al Apr. 15, 2004
`
`(60)
`
`(51)
`(52)
`
`(58)
`
`Related U.S. Application Data
`Provisional application No. 60/402,178, filed on Aug. 9,
`2002.
`
`Int. CI.' GO1C 21/26
`U.S. Cl. 701/207; 701/36; 701;220;
`701/300; 342/357.07
`Field of Search 701/33, 36, 207,
`701/220, 222, 225, 300; 342/357.01, 357.07,
`357.08, 424
`
`(56)
`
`References Cited
`
`U.S. PATENT DOCUMENTS
`
`1/2001 Foxlin 600/595
`6,176,837 B1
`9/2001 Frantz et al. 356/614
`6,288,785 B1
`6,611,141 B1 * 8/2003 Schulz et al. 324/226
`2002/0052674 Al * 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 P.C.
`
`(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-specific 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-specific software components may be dynamically
`incorporated into the system and the tracking component is
`then automatically configured to take advantage of measure-
`ments from the corresponding sensors without having to
`modify the tracking component.
`
`69 Claims, 12 Drawing Sheets
`
`META-GNTX-00011478
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 3 of 40
`
`U.S. Patent
`
`Jul. 26, 2005
`
`Sheet 1 of 12
`
`US 6,922,632 B2
`
`N-Frame
`
`v-108
`
`r."-- 108
`•
`
`106
`
`/I
`
`; ;
`
`1 ;
`1 ;
`1 ;
`1
`
`1
`1
`I
`
`102
`
`i
`\
`1 102
`\ 1
`104
`102,
`104 100
`
`B-Frame
`
`FIG. 1
`
`META-GNTX-00011479
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 4 of 40
`
`waled °Sifl
`
`ZI Jo Z loalIS
`
`Zff Z9`ZZ6`9 Sa
`
`FIG. 2
`
`Sensor Fusion Core
`
`State Update Module
`
`09171.1.000-XINO-V13V\I
`
`104-)
`
`IMU
`
`103
`
`Driver
`Meta-
`
`Drivers
`PSE
`
`1-111.
`
`^4
`
`PSE Devices
`
`214
`
`(-
`
`#
`
`120- 122--- ,
`
`Environment
`
`Map
`
`Vehicle
`
`Map
`
`State
`Vehicle
`
`r206
`
`r200
`
`190
`
`(-
`
`204--
`
`202-.
`
`Data Processing Unit
`
`105-
`
`90---4
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 5 of 40
`
`wawa *sea
`
`Z1 Jo £ lamIS
`
`Zff Z£9`ZZ6`9 Sa
`
`y-202
`
`State
`Vehicle
`
`-220
`
`DRU
`
`Location
`Vehicle
`
`Sensor Processing
`
`Module
`
`222
`
`y-214
`
`Filter
`Update
`
`304
`
`306-
`
`MMU
`
`FIG. 3
`
`1MU
`
`04--\
`
`Driver
`Meta-
`
`y-200
`
`Environment
`
`Map
`
`Vehicle
`
`Map
`
`SFC r 206
`
`204-
`
`122
`
`120--
`
`105-
`
`PSE
`
`105-
`
`1.917 I. 1.000-X1NO-VIV\I
`
`-N-310
`
`Database
`Driver
`
`I /
`
`309-\
`
`,,,,,,
`
`PSE
`
`105--
`
`308-)
`
`---------v /19'
`
`F-
`
`PSE
`
`,-
`
`•-
`
`---T,------..,
`
`..........e
`
`I
`
`•_______ 1
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 6 of 40
`
`U.S. Patent
`
`Jul. 26, 2005
`
`Sheet 4 of 12
`
`US 6,922,632 B2
`
`Tr
`
`0EL
`
`:
`
`META-GNTX-00011482
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 7 of 40
`
`wawa •sn
`
`Z1 Jo S lamIS
`
`Zff Z£9`ZZ6`9 Sa
`
`-220
`
`X
`
`DRU
`
`(665)
`
`FIG. 5
`
`660-)
`
`-104
`
`IMU
`
`314-)
`
`X
`
`PFx PFM PFF
`
`PMx NIA PMF
`
`6x, SF, 6M,Pxx PxM PxF
`
`6 {P
`
`Vehicle Map
`
`{mm)
`Manager
`
`{P
`
`6z, Hx, Hmi, Hfi, R (675)
`
`Update Filter
`
`306
`
`(-
`
`Pxx (670)
`
`{PO
`
`SF, {Pff}
`
`Map Manager
`Environment
`
`{Pff}
`
`310-
`
`635
`
`A
`
`625
`615
` MMU
`610
`
`(-
`
`(-
`
`00
`
`\y
`304
`
`r 6
`
`L
`
`200--
`
`C9.17 I. 1.000-XINO-VIIAI
`
`356-)
`
`Measurement
`
`Server
`
`354)
`
`Model Library
`352-)
`
`Server
`
`Arbiter
`
`122-\
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 8 of 40
`
`U.S. Patent
`
`Jul. 26, 2005
`
`Sheet 6 of 12
`
`US 6,922,632 B2
`
`PSE
`Driver
`
`Meta-
`Driver
`
`mmu
`
`Update
`Filter
`
`k(s,
`
`,y-304
`
`600
`
`rs °P SE
`
`type
`kt,
`
`y-120
`
`y-122
`
`\NistO
`
`Indic
`
`605
`
`Sends (s,
`
`st
`
`622 -
`
`Compute
`S, Hx
`H
`mj, fi•
`place H,
`HinjHfi
`in stack
`with (i,j)
`indicator
`
`(pre
`
`ioussirtn)eiutrerrient)
`
`After a
`number
`of cycles
`
`650
`
`(i •
`' "), 6
`642—}
`Find (s', t')
`(i',D on stack
`compute
`oz=z-Z
`
`645
`
`R
`
`FIG. 6
`
`META-GNTX-00011484
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 9 of 40
`
`wawa 'Si!
`
`Z1 JO L WIN
`
`Za Z£9`ZZ6`9 Sfl
`
`FIG. 7
`
`-404
`HW Driver (.exe)
`
`Infrastructural
`
`Regional
`
`PSE1
`
`PSE1
`
`PSE4
`
`PSE3
`
`PSE1
`
`PSE1
`
`406-N.
`
`PSE1
`
`IMU1
`
`t t
`
`PSE2
`
`PSE1
`
`PSE1
`
`IMU1
`408-N,____L.
`
`Set I- I- 000-X1N 0-V13 lAl
`
`sfDrvi\INN.d11
`
`• •
`
`sfDrv002.d11
`
`sfDrv001.d11
`
`- - - - -,.
`----------N-410
`HW.cfg
`
`.:
`.-
`
`t
`
`defaultGalaxyPSEs.cfg
`defaultVehiclePSEs.cfg
`
`4
`
`w
`
`420
`
`(-
`
`autoGalaxyPSEs.cfg
`autoVehiclePSEs.cfg
`
`422
`
`i
`
`sfMeta.d11
`
`t
`
`sfCore.d11
`
`sfCore.d11
`
`1
`
`200--
`
`402-N.
`
`411
`
`-416
`
` v
`
`sfCfgMgr.exe
`
`Mediatory-482
`
`Regional HW
`
`418_A Arbiter I-• ,
`
`sfShell.d11
`
`412-N.
`
`-----1
`
`,-
`
`sysPar.cfg
`
`422-N_
`
`- -
`
`Application (.exe)
`
`424-N_
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 10 of 40
`
`U.S. Patent
`
`Jul. 26, 2005
`
`Sheet 8 of 12
`
`US 6,922,632 B2
`
`Root
`
`Bin
`
`sfCore.d11
`sfShell.d11
`sfMeta.d11
`sfDrv001.d11
`sfDrv002.d11
`
`sfDryNNN.d11
`
`Cfg
`
`HW.cfg
`lastGoodHW.cfg
`sysPar.cfg
`typeRules.cfg
`Driver02.DLL
`
`4- Stationl
`
`defaultVehiclePSEs.cfg
`defaultGalaxyPSEs.cfg
`autoVehiclePSEs.cfg
`autoGalaxyPSEs.cfg
`
`Station2
`
`defaultVehiclePSEs.cfg
`defaultGalaxyPSEs.cfg
`autoVehiclePSEs.cfg
`autoGalaxyPSEs.cfg
`
`FIG. 8
`
`META-GNTX-00011486
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 11 of 40
`
`U.S. Patent
`
`Jul. 26, 2005
`
`Sheet 9 of 12
`
`US 6,922,632 B2
`
`oc-430
`
`Re-active those line
`in default and auto files
`where they appear
`
`431
`
`no
`
`no
`
`yes
`
`yes
`
`( Start cfgMgrexe
`432-\
`Has HW.cfg changed
`(from last GoodHW.cfg)?
`yes
`Were any new lines added?
`yes
`Do new lines match inactive
`PSE lines in either default file?
`440- no
`Is there enough info in the new
`lines to automatically assign
`them to vehicle or galaxy?
`442-
`no
`Open a GUI to ask user to
`assign new devices and fill in
`missing pose or other info.
`444-\
`Copy new lines to default-
`VehiclePSEs.cfg or
`defaultGalaxyPSEs.cfg
`
`Have any lines been subtracted
`from HW.cfg?
`448-
`yes
`Set status inactive in default and
`auto files for any lines that were
`subtracted in HW.cfg
`
`450-
`Are there enough active PSEs
`available in the default or
`auto files to track?
`454-
`yes.
`Copy HW.cfg to lastgoodHW.cfg
`
`no
`
`no
`
`452-\
`Tell user to add
`HW and abort}
`
`(Return contol to sfc.ex
`
`FIG. 9
`
`META-GNTX-00011487
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 12 of 40
`
`luaied 'Sill
`
`Z1 Jo OT lamIS
`
`Zff Z09`ZZ6`9 Sa
`
`FIG. 10
`
`200
`
`-200
`
`• • •
`
`Vehicle 2 Sensor Fusion Core
`
`(472
`
`Map
`V2
`
`Scheduler
`V2 Meas
`
`HW Drivers
`Vehicle 2
`
`Vehicle 1 Sensor Fusion Core
`
`r472
`
`Map
`V1
`
`Scheduler
`V1 Meas
`
`HW Drivers
`Vehicle 1
`
`476-- 0' 0-/ 474-\,,-
`
`99171. 1.000-X1NO-V-LV\I
`
`•••••
`
`•
`
`•••• • e`•
`
`**1,-
`
`•
`
`▪ •
`
`•
`
`S.
`
`.. /
`
`Sc
`
`-.
`
`..
`
`..
`
`••
`
`•
`
`•
`
`S.
`
`•
`•
`•
`
`•
`•
`
`•
`
`•
`
`•
`•
`•
`
`•
`
`./
`
`/
`
`/ • ....
`
`• /
`.."
`
`‘.
`
`.../ •
`
`/ •
`
`/ N.
`
`vir
`
` ••••••
`
`0,
`
`••••
`
`0'
`
`••••
`
`00
`
`I
`
`•••
`
`
`.•
`.0
`
`00. •••••
`
`••
`
`••
`
`•••
`
`.•
`
`00 0
`
`.••••
`
`.0
`
`•••••
`
` •••••
`
`0
`
`..
`
`Reconciliation
`Regional Map
`
`Filter
`
`484 ,
`
`Scheduler
`Regional
`
`Arbiter
`
`Infrastructural
`
`Hardware
`
`Driver
`
`482
`
`480
`
`0.
`
`•••
`
`••
`
`A/
`
`0,
`
`.0
`
`••
`
`••
`
`••
`
`••
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 13 of 40
`
`U.S. Patent
`
`Jul. 26, 2005
`
`Sheet 11 of 12
`
`US 6,922,632 B2
`
`CO
`0
`
`ss%
`
`•
`
`1
`
`•
`/ \
`....
`51.
`.... i
`.../ ....
`
`\
`14
`• \
`I
`/
`• •
`/ • \ I /
`• \ /
`/ • .., I ,
`•
`/
`
`•
`•
`
`I.
`
`• CO
`• 0
`• ln
`•
`/
`•
`_
`• ,..le‘;igt:
`— -
`
`CO
`
`1.0
`
`META-GNTX-00011489
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 14 of 40
`
`waled 'Sill
`
`Z1 JO Z1 WIN
`
`Zff Z9`ZZ6`9 Sa
`
`FIG. 12
`
`06171.1.000-XINO-V13V\I
`
`Y, Pxx Fused
`
`6x, Pxx
`
`Filter
`Fusion
`
`7, Pxx Fused
`
`8x, Pxx
`
`8x, dM, Pxx, pxm, pmm
`
`SLAC Filter
`
`364Th
`
`Pxx
`
`LO Filter
`
`360Th
`
`r366
`
`r 306
`
`7, Pxx Fused
`
`8x, Pxx
`
`6x/ dF/ PxF/ PFF/ PXX
`
`SLAM Filter
`
`362Th
`
`Update Filter
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 15 of 40
`
`US 6,922,632 B2
`
`1
`TRACKING, AUTO-CALIBRATION, AND
`MAP-BUILDING SYSTEM
`
`CROSS-REFERENCE TO RELATED
`APPLICATIONS
`
`This application claims the benefit of U.S. Provisional
`Application No. 60/402,178, filed Aug. 9, 2002, titled
`"Localization, Auto-Calibration, and Map-Building," the
`contents of which are incorporated herein by reference.
`
`5
`
`10
`
`BACKGROUND
`
`15
`
`20
`
`25
`
`30
`
`35
`
`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 specific sensors used in tracking the object.
`Some navigation systems perform simultaneous localiza-
`tion and mapping (SLAM), also known in the field of
`computer vision as structure-from-motion (SfM). The map-
`ping aspect relates to determining the locations of fixed
`landmarks or beacons in the environment while at the same
`time using sensor measurements from those fixed 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 55
`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 60
`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 finders, cameras, or pattern 65
`recognition devices, for determining their positions relative
`to reference points in the factory environment. The locations
`
`40
`
`45
`
`50
`
`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
`reflected 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-specific 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-specific software
`components may be dynamically incorporated into the sys-
`tem and the tracking component is then automatically con-
`figured 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. Configuration data is accepted
`from the sensor subsystem, and the estimation system is
`configured according to the accepted configuration 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 configuration 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 configuration information
`for one or more sensing elements fixed to the object.
`The configuration information for the one or more sensing
`elements fixed to the object includes information related to
`position or orientation of the sensing elements relative to the
`object.
`
`META-GNTX-00011491
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 16 of 40
`
`US 6,922,632 B2
`
`The configuration information for the one or more sensing
`elements fixed to the object includes operational parameters
`for the one or more sensing elements.
`The state estimate characterizes configuration information
`for one or more sensing elements fixed in an environment of
`the object.
`The configuration information for one or more sensing
`elements fixed 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 configuration of two of the sensing ele-
`ments.
`Providing information related to a relative geometric
`configuration 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
`identification 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
`reflector and a natural feature of an environment.
`
`4
`The object is selected from a group consisting of a
`vehicle, a robot, a person, a part of a person, a flying object,
`a floating object, an underwater moving object, an animal, a
`camera, a sensing apparatus, a helmet, a tool, a piece of
`5 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
`10 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 configured to provide configuration data
`is to the estimation subsystem and to provide measurement
`information to the estimation subsystem for localizing an
`object. The estimation subsystem is configured to update a
`location estimate for the object based on configuration data
`and measurement information accepted from the sensor
`20 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
`25 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
`30 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 configured to receive information related to
`40 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 configured 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
`55 available to a tracking system that includes an estimation
`subsystem that estimates a position or orientation of an
`object, and providing parameters specific to the set of
`sensing elements to the tracking system to enable the
`estimation subsystem to be configured based on the param-
`60 eters specific 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
`65 elements, the sequence based on an expected utility of a
`measurement associated with the elements to the estimation
`subsystem.
`
`50
`
`META-GNTX-00011492
`
`35
`
`
`
`54
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 17 of 40
`
`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 predefined 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 affixed 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 affixed 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 first 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 first
`observation matrix and the second observation matrix.
`In general, in another aspect, the invention features a
`method that includes estimating a first value associated with
`a pose of a first sensing element relative to a second sensing
`element. The first sensing element is fixed to an environment
`and the second sensing element is fixed to an object being
`tracked, One of the first 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 first sensing element,
`determining which of the first and second sensing elements
`is the sensor, and generating an innovation of a measurement
`of the target made by the sensor based on the first 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 first sensing element is the
`sensor.
`Estimating the first value and estimating the second value
`are performed by a process ignorant of which of the first and
`second sensing elements is a sensor.
`
`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 fixed either to an environment or to an object
`5 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
`10 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
`is 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-
`25 pling one or more sensor modules to the estimation
`subsystem, each associated with a different set of one or
`more sensors. The method includes configuring the tracking
`system, which includes providing configuration information
`from each of the sensor modules to the estimation subsystem
`30 regarding the characteristics of the sensors associated with
`the sensor module, and configuring the estimation sub-
`system using the provided configuration information. The
`method includes maintaining estimates of tracking param-
`eters in the estimation subsystem, including repeatedly
`35 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
`40 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
`45 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
`50 tracked object relative to a frame of reference fixed 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 fixed to the environment.
`Providing the estimation subsystem includes providing a
`module that is configurable to use different sets of sensor
`modules coupled to it.
`Maintaining estimates of the tracking parameters in the
`60
`estimation subsystem includes using a stochastic model in
`the estimation subsystem.
`Using a stochastic model includes implementing some or
`all of a Kalman filter in the estimation subsystem.
`Implementing some or all of the Kalman filter includes
`updating error estimates using linearized models of the
`sensor system.
`
`65
`
`META-GNTX-00011493
`
`20
`
`
`
`55
`
`

`

`Case 6:21-cv-00755-ADA Document 45-4 Filed 02/28/22 Page 18 of 40
`
`US 6,922,632 B2
`
`7
`Implementing some or all of the Kalman filter includes
`implementing a distributed Kalman filter, wherein each of a
`plurality of components of the distributed Kalman filter is
`associated with a different subset of the sensor modules.
`One of the components of the distributed Kalman filter is
`associated with a subset of sensor modules consisting of
`sensor modules that are affixed to a tracked object.
`One of the components of the distributed Kalman filter is
`associated with a subset of sensor modules consisting of
`sensor modules which are affixed to an environment.
`One of the components of the distributed Kalman filter is
`not associated with any sensor modules.
`Implementing the distributed Kalman filter includes
`implementing a Federated Kalman Filter.
`Providing configuration information from the sensor
`modules includes providing information characterizing a
`type of a sensor associated with a sensor module.
`Providing configuration information from the sensor
`modules includes providing information characterizing a
`position or an orientation of a sensor associated with a
`sensor module.
`Providing configuration 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 fixed to the object and makes
`measurements with respect to a target fixed to an environ-
`ment. The outside-in sensor is fixed to the environment and
`makes measurements with respect to a target fixed to the
`object. The machine-accessible medium, when accessed,
`results in the tracking or navigation system configuring 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