`
`
`
`
`
`
`
`
`
`
`
`DECLARATION OF SCOTT DELMAN
`
`I, Scott Delman, am over twenty-one (21) years of age. I have never been convicted of a felony,
`and I am fully competent to make this Declaration. I declare the following to be true to the best of my
`knowledge, information, and belief:
`
`1. I am Director of Publications for the Association for Computing Machinery (“ACM”).
`
`2. Neither I nor ACM itself is being compensated for this Declaration.
`
`3. Among my responsibilities as Director of Publications, I act as a custodian of records
`for ACM’s publications.
`
`4. I make this Declaration based on my personal knowledge, information contained in the
`business records of ACM, or confirmation with other responsible ACM personnel with
`such knowledge.
`
`5. As part of its ordinary course of business, ACM publishes technical papers, including
`ACM’s conference proceedings, journals, and other full-text publications. ACM
`publications are distributed to ACM’s individual and institutional subscribers, ACM
`members, and/or ACM conference attendees. Additionally, ACM publications are
`available for public download through ACM’s Digital Library (as described further
`herein), which has existed since July 1997. Prior to the creation of the Digital Library in
`July 1997, members of the public could have obtained access to ACM publications by
`contacting ACM directly.
`
`6. As part of its ordinary course of business, since July 1997, ACM has maintained a Digital
`Library where ACM publications are available to the public, either for free or purchase,
`depending on the article. The Digital Library includes keyword search functionality
`permitting users to search for articles using keyword search terms and to filter search
`results by whether the term appears in the title, full-text, index terms, and/or abstract of the
`article. In addition, users may search by author name, and may filter search results by the
`particular type of journal, type of proceeding, and/or the date of publication.
`
`7. In addition to providing access to ACM publications, the Digital Library also indexes
`papers by other publishers. When a non-ACM publication is indexed in the Digital
`Library, the public may access “metadata” concerning the publication through the Digital
`Library but cannot access the full text of the paper; instead, a member of the public who
`wishes to obtain such a paper would have to contact the publisher of that paper directly.
`
`8. “SCAAT: incremental tracking with incomplete information” by Greg Welch and Gary
`Bishop on August 3, 1997, in Proceedings of the 24th annual conference on Computer
`graphics and interactive techniques, (SIGGRAPH '97). ACM Press/Addison-Wesley
`Publishing Co., USA, 333–344. DOI:https://doi.org/10.1145/258734.258876. Exhibit 1 is
`a true and correct copy of this paper.
`
`
`
`
`
` 1
`
`META 1019
`META V. THALES
`
`
`
`9. The conference started on August 3, 1997, and the paper would have been available to the
`conference attendees on this date.
`
`10. I hereby declare that all statements made herein of my own knowledge are true and that
`all statements made on information and belief, are believed to be true, and further that
`these statements were made with the knowledge that willful false statements and the
`like are punishable by fine or imprisonment, or both, under 18 U.S.C. § 1001.
`
` declare under penalty of perjury that the foregoing statements are true and correct.
`
`
`
` I
`
`
`
`
`Executed on: __________________
`
`
`
`
`
`
`
`
`
`
`
`________________________________
`Scott Delman, Director of Publications
`
`December 20, 2021
`
` 2
`
`META 1019
`META V. THALES
`
`
`
`
`
`
`
`
`
`
`
`Exhibit 1
`Exhibit |
`
`
`
`TA 1019
`META V. THALES
`
`META 1019
`META V. THALES
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`SCAAT: Incremental Tracking with Incomplete Information
`
`Greg Welch and Gary Bishop
`
`†
`University of North Carolina at Chapel Hill
`
`Abstract
`We present a promising new mathematical method for tracking a
`user's pose (position and orientation) for interactive computer
`graphics. The method, which is applicable to a wide variety of both
`commercial and experimental systems, improves accuracy by
`properly assimilating sequential observations, filtering sensor
`measurements, and by concurrently autocalibrating source and
`sensor devices. It facilitates user motion prediction, multisensor
`data fusion, and higher report rates with lower latency than
`previous methods.
`Tracking systems determine the user's pose by measuring
`signals from low-level hardware sensors. For reasons of physics
`and economics, most systems make multiple sequential
`measurements which are then combined to produce a single tracker
`report. For example, commercial magnetic trackers using the
`SPASYN (
`) system sequentially measure three
`Space Synchro
`magnetic vectors and then combine them mathematically to
`produce a report of the sensor pose.
`Our new approach produces tracker reports as each new low-
`level sensor measurement is made rather than waiting to form a
`complete collection of observations. Because single observations
`under-constrain the mathematical solution, we refer to our
`approach as single-constraint-at-a-time or SCAAT tracking. The
`key is that the single observations provide some information about
`the user's state, and thus can be used to incrementally improve a
`previous estimate. We recursively apply this principle,
`incorporating new sensor data as soon as it is measured. With this
`approach we are able to generate estimates more frequently, with
`less latency, and with improved accuracy. We present results from
`both an actual implementation, and from extensive simulations.
`
`CR Categories and Subject Descriptors
`: I.3.7 [Computer
`Graphics] Three-Dimensional Graphics and Realism—Virtual
`reality; I.4.4 [Image Processing] Restoration—Kalman filtering;
`I.4.8 [Image Processing] Scene Analysis—Sensor fusion; G.0
`[Mathematics of Computing] General—Numerical Analysis,
`Probability and Statistics, Mathematical Software.
`
`Additional Key Words and Phrases
`: virtual environments
`tracking, feature tracking, calibration, autocalibration, delay,
`latency, sensor fusion, Kalman filter.
`
`† CB 3175, Sitterson Hall, Chapel Hill, NC, 27599-3175
`welch@cs.unc.edu, http://www.cs.unc.edu/~welch
`gb@cs.unc.edu, http://www.cs.unc.edu/~gb
`
`1 INTRODUCTION
`The method we present requires, we believe, a fundamental change
`in the way people think about estimating a set of unknowns in
`general, and tracking for virtual environments in particular. Most
`of us have the preconceived notion that to estimate a set of
`unknowns we need as many constraints as there are degrees of
`freedom at any particular instant in time. What we present instead
`is a method to constrain the unknowns
`, continually
`over time
`
`
`refining an estimate for the solution, a single constraint at a time.
`For applications in which the constraints are provided by real-
`time observations of physical devices, e.g. through measurements
`of sensors or visual sightings of landmarks, the SCAAT method
`isolates the effects of error in individual measurements. This
`isolation can provide improved filtering as well as the ability to
`individually calibrate the respective devices or landmarks
`concurrently and continually while tracking. The method
`facilitates user motion prediction, multisensor or multiple modality
`data fusion, and in systems where the constraints can only be
`determined sequentially, it provides estimates at a higher rate and
`with lower latency than multiple-constraint (batch) approaches.
`With respect to tracking for virtual environments, we are
`currently using the SCAAT method with a new version of the UNC
`wide-area optoelectronic tracking system (section 4). The method
`could also be used by developers of commercial tracking systems
`to improve their existing systems or it could be employed by end-
`users to improve custom multiple modality hybrid systems. With
`respect to the more general problem of estimating a set of
`unknowns that are related by some set of mathematical constraints,
`one could use the method to trade estimate quality for computation
`time. For example one could incorporate individual constraints,
`one at a time, stopping when the uncertainty in the solution
`reached an acceptable level.
`1.1 Incomplete Information
`The idea that one might build a tracking system that generates a
`new estimate with each individual sensor measurement or
`
`observation is a very interesting one. After all, individual
`observations usually provide only partial information about a
`user’s complete state (pose), i.e. they are “incomplete”
`observations. For example, for a camera observing landmarks in a
`scene, only limited information is obtained from observations of
`any single landmark. In terms of control theory, a system designed
`to operate with only such incomplete measurements is
`characterized as
` because the user state cannot be
`unobservable
`observed (determined) from the measurements.
`The notion of observability can also be described in terms of
`constraints on the unknown parameters of the system being
`estimated, e.g. constraints on the unknown elements of the system
`state. Given a particular system, and the corresponding set of
`unknowns that are to be estimated, let
` be defined as the minimal
`C
`number of independent simultaneous constraints necessary to
`uniquely determine a solution, let
` be the number actually used
`N
`to generate a new estimate, and let
` be the number of
`Nind
`
`independent constraints that can be formed from the
`N
`constraints. For any
` constraints, if
` the
`C=
`N Nind
`Nind
`Nind C>
` it is
`,
`, if
`problem is
`well constrained
`over constrained
`Nind C<
`and if
` it is
`
`under-constrained. (See Figure 1.)
`
`Copyright ©1997 by the Association for Computing Machinery, Inc.
`Permission to make digital or hard copies of part or all of this work for
` personal or classroom use is granted without fee provided that copies are
`not made or distributed for profit or commercial advantage and that copies
`bear this notice and the full citation on the first page. To copy otherwise, to
`republish, to post on servers, or to distribute to lists, requires prior specific
`permission and/or a fee.
`
`META 1019
`META V. THALES
`
` 1
`
`‡
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`over-constrained
`well-constrained
`
`under-constrained
`
`SCAAT
`
`Nind C>
`
`Nind
`
`C=
`
`Nind C<
`
`Nind
`
`1=
`
`observable
`unobservable
`
`SCAAT
`
` is the minimal number of independent simultaneous
`Figure 1: SCAAT and constraints on a system of simultaneous equations.
`C
`constraints necessary to uniquely determine a solution,
` is the number of given constraints, and
` is the number of independent
`N
`Nind
`C 1>
`constraints that can be formed from the
`. (For most systems of interest
`). The conventional approach is to ensure
` and
`N
`N Nind
`Nind C‡
`, i.e. to use enough measurements to well-constrain or even over-constrain the estimate. The SCAAT approach is to employ the
`smallest number of constraints available at any one time, generally
` constraint. From this viewpoint, each SCAAT
`=
`=
`1
`N
`Nind
`estimate is severely under-constrained.
`
`1.2 Landmark Tracking
`Consider for example a system in which a single camera is used to
`observe known scene points to determine the camera position and
`orientation. In this case, the constraints provided by the
`observations are multi-dimensional: 2D image coordinates of 3D
`scene points. Given the internal camera parameters, a set of four
`known coplanar scene points, and the corresponding image
`coordinates, the camera position and orientation can be uniquely
`=
`=
`4
`determined in closed-form [16]. In other words if
`N
`C
`constraints (2D image points) are used to estimate the camera
`position and orientation, the system is completely observable. On
`N C<
`the other hand, if
` then there are multiple solutions. For
`3=
`example with only
` non-collinear points, there are up to 4
`N
`2=
`1=
`solutions. Even worse, with
` or
` points, there are
`N
`N
`infinite combinations of position and orientation that could result
`in the same camera images.
`In general, for closed-form tracking approaches, a well or
`N C‡
`over-constrained system with
` is observable, an under-
`N C<
`constrained system with
` is not. Therefore, if the individual
`observations provide only partial information, i.e. the
`measurements provide insufficient constraints, then multiple
`devices or landmarks must be excited and (or) sensed prior to
`estimating a solution. Sometimes the necessary observations can
`be obtained simultaneously, and sometimes they can not. Magnetic
`trackers such as those made by Polhemus and Ascension perform
` source excitations, each in conjunction with a
`three
`sequential
`complete sensor unit observation. And while a camera can indeed
`observe multiple landmarks simultaneously in a single image, the
`image processing to identify and locate the individual landmarks
`must be done sequentially for a single CPU system. If the
`landmarks can move independently over time, for example if they
`are artificial marks placed on the skin of an ultrasound patient for
`the purpose of landmark-based tracking [41], batch processing of
`the landmarks can reduce the effectiveness of the system. A
`SCAAT implementation might grab an image, extract a
`single
`landmark, update the estimates of both the camera
` landmark
`and
`positions, and then throw-away the image. In this way estimates
`are generated faster and with the most recent landmark
`configurations.
`
`1.3 Putting the Pieces Together
`Given a tracker that uses multiple constraints that are each
` for any one of
`individually incomplete, a
`measurement model
`incomplete constraints would be characterized as
`locally
`. Such a system must incorporate a sufficient set of
`unobservable
`these incomplete constraints so that the resulting overall system is
`observable. The corresponding aggregate measurement model can
`then be characterized as
`. Global observability
`globally observable
` or over
`. The SCAAT method
`can be obtained over
`space
`time
`adopts the latter scheme, even in some cases where the former is
`possible.
`
`2 MOTIVATION
`2.1 The Simultaneity Assumption
`Several well-known virtual environment tracking systems collect
`position and orientation constraints (sensor measurements)
`sequentially. For example, tracking systems developed by
`Polhemus and Ascension depend on sensing a sequence of
`variously polarized electromagnetic waves or fields. A system that
`facilitated simultaneous polarized excitations would be very
`difficult if not impossible to implement. Similarly both the original
`UNC optoelectronic tracking system and the newer HiBall version
`are designed to observe only one ceiling-mounted LED at a time.
`Based on the available literature [25,27,37] these systems currently
`assume (mathematically) that their sequential observations were
`collected simultaneously. We refer to this as the
`simultaneity
`. If the target remains motionless this assumption
`assumption
`introduces no error. However if the target is moving, the violation
`of the assumption introduces error.
`To put things into perspective, consider that typical arm and
`wrist motion can occur in as little as 1/2 second, with typical “fast”
`wrist tangential motion occurring at 3 meters/second [1]. For the
`current versions of the above systems such motion corresponds to
` the
`approximately 2 to 6 centimeters of translation
`throughout
`sequence of measurements required for a single estimate. For
`systems that attempt sub-millimeter accuracies, even slow motion
`occurring during a sequence of sequential measurements impacts
`the accuracy of the estimates.
`
`META 1019
`META V. THALES
`
` 2
`
`‡
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`image deflection techniques are sometimes employed in an attempt
`to address latency variability in the rendering pipeline [32,39].
`Such methods are most effective when they have access to (or
`generate) accurate motion predictions and low-latency tracker
`updates. With accurate prediction the best possible position and
`orientation information can be used to render a preliminary image.
`With fast tracker updates there is higher probability that when the
`preliminary image is ready for final deflection, recent user motion
`has been detected and incorporated into the deflection.
`With these requirements in mind, let us examine the effect of
`the measurements on the estimate latency and rate. Let
` be the
`tm
`time needed to determine one constraint, e.g. to measure a sensor
`or extract a scene landmark, let
` be the number of (sequential)
`N
`constraints used to compute a complete estimate, and let
` be the
`tc
`time needed to actually compute that estimate. Then the estimate
`latency
` and rate
` are
`te
`re
`
`
`te Ntm tc+=
`
` ,
`
`1
`-------------------- .
`Ntm tc+
`
`(1)
`
`1 t
`
`----
`e
`
`re
`
`=
`
`= N
`
` increases, equation (1) shows how
`As the number of constraints
`the estimate latency and rate increase and decrease respectively.
`For example the Polhemus Fastrak, which uses the SPASYN
`) method for determining relative position and
`(
`Space Synchro
`3=
`orientation, employs
` sequential electromagnetic
`N
`excitations and measurements per estimate [25,27,37], the original
`University of North Carolina (UNC) optoelectronic tracking
`£ £
`10 N 20
`system sequentially observed
` beacons per estimate
`[3,44], and the current UNC hybrid landmark-magnetic tracking
`system extracts (from a camera image) and then incorporates
`4=
` landmarks per update. The SCAAT method seeks to
`N
`improve the latencies and data rates of such systems by updating
`the current estimate with each new (individual) constraint, i.e. by
`at 1. In other words, it increases the estimate rate to
`fixing
`N
`
`approximately the rate that individual constraints can be obtained
`and likewise decreases the estimate latency to approximately the
`time required to obtain a single constraint, e.g. to perform a single
`measurement of a single sensor, or to extract a single landmark.
`Figure 2 illustrates the increased data rate with a timing
`diagram that compares the SPASYN (Polhemus Navigation
`Systems) magnetic position and orientation tracking system with a
`hypothetical SCAAT implementation. In contrast to the SPASYN
`system, a SCAAT implementation would generate a new estimate
`after sensing each
` excitation vector rather than waiting
`individual
`for a complete pattern.
`
`x
`
`y z
`
`x
`
`y z
`
`x
`
`y z
`
`Source Excitation
`
`Sensor Measurement
`
`SPASYN Estimate
`
`SCAAT Estimate
`
`time
`
`Figure 2: A timing diagram comparing the SPASYN
`(Polhemus Navigation Systems) magnetic position and
`orientation tracking system with a hypothetical SCAAT
`implementation.
`
`The error introduced by violation of the simultaneity
`assumption is of greatest concern perhaps when attempting any
`form of system
`. Gottschalk and Hughes note that
`autocalibration
`motion during their autocalibration procedure must be severely
`restricted in order to avoid such errors [19]. Consider that for a
`multiple-measurement system with 30 milliseconds total
`measurement time, motion would have to be restricted to
`approximately 1.5 centimeters/second to confine the translation
`(throughout a measurement sequence) to 0.5 millimeters. For
`complete autocalibration of a large (wide-area) tracking system,
`this restriction results in lengthy specialized sessions.
`2.2 Device Isolation & Autocalibration
`Knowledge about source and sensor imperfections can be used to
`improve the accuracy of tracking systems. While intrinsic sensor
`parameters can often be determined off-line, e.g. by the
`manufacturer, this is generally not the case for extrinsic
`parameters. For example it can be difficult to determine the exact
`geometric relationship between the various sensors of a hybrid
`system. Consider that the coordinate system of a magnetic sensor
`is located at some unknown location inside the sensor unit.
`Similarly the precise geometric relationship between visible
`landmarks used in a vision-based system is often difficult to
`determine. Even worse, landmark positions can change over time
`as, for example, a patient’s skin deforms with pressure from an
`ultrasound probe. In general, goals such as flexibility, ease of use,
`and lower cost, make the notion of self-calibration or
`
`autocalibration attractive.
`The general idea for autocalibration is not new. See for
`example [19,45]. However, because the SCAAT method
`isolates
`the measurements provided by each sensor or modality, the
`method provides a new and elegant means to autocalibrate
`concurrently while tracking. Because the SCAAT method isolates
`the individual measurements, or measurement dimensions,
`individual source and sensor imperfections are more easily
`identified and dealt with. Furthermore, because the simultaneity
`assumption is avoided, the motion restrictions discussed in
`section 2.1 would be removed, and autocalibration could be
`
`
`performed while concurrently tracking a target.
`The isolation enforced by the SCAAT approach can improve
`results even if the constraints are obtained simultaneously through
`multidimensional measurements. An intuitive explanation is that if
`the elements (dimensions) are corrupted by independent noise,
`then incorporating the elements independently can offer improved
`filtering over a batch or ensemble estimation scheme.
`2.3 Temporal Improvements
`or
`Per Shannon’s sampling theorem [24] the measurement
`
` frequency should be at least twice the true target motion
`sampling
`bandwidth, or an estimator may track an alias of the true motion.
`Given that common arm and head motion bandwidth specifications
`
`sampling rate should ideally
`range from 2 to 20 Hz [13,14,36], the
`be greater than 40 Hz. Furthermore, the
`rate should be as
`estimate
`high as possible so that normally-distributed white estimate error
`can be discriminated from any non-white error that might be
`observed during times of significant target dynamics, and so
`estimates will always reflect the most recent user motion.
`In addition to increasing the estimate rate, we want to reduce
`the latency associated with generating an improved estimate, thus
`reducing the overall latency between target motion and visual
`feedback in virtual environment systems [34]. If too high, such
`latency can impair adaptation and the illusion of presence [22], and
`can cause motion discomfort or sickness. Increased latency also
`contributes to problems with head-mounted display registration
`[23] and with motion prediction [4,15,29]. Finally, post-rendering
`
`META 1019
`META V. THALES
`
` 3
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`2.4 Data Fusion & Hybrid Systems
`The Kalman filter [26] has been widely used for data fusion. For
`example in navigation systems [17,30], virtual environment
`tracking systems [5,12,14], and in 3D scene modeling [20,42].
`However the SCAAT method represents a new approach to
`Kalman filter based
`
`multi-sensor data fusion. Because constraints
`are intentionally
`incorporated one at a time, one can pick and
`
`choose which ones to add, and when to add them. This means that
`information from different sensors or modalities can be woven
`together in a common, flexible, and expeditious fashion.
`Furthermore, one can use the approach to ensure that each estimate
`is computed from the most recently obtained constraint.
`Consider for a moment the UNC hybrid landmark-magnetic
`presented at SIGGRAPH 96 [41]. This system uses an off-the-shelf
`Ascension magnetic tracking system along with a vision-based
`landmark recognition system to achieve superior synthetic and real
`image registration for augmented reality assisted medical
`procedures. The vision-based component attempts to identify and
`locate multiple known landmarks in a single image before
`applying a correction to the magnetic readings. A SCAAT
`implementation would instead identify and locate only one
`landmark per update, using a new image (frame) each time. Not
`only would this approach increase the frequency of landmark-
`based correction (given the necessary image processing) but it
`would offer the added benefit that unlike the implementation
`presented in [41], no special processing would be needed for the
`cases where the number of visible landmarks falls below the
`number
` necessary to determine a complete position and
`C
`orientation solution. The SCAAT implementation would simply
`cycle through any available landmarks, one at a time. Even with
`only one visible landmark the method would continue to operate as
`usual, using the information provided by the landmark sighting to
`refine the estimate where possible, while increasing the uncertainty
`where not.
`
`3 METHOD
` (KF) in an unusual
`The SCAAT method employs a
`Kalman filter
`fashion. The Kalman filter is a mathematical procedure that
`provides an efficient computational (recursive) method for the
`least-squares estimation of a linear system. It does so in a
` fashion, predicting short-term (since the last
`predictor-corrector
`estimate) changes in the state using a
`, and then
`dynamic model
`correcting them with a measurement and a corresponding
`. The
`Kalman filter (EKF) is a
`measurement model
`extended
`
`variation of the Kalman filter that supports estimation of nonlinear
`systems, e.g. 3D position and orientation tracking systems. A basic
`introduction to the Kalman filter can be found in Chapter 1 of [31],
`while a more complete introductory discussion can be found in
`[40], which also contains some interesting historical narrative.
`More extensive references can be found in [7,18,24,28,31,46].
`The Kalman filter has been employed previously for virtual
`environment tracking estimation and prediction. For example see
`[2,5,12,14,42], and most recently [32]. In each of these cases
`however the filter was applied directly and only to the 6D pose
`estimates delivered by the off-the-shelf tracker. The SCAAT
`approach could be applied to either a hybrid system using off-the-
`shelf and/or custom trackers, or it could be employed by tracker
`developers to improve the existing systems for the end-user
`graphics community.
`In this section we describe the method in a manner that does
`not imply a specific tracking system. (In section 3.4 we present
`experimental results of a specific implementation, a SCAAT wide-
`area optoelectronic tracking system.) In section 3.1 we describe
`the method for tracking, and in section 3.2 we describe one
`possible method for concurrent autocalibration.
`
`Throughout we use the following conventions.
`=
`scalar (lower case)
`x
`general vector (lower case, arrow) indexed as x r[ ]
`=
`x
`xˆ
`filter estimate vector (lower case, hat)
`=
`matrix (capital letters) indexed as A r c,[
`=
`A
`A 1–
`matrix inverse
`=
`the identity matrix
`=
`I
`-
`matrix/vector prediction (super minus)
`=
`b T
`matrix/vector transpose (super T)
`=
`matrix/vector/scalar identifier (subscript)
`=
`
`]
`
`)
`
`=
`
`)x t( ) w d t(+
`
`)
`
`.
`
`(2)
`
`i
`E •{ }
`=
`mathematical expectation
`3.1 Tracking
`3.1.1 Main Tracker Filter
`The use of a Kalman filter requires a mathematical (state-space)
`model for the dynamics of the process to be estimated, the target
`motion in this case. While several possible dynamic models and
`associated state configurations are possible, we have found a
`simple
` model to suffice for the dynamics of our
`position-velocity
`applications. In fact we use this same form of model, with different
`parameters, for all six of the position and orientation components
`(
`
`
`x y z f q y, , , ,
`
`,
`)
`
`. Discussion of some other potential models and
`the associated trade-offs can be found in [7] pp. 415-420. Because
`d t
`our implementation is discrete with inter sample time
` we
`model the target’s dynamic motion with the following linear
`difference equation:
`
`A d t(
`
`x t d t+(
`In the standard model corresponding to equation (2), the
`n
`
`x t( )
`dimensional Kalman filter
`
` would completely
`state vector
`describe the target position and orientation at any time
`. In
`t
`practice we use a method similar to [2,6] and maintain the
`complete target orientation externally to the Kalman filter in order
`to avoid the nonlinearities associated with orientation
`x t( )
`computations. In the internal state vector
` we maintain the
`(
`,
`,
`)
`target position as the Cartesian coordinates
`, and the
`x y z
`(
`f q y,
`,
`)
`orientation as small rotations
` about the
`incremental
`(
`,
`,
`)
` axis. Externally we maintain the target orientation as the
`x y z
`a a
`(
`,
`(
`,
`,
`)
`)
`. (See [9] for
`=
`external quaternion
`w
`x
`y
`z
`discussion of quaternions.) At each filter update step, the
`(
`f q y,
`,
`)
`incremental orientations
` are factored into the external
`quaternion
`, and then zeroed as shown below. Thus the
`incremental orientations are linearized for the EKF, centered about
`zero. We maintain the derivatives of the target position and
`x t( )
`orientation internally, in the state vector
`. We maintain the
`angular velocities internally because the angular velocities behave
`like orthogonal vectors and do not exhibit the nonlinearities of the
`angles themselves. The target state is then represented by the
` element internal state vector
`12=
`n
`
`x
`
`=
`
`x y z x˙ y˙ z˙ f q y f
`
`T
`
`˙ q˙ y ˙
`
`and the four-element external orientation quaternion
`a a
`(
`,
`(
`,
`,
`)
`)
`,
`=
`
`w
`
`x
`
`y
`
`z
`
`where the time designations have been omitted for clarity.
`
`(3)
`
`(4)
`
`META 1019
`META V. THALES
`
` 4
`
`b
`a
`a
`a
`a
`a
`a
`a
`a
`
`
`)
`A d t(
` in (2) projects the
`. For our linear model, the
`
`x˙ t( )d t
`
`+
`
`(5)
`
`n·
`The
` state transition matrix
`n
`d t+
`state forward from time
` to time
`t
`t
`matrix implements the relationships
`)
`
`x t d t+(
`x t( )
`=
`
`x˙ t d t+(
`x˙ t( )
`)
`=
`and likewise for the remaining elements of (3).
`)
`1·
`w d t(
`The
` process noise vector
` in (2) is a normally-
`n
`distributed zero-mean sequence that represents the uncertainty in
`d t
`n·
`the target state over any time interval
`. The corresponding
`n
`process noise covariance matrix is given by
`Q d t(
`),
`0„(cid:238)(cid:237)(cid:236)
`0,
`
`{
`E w d t(
`
`
`
`)wT d t(
`
`e+
`
`)
`
`}
`
`=
`
`0=
`
`.
`
`(6)
`
`d t
`,
`Because our implementation is discrete with inter sample time
`we can use the transfer function method illustrated by [7] pp. 221-
`222 to compute a sampled process noise covariance matrix.
`(Because the associated random processes are presumed to be time
`stationary, we present the process noise covariance matrix as a
`d t
`function of the inter-sample duration
` only.) The non-zero
`)
`Q d t(
`elements of
` are given by
`Q d t(
`
`) i i,[
`
`]
`
`=
`
`Q d t(
`
`
`
`) i j,[
`
`]
`
`=
`
`Q d t(
`
`) j i,[
`
`]
`
`Q d t(
`
`
`
`) j j,[
`
`]
`
`=
`
`=
`
`)3
`i[ ] d t(
`------------
`3
`)2
`i[ ] d t(
`------------
`2
`i[ ] d t(
`
`)
`
`(7)
`
`{
`
`),
`
`),
`
`),
`
`,
`
`}
`
`elements, while if the manufacturer were to use the SCAAT
`3=
`implementation,
` for each 3-axis electromagnetic
`ms
`response to a single excitation. For an image-based landmark
`tracker such as [41] the measurement function would, given
`estimates of the camera pose and a single landmark location,
`transform the landmark into camera space and then project it onto
`the camera image plane. In this case
` for the 2D image
`2=
`ms
`coordinates of the landmark.
`1·
`t( )
` in (8) is a
`The
` measurement noise vector
`ms
`vs
`normally-distributed zero-mean sequence that represents any
`random error (e.g. electrical noise) in the measurement. This
`p a r a m e t e r c a n b e d e t e r m i n e d f r o m c o m p o n e n t d e s i g n
`specifications, and (or) confirmed by off-line measurement. For
`ms·
`our simulations we did both. The corresponding
`ms
`measurement noise covariance matrix is given by
`t( ),
`0,
`
`Rs
`
`0„(cid:238)(cid:237)(cid:236)
`
`0=
`
`.
`
`(9)
`
`{
`E vs
`
`
`
`t( )vsT t e+(
`
`)
`
`}
`
`=
`
`•( )
`
`hs
`
` we determine the
`
`Hs
`
`(
`
`hs
`
`
`
`x t( ) bt ct, ,
`
`
`
`) i[ ]
`
`,
`
`(10)
`
`For each measurement function
`corresponding Jacobian function
`¶ ¶
`
`x t( ) bt ct, ,
`
`(
`
`) i j,[
`]
`x j[ ]
`£ £
`i ms£ £
`. Finally, we note the use of the
` and
`where
`1
`1
`j
`n
`P t( )
`n·
` error covariance matrix
`standard (Kalman filter)
`n
`which maintains the covariance of the error in the estimated state.
`
`˛h
`
`x x˙,(
`
`
`
`),
`
`y y˙,(
`
`
`
`z z˙,(
`
`
`
`f f ˙,(
`
`
`
`q q ˙,(
`
`)
`
`(
`
`y y
`
`˙,
`
`)
`
`.
`
`for each pair
`)
`i j,(
`
`i[ ]
` in (7) are the correlation kernels of the (assumed
`The
`constant) noise sources presumed to be driving the dynamic
`model. We determined a set of values using Powell’s method, and
`then used these in both simulation and our real implementation.
`The values can be “tuned” for different dynamics, though we have
`found that the tracker works well over a broad range of values.
`The use of a Kalman filter requires not only a dynamic model
`as described above, but also a measurement model for each
`available type of measurement. The measurement model is used to
`predict the ideal noise-free response of each sensor and source
`pair, given the filter’s current estimate of the target state as in
`equations (3) and (4).
`It is the nature of the measurement models and in