`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`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.
`
` 1
`
`META 1008
`META V. THALES
`
`‡
`

`

`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`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.
`
` 2
`
`META 1008
`META V. THALES
`
`‡
`

`

`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`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
`
` 3
`
`META 1008
`META V. THALES
`
`

`

`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`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)
`
` 4
`
`META 1008
`META V. THALES
`
`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 indeed
`the actual sensor measurements that distinguishes a
`SCAAT Kalman filter from a well-constrained one.
`For each sensor type s
`1·
` we define the
` measurement
`ms
`t( )
`•( )
`vector
` and corresponding measurement function
` such
`zs
`hs
`that
`
`(8)
`
`zs
`
`t,
`
`=
`
`hs
`
`(
`
`x t( ) bt ct, ,
`
`
`
`)
`
`+
`
`vs
`
`t( )
`
`.
`
`
` and the
`Note that in the “purest” SCAAT implementation
`1=
`ms
`measurements are incorporated as single scalar values. However if
`it is not possible or necessary to isolate the measurements, e.g. to
`perform autocalibration, then multi-dimensional measurements
`can be incorporated also. Guidelines presented in [47] lead to the
`following heuristic for choosing the SCAAT Kalman filter
`measurement elements (constraints):
`During each SCAAT Kalman filter measurement update
`one should observe a single sensor and source pair only.
`For example, to incorporate magnetic tracker data as an end-user,
` for the three position and four orientation (quaternion)
`7=
`ms
`
`3.1.2 Tracking Algorithm
`xˆ 0( )
` and error covariance estimate
`Given an initial state estimate
`P 0( )
`, the SCAAT algorithm proceeds similarly to a conventional
`EKF, cycling through the following steps whenever a discrete
` from some sensor (type s ) and source becomes
`measurement
`zs
`t,
`available at time
`:
`t
`d t
` since the previous estimate.
`a. Compute the time
`b. Predict the state and error covariance.
`)
`A d t(
`
`)xˆ t d t–(
`
`A d t(
`
`)P t d t–(
`)AT d t(
`c. Predict the measurement and compute the corresponding Jaco-
`bian.
`
`xˆ -
`P-
`
`=
`=
`
`) Q d t(+
`
`)
`
`(11)
`
`(12)
`
`(13)
`
`zˆ
`
`H
`
`=
`
`=
`
`hs
`
`Hs
`
`)
`
`(
`
`,
`,
`xˆ - bt ct
`(
`,
`,
`xˆ - bt ct
`
`)
`
`t( )
`
`) 1–
`
`d. Compute the Kalman gain.
`(
`P-HT HP-HT Rs
`+
`
`K
`
`=
`
`e. Compute the residual between the actual sensor measurement
` and the predicted measurement from (12).
`zs
`t,
`D z
`
`=
`
`zs
`
`zˆ–
`
`t,
`
`(14)
`
`f. Correct the predicted tracker state estimate and error covariance
`from (11).
`
`xˆ t( )
`P t( )
`
`=
`=
`
`xˆ - KD z+
`
`I KH–(
`)P-
`
`(15)
`
` 5
`
`META 1008
`META V. THALES
`
`e
`e
`h
`h
`h
`e
`e
`”
`

`

`g. Update the external orientation of equation (4) per the change
`f q y,
`(
`,
`)
` elements of the state.*
`indicated by the
`
`quaternion xˆ f[ ] xˆ q[ ] xˆ y[( , ,
`
`]
`)
`
`Da
`a ˆ
`ˆ
`
`=
`=
`
`(16)
`
`ˆ
`a ˆ
`
`=
`
`=
`
`=
`
`0
`
`(17)
`
`2·q( )
`
`h. Zero the orientation elements of the state vector.
`]
`xˆ f[ ]
`xˆ q[ ]
`xˆ y[
`The equations (11)-(17) may seem computationally complex,
`however they can be performed quite efficiently. The computations
`can be optimized to eliminate operations on matrix and vector
`elements that are known to be zero. For example, the elements of
`the Jacobian
` in (12) that correspond to the velocities in the state
`H
`xˆ t( )
` will always be zero. In addition, the matrix inverted in the
`computation of
` in (13) is of rank
` (
` for our example in
`2
`K
`ms
`section 3.4) which is smaller for a SCAAT filter than for a
`corresponding conventional EKF implementation. Finally, the
`i n c r e a s e d d a t a r a t e a l l ow s t h e u s e o f t h e s m a l l a n g l e
`q( )
`q=
`•( )
`approximations
` and
` in
` and
`sin
`cos
`1=
`hs
`•( )
`. The total per estimate computation time can therefore
`Hs
`actually be less than that of a corresponding conventional
`implementation. (We are able to execute the SCAAT filter
`computations, with the autocalibration computations discussed in
`100m s
`the next section, in approximately
` on a 200 MHz PC-
`compatible computer.)
`
`3.1.3 Discussion
`The key to the SCAAT method is the number of constraints
`provided by the measurement vector and measurement function in
`equation (8). For the 3D-tracking problem being solved, a unique
`6=
`solution requires
` non-degenerate constraints to resolve six
`C
`degrees of freedom. Because individual sensor measurements
`typically provide les