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

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 2 of 38
`
`1III 11111111 III 11111 11111 11111 11111 11111 11111 11111 11111 111111 1111 1111 1111
`
`US007725253B2
`
`(12) United States Patent
`Foxlin
`
`(10) Patent No.:
`(45) Date of Patent:
`
`US 7,725,253 B2
`*May 25, 2010
`
`(54) TRACKING, AUTO-CALIBRATION, AND
`MAP-BUILDING SYSTEM
`
`(75)
`
`Inventor: Eric Foxlin, Arlington, MA (US)
`
`(73) 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) Appl. No.: 11/147,688
`
`(22) Filed:
`
`Jun. 8, 2005
`
`(65)
`
`Prior Publication Data
`
`US 2006/0027404 Al
`
`Feb. 9, 2006
`
`Related U.S. Application Data
`
`2002/0052674 Al
`
`5/2002 Chang etal.
`
`FOREIGN PATENT DOCUMENTS
`
`WO
`
`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
`Actuators—Transducer 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)
`
`(63) Continuation of application No. 10/639,242, filed on
`Aug. 11,2003, now Pat. No. 6,922,632.
`
`Primary Examiner Gertrude Arthur Jeanglaud
`(74) Attorney Agent, or Firm Fish & Richardson P.C.
`
`(60) Provisional application No. 60/402,178, filed on Aug.
`9, 2002.
`
`(57)
`
`ABSTRACT
`
`(51) Int. Cl.
`(2006.01)
`GO1C 21/00
`(52) U.S. Cl. 701/207; 701/36; 701/220;
`701/300; 342/357.07
`(58) Field of Classification Search 701/33,
`701/36, 207, 220, 222, 225, 300; 340/357.01,
`340/357.08
`See application file for complete search history.
`
`(56)
`
`References Cited
`
`U.S. PATENT DOCUMENTS
`
`3/1997 Horton etal.
`5,615,132 A
`1/2001 Foxlin
`6,176,837 B1
`9/2001 Frantz etal.
`6,288,785 B1
`8/2003 Schultz et al.
`6,611,141 B1
`6,922,632 B2 * 7/2005 Foxlin 701/207
`
`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-specific 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-specific software com-
`ponents may be dynamically incorporated into the system and
`the tracking component is then automatically configured to
`take advantage of measurements from the corresponding sen-
`sors without having to modify the tracking component.
`
`9 Claims, 12 Drawing Sheets
`
`META-GNTX-00011757
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 3 of 38
`
`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-331, 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-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 Aug-
`mented Reality Applications". IEEE, pp. 37-44, 2000.
`
`* cited by examiner
`
`META-GNTX-00011758
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 4 of 38
`
`U.S. Patent
`
`May 25, 2010
`
`Sheet 1 of 12
`
`US 7,725,253 B2
`
`N-Frame
`
`106
`
`102
`
`102
`
`102
`104
`100
`
`• 104
`
`B-Frame
`
`FIG. 1
`
`META-GNTX-00011759
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 5 of 38
`
`wawa •sn
`
`ZI Jo Z WIN
`
`za csesu'L sa
`
`FIG. 2
`
`09L1.1.000-XINO-VIV\I
`
`Sensor Fusion Core
`
`State Update Module
`
`104-)
`
`IMU
`
`103
`
`Driver
`Meta-
`
`Drivers
`PSE
`
`PSE Devices
`
`r214
`
`122-
`
`120-
`
`Environment
`
`Map
`
`Vehicle
`
`Map
`
`State
`Vehicle
`
`r206
`
`r200
`
`r190
`
`204-
`
`202-
`
`Data Processing Unit
`
`105-
`
`90-4
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 6 of 38
`
`wawa •sn
`
`ZI Jo £ WIN
`
`za csesu'L sa
`
`Location
`Vehicle
`
`y-202
`
`State
`Vehicle
`
`-220
`
`DRU
`
`Sensor Processing
`
`Module
`
`222
`
`y-214
`
`Filter
`Update
`
`y-304
`
`MMU
`
`306-N,
`
`FIG. 3
`
`IMU
`
`04
`
`'""•------.__._------°'"''
`
`s"
`
`-N-310
`
`Database
`
`Driver
`
`I
`
`-T
`
`1.9L1.1.000-XINO-VIV\I
`
`,/
`
`I 1
`I ,
`1 ,
`
`I
`
`I
`r
`
`I
`
`I
`
`.111••••••
`
`Driver
`Meta-
`
`309--
`
`,
`
`,
`
` r
`
`r
`I
`
`
`PSE
`
`105--
`
`y-200
`
`Environment
`
`Map
`
`Vehicle
`
`Map
`
`204 SFC
`
`122-
`
`120---\
`
`105--
`
`PSE
`
`105-
`
`PSE 14<r ' 308-)
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 7 of 38
`
`U.S. Patent
`
`May 25, 2010
`
`Sheet 4 of 12
`
`US 7,725,253 B2
`
`c.0
`CO
`CO
`
`•
`
`IA
`U)
`0-
`ii-
`
`N--
`0 I
`x--
`I
`
`I
`I
`I
`I
`
`I
`I
`I
`
`41
`
`Co
`0
`
`/
`/
`
`I 1
`t 1
`/
`I C')
`1
`,x
`co
`• , 1
`/
` //
`i
`LLJ
`1
`/
`A CD
`1
`/
`/
`CL
`/
`I 1
`2 0) ‘ 1
`co
`/
`0 `1
`co
`/
`,-,---'
`co v-
`.,\ =
`/
`. /
`
`C
`
`Li..
`
`META-GNTX-00011762
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 8 of 38
`
`wawa •sn
`
`Z1 Jo S JamiS
`
`za cseszL`L sa
`
`-220
`
`5x
`
`DRU
`
`(665)
`
`PFx PFM PFF
`
`x PMM PMF
`
`'M
`
`ox, OF, 5M,Pxx Px M Px F
`
`306
`
`(-
`
`Update Filter
`
`(670)
`
`{Pff}
`
`{Pff}
`Map Manager
`Environment
`
`310-
`
`I
`
`352}
`
`Arbiter
`
`00
`
`r 6
`
`43F, {Pf
`
`200-,
`
`122-
`
`FIG. 5
`
`660}
`
`-104
`
`IMU
`
`6M, {P
`
`314-)
`
`Vehicle Map
`
`{P.m}
`Manager
`
`{Pmm
`
`AX
`
`-635
`
`•
`r625
`
`6z, Hx, Hmi, Hf, R (675)
`
` MMU
`r610
`605
`
`‘•
`
`£9L1.1.000-XINO-VIV\1
`
`356-)
`
`Measurement
`
`Server
`
`354-1
`
`Model Library
`
`Server
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 9 of 38
`
`U.S. Patent
`
`May 25, 2010
`
`Sheet 6 of 12
`
`US 7,725,253 B2
`
`PSE
`Driver
`
`Meta-
`Driver
`
`MMU
`
`Update
`Filter
`
`y-120
`
`y-122
`
`osy0st k(s'
`
`y-304
`
`600
`
`I
`ndicators
`of PSE
`availability
`
`605
`
`type
`Sends (s, t)/>Xst,lcs, VA,
`610
`2st, R, ,
`A' , H
`s k
`
`615
`
`(s-
`
`7-Compute
`
`',- St
`622-
`
`620-)
`Send ist, (s, t), S,
`measurement
`equest
`625
`r
`
`630
`
`(pre viotiss" rnt').: t
`uasur
`ernent)
`
`635
`
`Hinj, Hfi;
`place Hx,
`H • Hmy fi
`in stack
`with (ij)
`indicator
`
`After a
`number
`of cycles
`
`640-}
`Find (s', t')=
`(i', j') on stack
`compute
`8z=z-Z
`
`645
`
`R
`
`(s, zSt
`650
`
`FIG. 6
`
`META-GNTX-00011764
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 10 of 38
`
`wawa •sn
`
`ZI Jo L WIN
`
`za cseszL`L sa
`
`FIG. 7
`
`-404
`HW Driver (.exe)
`
`Infrastructural
`
`Regional
`
`PSE1
`
`PSE1
`
`PSE3PSE4
`
`PSE1
`
`406-N„PSE1
`
`PSE1
`
`IMU1
`
`PSE2
`
`PSE1
`
`PSE1
`
`IMU1
`408-N.-t-
`
`sfDrvNINN.d11
`
`sfDrv002.d11• •
`
`sfDrv001.d11
`
`99L1.1.000-XINO-VIV\I
`
`410
`
`HW.cfg
`
`defaultGalaxyPSEs.cfg
`defaultVehiclePSEs.cfg
`
`420
`
`autoGalaxyPSEs.cfg
`autoVehiclePSEs.cfg
`
`422
`
`416
`Regional HW MediatorK482
`
` K
`
`sfCfgMgr.exe
`
`sfMeta.d11
`
`122
`
`sfCore.d11
`
`sfCore.d11
`
`200-
`
`I Arbiter 1*
`
`y-
`
`418
`
`sfShell.d11
`
`412-N,
`
`sysPar.cfg
`
`422-N.
`
`-
`
`Application (.exe)
`
`424
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 11 of 38
`
`U.S. Patent
`
`May 25, 2010
`
`Sheet 8 of 12
`
`US 7,725,253 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
`
`Station1
`
`defaultVehiclePSEs.cfg
`defaultGalaxyPSEs.cfg
`autoVehiclePSEs.cfg
`autoGalaxyPSEs.cfg
`
`Station2
`
`defaultVehiclePSEs.cfg
`defaultGalaxyPSEs.cfg
`autoVehiclePSEs.cfg
`autoGalaxyPSEs.cfg
`
`FIG. 8
`
`META-GNTX-00011766
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 12 of 38
`
`U.S. Patent
`
`May 25, 2010
`
`Sheet 9 of 12
`
`US 7,725,253 B2
`
`oe---430
`
`438-
`Re-active those line
`in default and auto files
`where they appear
`
`431
`
`no
`
`no
`
`yes
`
`yes
`
`( Start cfgMgr.exe
`432-\
`
`Has HW.cfg changed
`(from last GoodHW.cfg)?
`434-\ yes
`Were any new lines added?
`436-
`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?
`no.
`442 --
`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
`
`446
`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
`456- .
`(Return contol to sfc.exe
`
`no
`
`452
`no Tell user to add)
`HW and abort)
`
`FIG. 9
`
`META-GNTX-00011767
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 13 of 38
`
`wawa •sn
`
`Z1 Jo 01 WIN
`
`za cseszL`L sa
`
`FIG. 10
`
`• • •
`
`Vehicle 2 Sensor Fusion Core
`
`r472
`
`Map
`V2
`
`Scheduler
`V2 Meas
`
`HW Drivers
`Vehicle 2
`
`.••••
`
`•
`•
`•
`•
`•
`•
`
`• •
`
`5.
`
`. /
`
`Reconciliation
`Regional Map
`
`Filter
`
`484
`
`•••
`
`••
`
`••••
`
`••••
`
`..
`
`..
`,
`..
`
`I
`
`, .•••
`
`..
`
`..
`
`...•
`
`..o.
`
`.....
`
`•••
`
`•••
`
`....
`
`••••
`
`..,
`
`...•
`
`I'
`
`..,
`
`...
`
`..
`
`..
`
`..
`
`••
`
`••
`
`..
`
`..
`
`.•
`
`..
`
`I ..,
`
`,
`
`99L1.1.000-XINO-VIV\I
`
`Scheduler
`V1 Meas
`476- 0, 478- 0-'
`
`HW Drivers
`Vehicle 1
`
`I.
`
`/
`/
`
`Scheduler
`Regional
`--
`
`Arbiter
`
`482
`
`I ,
`
`a/
`
`, ....
`
`.
`, ...,
`
`..
`
`••••
`
`••
`
`••
`
`
`S.
`
`•
`
`,..
`
`Infrastructural
`
`Hardware
`
`Driver
`
`480
`
`90-‘
`
`/ • /
`
`/ •
`
`/ /
`
`/ /
`
`/ /
`
`/ /
`
`•
`34
`
`/ ▪ • /
`
`Vehicle 1 Sensor Fusion Core
`
`r472
`
`Map
`VI
`
`/ •
`
`/ •
`
`S.
`
`S.
`
`/ /
`
`/ •
`
`• /
`
`•
`
`5.
`
`-••••
`
`.*
`
`4‘
`
`•
`
`•
`
`••
`
`•
`•
`•
`
`/ •
`• /
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 14 of 38
`
`U.S. Patent
`
`May 25, 2010
`
`Sheet 11 of 12
`
`US 7,725,253 B2
`
`CO
`
`LO
`
`• / \
`• , \
`• •
`I-. \
`•
`/ •
`/ • \ .1 /
`• \ /
`/
`•
`/
`
`S.
`•
`•
`•
`•
`• /
`
`,;t•
`LO
`
`CD
`
`L. 0
`
`/
`
`META-GNTX-00011769
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 15 of 38
`
`wawa •sn
`
`ZIP ZI WIN
`
`za csesu'L sa
`
`Filter
`Fusion
`
`y, I% Fused
`
`Ox PXX
`
`• 4
`
`45X, PXX
`
`LO Filter
`
`y, Pxx Fused
`
`6x/ Pxx
`
`r 366
`
`r306
`
`-xF, -FF 5 -XX
`
`13x, dF, P P P
`
`SLAM Filter
`
`Update Filter
`
`362-
`
`7, Pxx Fused
`
`Ox, Pxx
`
`FIG. 12
`
`ox, 84) Pxx) PxM5 PN04
`
`SLAC Filter
`
`OLL1.1.000-XINO-V 1V\I
`
`364-
`
`360-
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 16 of 38
`
`US 7,725,253 B2
`
`1
`TRACKING, AUTO-CALIBRATION, AND
`MAP-BUILDING SYSTEM
`
`CROSS-REFERENCE TO RELATED
`APPLICATIONS
`
`This application is a continuation of U.S. application Ser.
`No. 10/639,242, filedAug. 11, 2003 now U.S. Pat. No. 6,922,
`632, which claims the benefit of U.S. Provisional Application
`No. 60/402,178, filed 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
`specific sensors used in tracking the object.
`Some navigation systems perform simultaneous localiza-
`tion and mapping (SLAM), also known in the field of com-
`puter vision as structure-from-motion (SfM). The mapping
`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 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 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 ultrasound
`receivers, laser range finders, cameras, or pattern recognition
`devices, for determining their positions relative to reference
`points in the factory environment. The locations of the refer-
`
`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.
`5 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
`io 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-
`is tion and mapping approaches, the characteristics of the cali-
`bration parameters are typically reflected in the implementa-
`tion of the Kalman Filter techniques. Some systems combine
`localization, mapping, and auto-calibration.
`
`20
`
`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
`25 that takes advantage of information in the sensor measure-
`ments. The architecture of this system enables development
`of sensor-specific components independently of the tracking
`component, and enables sensors and their associated compo-
`nents to be added or removed without having to re-implement
`30 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 advan-
`tage of measurements from the corresponding sensors with-
`35 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-
`40 tions 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, includ-
`ing accepting measurement information from the sensor sub-
`45 system, and updating the state estimate according to the
`accepted configuration 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 sensor mea-
`33 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.
`60 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
`65 elements fixed to the object includes information related to
`position or orientation of the sensing elements relative to the
`object.
`
`so
`
`META-GNTX-00011771
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 17 of 38
`
`US 7,725,253 B2
`
`3
`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 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 configuration of two of the sensing ele-
`ments.
`Providing information related to a relative geometric con-
`figuration 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
`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 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
`reflector 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 flying object, a floating
`object, an underwater moving object, an animal, a camera, a
`sensing apparatus, a helmet, a tool, a piece of sports equip-
`
`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-
`5 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
`10 subsystem is configured to provide configuration data to the
`estimation subsystem and to provide measurement informa-
`tion to the estimation subsystem for localizing an object. The
`estimation subsystem is configured to update a location esti-
`mate for the object based on configuration data and measure-
`15 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 cone-
`20 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 subsystem 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
`30 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
`35 sensor module is configured 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.
`40 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
`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 sub-
`system to be configured based on the parameters 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
`60 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
`65 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
`
`25
`
`45
`
`5
`
`55
`
`META-GNTX-00011772
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 18 of 38
`
`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 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 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 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 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 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 estimat-
`ing a second value associated with a pose of the second
`sensing element relative to the first sensing element, deter-
`mining 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.
`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
`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
`
`15
`
`20
`
`25
`
`35
`
`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
`5 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 sensor based in part on
`10 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 configuring the tracking sys-
`tem, which includes providing configuration information
`from each of the sensor modules to the estimation subsystem
`regarding the characteristics of the sensors associated with
`the sensor module, and configuring the estimation subsystem
`using the provided configuration 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,
`30 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
`40 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 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
`50 module that is configurable 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 filter in the estimation subsystem.
`Implementing some or all of the Kalman filter includes
`updating error estimates using linearized models of the sensor
`60 system.
`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.
`65 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.
`
`45
`
`55
`
`META-GNTX-00011773
`
`

`

`Case 6:21-cv-00755-ADA Document 45-5 Filed 02/28/22 Page 19 of 38
`
`US 7,725,253 B2
`
`7
`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 imple-
`menting a Federated Kalman Filter.
`Providing configuration information from the sensor mod-
`ules includes providing information characterizing a type of a
`sensor associated with a sensor module.
`Providing configuration information from the sensor mod-
`ules includes providing information characterizing a position
`or an orientation of a sensor associated with a sensor module.
`Providing configuration 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