`
`
`
`
`
`
`
`
`SCAAT: Incremental Tracking
`with Incomplete Information
`
`TR96-051
`October 1996
`
`Gregory F. Welch
`
`Department of Computer Science
`CB #3175, Sitterson Hall
`UNC-Chapel Hill
`Chapel Hill, NC 27599-3175
`
`UNC is an Equal Opportunity/Affirmative Action Institution
`
`META 1009
`META V. THALES
`
`

`

`
`
`
`
`
`
`
`
`
`SCAAT: Incremental Tracking
`with Incomplete Information
`
`by
`
`Gregory Francis Welch
`
`A dissertation submitted to the faculty of the University of North Carolina at Chapel Hill
`in partial fulfillment of the requirements for the degree of Doctor of Philosophy in the
`Department of Computer Science.
`
`Chapel Hill
`
`1996
`
`Approved by:
`
`Dr. Gary Bishop, Adviser
`
`Dr. Henry Fuchs
`
`Vernon Chi, Reader
`
`Dr. Anselmo Lastra
`
`Dr. Russell Taylor, Reader
`
`Dr. John Poulton
`
`META 1009
`META V. THALES
`
`

`

`
`
`© 1996
`Gregory Francis Welch
`ALL RIGHTS RESERVED
`
`ii
`
`META 1009
`META V. THALES
`
`

`

`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`ABSTRACT
`
`Gregory Francis Welch
`SCAAT: Incremental Tracking with Incomplete Information
`(Under the direction of T. Gary Bishop)
`
`The Kalman filter provides a powerful mathematical framework within which a
`
`minimum mean-square-error estimate of a user’s position and orientation can be tracked
`
`using a sequence of
`
` sensor observations, as opposed to
`single
`
`groups
`
` of observations. We
`
`refer to this new approach as
`
`single-constraint-at-a-time
`
`or SCAAT tracking. The method
`
`improves accuracy by properly assimilating sequential observations, filtering sensor
`
`measurements, and by concurrently
`
` mechanical or electrical devices. The
`autocalibrating
`
`method facilitates user motion prediction, multisensor data fusion, and in systems where
`
`the observations are only available sequentially it provides estimates at a higher rate and
`
`with lower latency than a multiple-constraint approach.
`
`Improved accuracy is realized primarily for three reasons. First, the method avoids
`
`mathematically treating truly sequential observations as if they were simultaneous.
`
`Second, because each estimate is based on the observation of an individual device,
`
`perceived error (statistically unusual estimates) can be more directly attributed to the
`
`corresponding device. This can be used for concurrent autocalibration which can be
`
`elegantly incorporated into the existing Kalman filter. Third, the Kalman filter inherently
`
`addresses the effects of noisy device measurements. Beyond accuracy, the method nicely
`
`facilitates motion prediction because the Kalman filter already incorporates a model of the
`
`user’s dynamics, and because it provides smoothed estimates of the user state, including
`
`potentially unmeasured elements. Finally, in systems where the observations are only
`
`available sequentially, the method can be used to weave together information from
`
`individual devices in a very flexible manner, producing a new estimate as soon as each
`
`individual observation becomes available, thus facilitating multisensor data fusion and
`
`improving the estimate rates and latencies.
`
`The most significant aspect of this work is the introduction and exploration of the
`
`SCAAT approach to 3D tracking for
`. However I also believe that this
`virtual environments
`
`work may prove to be of interest to the larger scientific and engineering community in
`
`addressing a more general class of tracking and estimation problems.
`
`iii
`
`META 1009
`META V. THALES
`
`

`

`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`Thanks to
`
`ACKNOWLEDGEMENTS
`
`Gary Bishop for being my advisor and friend, for begin creative, insightful, and
`humorous, and for sharing with me his original idea for
`one-LED-at-a-time
`;
`tracking
`
`Gary Bishop, Vernon Chi, Henry Fuchs, Anselmo Lastra, John Poulton, and
`Russell Taylor for serving on my doctoral dissertation committee;
`
`Vernon Chi and Russell Taylor for their extremely valuable contributions to my
`dissertation while acting as official readers;
`
`Russell Taylor for consenting to replace Henry Fuchs as an official reader
`following Henry’s unexpected surgery just prior to my defense;
`
`John Poulton for his support and humor, and for agreeing to serve on my
`committee following Henry Fuchs’ surgery (in case Henry could not fully
`participate);
`
`Henry Fuchs for remaining on my committee (in a limited capacity) in spite of
`orders to “drop everything” following his surgery;
`
`Dr. Gil Crouse, Jr. of BBN Systems and Technologies (Arlington, VA) for
`introducing me to the bearings-only tracking work done in the 1970s and 1980s;
`
`Mark Mine for enticing me to come to UNC-CH, for Mark and his wife Sandra
`sharing their home with me and my wife Linda for many months after our arrival
`in Chapel Hill, and Mark most recently for helping arrive at the phrase
`single-
`;
`constraint-at-a-time
`
`and most of all
`
`Linda, my wife, for her support, patience, and warmth, and in particular for
`commuting between our home in Carrboro and her job in Clayton each day
`(55 miles, approximately one hour) so that I might spend less of
` time
`my
`commuting. I will happily chauffeur us around for the rest of my life.
`
`Financial support for this work came from the following grants:
`
`DARPA/ETO contract no. DABT 63-93-C-0048, “Enabling Technologies and
`Application Demonstrations for Synthetic Environments”, Principle Investigators
`Frederick P. Brooks Jr. and Henry Fuchs (UNC Chapel Hill);
`
`NSF Cooperative Agreement no. ASC-8920219 and DARPA: “Science and
`Technology Center for Computer Graphics and Scientific Visualization”, Center
`Director A. van Dam (Brown University), Principle Investigators Andy van Dam,
`Al Barr (California Institute of Technology), Don Greenberg (Cornell University),
`Henry Fuchs (UNC Chapel Hill), and Rich Riesenfeld (University of Utah).
`
`iv
`
`META 1009
`META V. THALES
`
`

`

`
`
`(This page intentionally left blank.)
`
`v
`
`META 1009
`META V. THALES
`
`

`

`
`
`
`
`
`TABLE OF CONTENTS
`
`LIST OF TABLES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiii
`
`LIST OF FIGURES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiv
`
`LIST OF ABBREVIATIONS. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xviii
`
`GENERAL NOTATION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xix
`
`LIST OF SYMBOLS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xx
`
`GREEK SYMBOLS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xxviii
`
`Chapter 1. Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
`
`1.1 Narrative, Names, and Nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
`
`1.2 Observability and Partial Information . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
`
`1.3 An Unusual Approach to Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
`
`1.4 Contribution (Thesis Statement) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
`
`Chapter 2. Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
`
`2.1 Why a Kalman Filter? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
`
`2.1.1 Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
`
`2.1.2 Data Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
`
`2.1.3 Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
`
`2.1.4 Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
`
`2.2 Temporal Concerns . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
`
`2.2.1 Fewer Observations, Improved Timing and Accuracy . . . . . . . . . . 48
`
`2.2.2 Estimation Rates and Latencies . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
`
`2.2.3 Measurement Collection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
`
`vi
`
`META 1009
`META V. THALES
`
`

`

`
`
`
`
`2.2.4 Computational Complexity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
`
`2.3 The Simultaneity Assumption. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
`
`2.3.1 Mathematics Unaware that Target is in Motion . . . . . . . . . . . . . . . 53
`
`2.3.2 When is it a Problem? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
`
`2.4 Hybrid Systems and Multisensor Data Fusion . . . . . . . . . . . . . . . . . . . . . . . . 56
`
`2.4.1 Hybrid Systems, the Past and the Future . . . . . . . . . . . . . . . . . . . . . 56
`
`2.4.2 Multisensor Data Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
`
`2.4.3 A Simple Hybrid Example. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
`
`2.5 Source and Sensor Autocalibration. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
`
`2.5.1 What to Calibrate? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
`
`2.5.2 When and Where to Calibrate? . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
`
`2.5.3 Calibration vs. Autocalibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
`
`2.5.4 Why SCAAT? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
`
`2.6 Broad Applicability. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
`
`2.7 Putting Things In Perspective . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
`
`Chapter 3. Related Work. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
`
`3.1 Bearings-Only Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
`
`3.2 Sequential Updates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
`
`3.3 The Iterated Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
`
`3.4 The Collinearity Method. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
`
`3.5 Autocalibration (Gottschalk & Hughes) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
`
`3.6 The SELSPOT System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
`
`3.7 MCAAT Estimators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
`
`vii
`
`META 1009
`META V. THALES
`
`

`

`
`
`
`
`3.8 Kalman Filters and GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
`
`Chapter 4. The SCAAT Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
`
`4.1 The Kalman Filter in Brief . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
`
`4.2 The SCAAT Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
`
`4.2.1 The Dynamic (Process) Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
`
`4.2.2 The Model State and Discrete Transitions. . . . . . . . . . . . . . . . . . . . 75
`
`4.2.3 The Process Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
`
`4.2.4 The Measurement Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
`
`4.2.5 The Measurement Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
`
`4.2.6 The Measurement Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
`
`4.2.7 The State Error Covariance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83
`
`4.3 The SCAAT Algorithm for Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83
`
`4.3.1 Time Update. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83
`
`4.3.2 Measurement Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
`
`4.3.3 The Kalman Gain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87
`
`4.3.4 The Measurement Residual . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
`
`4.3.5 Correct the State and Update the Error Covariance. . . . . . . . . . . . . 94
`
`4.3.6 Update External Orientation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96
`
`4.3.7 The SCAAT Algorithm Summary . . . . . . . . . . . . . . . . . . . . . . . . . . 97
`
`4.3.8 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98
`
`4.4 SCAAT Autocalibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
`
`4.4.1 An Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
`
`4.4.2 Device Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
`
`viii
`
`META 1009
`META V. THALES
`
`

`

`
`
`
`
`4.4.3 Revised Tracking Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
`
`4.4.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102
`
`4.5 Code Optimization Strategies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
`
`4.5.1 Jacobian Evaluation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
`
`4.5.2 Sparse Matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
`
`4.5.3 Symmetry. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
`
`4.5.4 Matrix Inversion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
`
`4.5.5 Small Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
`
`4.6 Source & Sensor Ordering Schemes. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
`
`Chapter 5. Mathematical Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107
`
`5.1 Filter Stability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107
`
`5.1.1 General Conditions for Kalman Filter Stability. . . . . . . . . . . . . . . 107
`
`5.1.2 Locally Unobservable Measurement Systems . . . . . . . . . . . . . . . . 107
`
`5.1.3 Complete Conditions for SCAAT Filter Stability . . . . . . . . . . . . . 108
`
`5.1.4 SCAAT Filter With Calibration. . . . . . . . . . . . . . . . . . . . . . . . . . . 111
`
`5.1.5 Inaccuracy of the Dynamic Model. . . . . . . . . . . . . . . . . . . . . . . . . 111
`
`5.2 Computational Complexity. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112
`
`5.2.1 EKF Methods (SCAAT and MCAAT) . . . . . . . . . . . . . . . . . . . . . 113
`
`5.2.2 The Collinearity Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116
`
`5.2.3 Collinearity vs. the EKF Methods . . . . . . . . . . . . . . . . . . . . . . . . . 116
`
`Chapter 6. Experiments. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
`
`6.1 SCAAT Filter Configuration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
`
`ix
`
`META 1009
`META V. THALES
`
`

`

`
`
`
`
`6.1.1 The HiBall Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
`
`6.1.2 The Beacon Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
`
`6.1.3 Process Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121
`
`6.1.4 Initial Error Covariance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122
`
`6.1.5 Measurement Function. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123
`
`6.1.6 Measurement Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
`
`6.2 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
`
`6.2.1 EKF Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
`
`6.2.2 Error Nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 130
`
`6.2.3 EKF and Collinearity Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . 131
`
`6.2.4 EKF Beacon Autocalibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142
`
`6.2.5 Simultaneous Observations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144
`
`6.2.6 Moving-Window Observations . . . . . . . . . . . . . . . . . . . . . . . . . . . 147
`
`6.2.7 Cold Start . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148
`
`6.2.8 Blocked Cameras . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149
`
`6.2.9 Low Data Rates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149
`
`6.2.10 Low Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152
`
`6.2.11 Single Scalar Measurements . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153
`
`Chapter 7. Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155
`
`7.1 An Improved Dynamic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155
`
`7.2 Model Parameter Sensitivity. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155
`
`7.3 An Adaptive or Multiple-Model Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 156
`
`7.4 A Better Estimator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 157
`
`x
`
`META 1009
`META V. THALES
`
`

`

`
`
`
`
`7.5 Source and Sensor Ordering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158
`
`7.6 Solving a System of Simultaneous Equations . . . . . . . . . . . . . . . . . . . . . . . 158
`
`7.7 Other Tracking Implementations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159
`
`APPENDIX A. THE SIMULTANEITY ASSUMPTION . . . . . . . . . . . . . . . . . . . . . . . 161
`
`A.1 An Example 2D Tracker . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 161
`
`A.2 The Target in Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162
`
`A.3 Further Illustrations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164
`
`APPENDIX B. THE KALMAN FILTER . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167
`
`B.1 The Discrete Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167
`
`B.1.1 The Process to be Estimated . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167
`
`B.1.2 The Computational Origins of the Filter . . . . . . . . . . . . . . . . . . . . 168
`
`B.1.3 The Probabilistic Origins of the Filter. . . . . . . . . . . . . . . . . . . . . . 170
`
`B.1.4 The Discrete Kalman Filter Algorithm . . . . . . . . . . . . . . . . . . . . . 170
`
`B.1.5 Filter Parameters and Tuning . . . . . . . . . . . . . . . . . . . . . . . . . . . . 172
`
`B.2 The Extended Kalman Filter (EKF) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174
`
`B.2.1 The Process to be Estimated . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174
`
`B.2.2 The Computational Origins of the Filter . . . . . . . . . . . . . . . . . . . . 175
`
`B.3 Example: Estimating a Random Constant . . . . . . . . . . . . . . . . . . . . . . . . . . 179
`
`B.3.1 The Process Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 179
`
`B.3.2 The Filter Equations and Parameters . . . . . . . . . . . . . . . . . . . . . . 180
`
`B.3.3 The Simulations. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 180
`
`xi
`
`META 1009
`META V. THALES
`
`

`

`
`
`
`
`APPENDIX C. ACTUAL PROBABILITY DENSITIES . . . . . . . . . . . . . . . . . . . . . . . 184
`
`APPENDIX D. THE UNC HIBALL TRACKER . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 191
`
`D.1 The Ceiling. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192
`
`D.2 The Hiball . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192
`
`APPENDIX E. THE SIMULATION ENVIRONMENT . . . . . . . . . . . . . . . . . . . . . . . . 194
`
`E.1 User Motion Data Sets . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 194
`
`E.2 Beacon Observations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 194
`
`E.3 EKF and Collinearity Implementations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 195
`
`E.3.1 SCAAT and MCAAT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 196
`
`E.3.2 Collinearity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 196
`
`E.3.3 Moving-Window Observations . . . . . . . . . . . . . . . . . . . . . . . . . . . 196
`
`E.4 EKF Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 197
`
`E.5 Assumptions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 198
`
`REFERENCES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201
`
`xii
`
`META 1009
`META V. THALES
`
`

`

`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`LIST OF TABLES
`
`Table 2.1:
`The practical timing characteristics of a few existing tracking systems. . . . . 54
`
`Table 2.2:
`Some example technologies and SCAAT observations. . . . . . . . . . . . . . . . . 63
`
`Table 4.1:
`Output, correlation, and spectral density units . . . . . . . . . . . . . . . . . . . . . . . . 74
`
`Table 4.2:
`Variance units for the elements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
`
`Table 5.1:
`Upper bound on EKF floating-point operations. . . . . . . . . . . . . . . . . . . . . . 113
`
`Table 5.2:
`Added floating-point operations with autocalibration. . . . . . . . . . . . . . . . . . 115
`
`Table 6.4:
`EKF parameter sets for perfect beacons. . . . . . . . . . . . . . . . . . . . . . . . . . . . 127
`
`Table 6.5:
`EKF parameter sets for 1.7 mm beacon error without autocalibration. . . . . 127
`
`Table 6.6:
`EKF parameter sets for 1.7 mm beacon error with autocalibration.. . . . . . . 128
`
`Table 6.7:
`SCAAT parameter sets for low sampling rates. . . . . . . . . . . . . . . . . . . . . . . 129
`
`Table 6.8:
`SCAAT parameters for “still” target dynamics. . . . . . . . . . . . . . . . . . . . . . . 130
`
`Table B.1:
`Discrete Kalman filter time update equations. . . . . . . . . . . . . . . . . . . . . . . . 171
`
`Table B.2:
`Discrete Kalman filter measurement update equations.. . . . . . . . . . . . . . . . 172
`
`Table B.3:
`Extended Kalman filter time update equations. . . . . . . . . . . . . . . . . . . . . . . 177
`
`Table B.4:
`Extended Kalman filter measurement update equations. . . . . . . . . . . . . . . . 177
`
`Table E.1:
`Characteristics of the “typical” data set. . . . . . . . . . . . . . . . . . . . . . . . . . . . 195
`
`xiii
`
`META 1009
`META V. THALES
`
`

`

`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`LIST OF FIGURES
`
`Figure 1.1:
` Sensors vs. sensing units. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
`
`Figure 1.2:
` A simple estimator example. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
`
`Figure 1.3:
` SCAAT and constraints on a system of simultaneous equations. . . . . . . . . . 38
`
`Figure 1.4:
` Camera position and orientation from four scene points. . . . . . . . . . . . . . . . 39
`
`Figure 1.5:
` Camera position and orientation from two scene points. . . . . . . . . . . . . . . . 40
`
`Figure 1.6:
` Camera position and orientation from one scene point. . . . . . . . . . . . . . . . . 41
`
`Figure 1.7:
` Putting the pieces together. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
`
`Figure 2.1:
` An ideal filter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
`
`Figure 2.2:
` State versus parameter estimation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
`
`Figure 2.3:
` Timing diagram for a Polhemus Fastrak magnetic tracker. . . . . . . . . . . . . . 51
`
`Figure 2.4:
` Timing diagram for a (hypothetical) SCAAT magnetic tracker. . . . . . . . . . 52
`
`Figure 2.5:
` The simultaneity assumption. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
`
`Figure 2.6:
` Error caused by the simultaneity assumption.. . . . . . . . . . . . . . . . . . . . . . . . 55
`
`Figure 2.7:
` Timing diagram for a hypothetical conventional hybrid tracking system. . . 57
`
`Figure 2.8:
` Timing diagram for a SCAAT inertial-acoustic hybrid tracking system. . . . 58
`
`Figure 2.9:
` Intrinsic parameters. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
`
`Figure 2.10:
` Extrinsic parameters. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
`
`Figure 2.11:
` Autocalibration and attribution of measurement error. . . . . . . . . . . . . . . . . 62
`
`Figure 3.1:
` The circumstances surrounding the bearings-only tracking problem. . . . . . 64
`
`Figure 3.2:
` A timing diagram for a conventional Kalman filter. . . . . . . . . . . . . . . . . . . . 66
`
`Figure 3.3:
` A timing diagram for a Kalman filter using the sequential updates method. 66
`
`Figure 3.4:
` A timing diagram for a Kalman filter using the SCAAT method. . . . . . . . . 67
`
`xiv
`
`META 1009
`META V. THALES
`
`

`

`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`Figure 3.5:
` A timing diagram for an iterated Kalman filter. . . . . . . . . . . . . . . . . . . . . . . 68
`
`Figure 4.1:
` Continuous-time position-velocity dynamic model. . . . . . . . . . . . . . . . . . . . 74
`
`Figure 4.2:
` Geometric view of state change during time update step.. . . . . . . . . . . . . . . 85
`
`Figure 4.3:
` Geometric view of error covariance change during time update step. . . . . . 85
`
`Figure 4.4:
` Measurement prediction from the state and associated parameters. . . . . . . . 86
`
`Figure 4.5:
` The measurement Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87
`
`Figure 4.6:
` Project state-space uncertainty into measurement space. . . . . . . . . . . . . . . . 89
`
`Figure 4.7:
` Completing the denominator of the Kalman gain. . . . . . . . . . . . . . . . . . . . . 90
`
`Figure 4.8:
` The measurement residual. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
`
`Figure 4.9:
` Geometric view of state change after measurement correction. . . . . . . . . . . 95
`
`Figure 4.10:
` Geometric view of error covariance change after measurement update. . . 95
`
`Figure 4.11:
` Complete sequence of filter state coordinate frame transitions. . . . . . . . . . 97
`
`Figure 4.12:
` The revised tracking algorithm for autocalibration. . . . . . . . . . . . . . . . . . 102
`
`Figure 4.13:
` A timing diagram for autocalibration with d devices.. . . . . . . . . . . . . . . . 103
`
`Figure 5.1:
` Floating-point operation comparison: Collinearity vs. EKF. . . . . . . . . . . . 118
`
`Figure 6.1:
` Measurement signal strength. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125
`
`Figure 6.2:
` Collinearity vs. EKF for perfect beacons . . . . . . . . . . . . . . . . . . . . . . . . . . 132
`
`Figure 6.3:
` Collinearity vs. EKF without autocalibration . . . . . . . . . . . . . . . . . . . . . . . 133
`
`Figure 6.4:
` EKF with autocalibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134
`
`Figure 6.6:
` Summary—Collinearity vs. SCAAT with autocalibration. . . . . . . . . . . . . 135
`
`Figure 6.5:
` Collinearity vs. SCAAT with autocalibration . . . . . . . . . . . . . . . . . . . . . . . 136
`
`Figure 6.7:
` Error spectra for perfect beacons . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138
`
`Figure 6.8:
` Error spectra without autocalibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139
`
`xv
`
`META 1009
`META V. THALES
`
`

`

`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`Figure 6.9:
` SCAAT error spectra. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 140
`
`Figure 6.10:
` Peaks in the error spectrum. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141
`
`Figure 6.11:
` Final beacon error for EKF runs with autocalibration . . . . . . . . . . . . . . . 143
`
`Figure 6.12:
` Final beacon error for SCAAT with autocalibration. . . . . . . . . . . . . . . . . 143
`
`Figure 6.13:
` Simultaneous observations, perfect beacons. . . . . . . . . . . . . . . . . . . . . . . 145
`
`Figure 6.14:
` Simultaneous observations with autocalibration. . . . . . . . . . . . . . . . . . . . 146
`
`Figure 6.15: Autocalibration with truly simultaneous observations . . . . . . . . . . . . . . . 147
`
`Figure 6.16: Cold start . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150
`
`Figure 6.17: Blocked HiBall cameras . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150
`
`Figure 6.18: Error spectra for SCAAT with low sampling rates . . . . . . . . . . . . . . . . . . 151
`
`Figure 6.19: Collinearity vs. SCAAT with autocalibration (“still” data set). . . . . . . . . 152
`
`Figure 6.20: Error spectra for the “still” data set. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154
`
`Figure A.1: A simple 2D inside-looking-out optoelectronic tracker. . . . . . . . . . . . . . . 161
`
`Figure A.2: A close up of the 2D target platform and the two 1D pinhole cameras. . . 162
`
`Figure A.3: Sample track. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163
`
`Figure A.4: Estimated position versus measurement delay. . . . . . . . . . . . . . . . . . . . . . 165
`
`Figure A.5: A parametric version of figure A.4. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 165
`
`Figure A.6: Error caused by the simultaneity assumption. . . . . . . . . . . . . . . . . . . . . . . 166
`
`Figure B.1: The ongoing discrete Kalman filter cycle. . . . . . . . . . . . . . . . . . . . . . . . . . 171
`
`Figure B.2: The complete Kalman filter algorithm. . . . . . . . . . . . . . . . . . . . . . . . . . . . 173
`
`Figure B.3: The complete extended Kalman filter operation. . . . . . . . . . . . . . . . . . . . . 178
`
`Figure B.4: The first simulation:.. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182
`
`Figure B.5: Error covariance, first simulation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182
`
`xvi
`
`META 1009
`META V. THALES
`
`

`

`Figure B.6: Second simulation: . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183
`
`Figure B.7: Third simulation: . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183
`
`Figure C.1: Actual state error density before and after time update step. . . . . . . . . . . . 185
`
`Figure C.2: Multiple viewpoints of densities from figure C.1. . . . . . . . . . . . . . . . . . . . 186
`
`Figure C.3: Actual measurement Jacobian H. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 187
`
`Figure C.4: Actual measurement-space error densities. . . . . . . . . . . . . . . . . . . . . . . . . 188
`
`Figure C.5: Actual state error density before and after complete filter update. . . . . . . 189
`
`Figure C.6: Multiple viewpoints of densities from figure C.5. . . . . . . . . . . . . . . . . . . . 190
`
`Figure D.1: An outward-looking optoelectronic tracking system. . . . . . . . . . . . . . . . . 191
`
`Figure D.2: The original UNC optoelectronic ceiling tracker. . . . . . . . . . . . . . . . . . . . 192
`
`Figure D.3: The new HiBall camera cluster. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 193
`
`Figure E.1: Moving vs. fixed-window observations. . . . . . . . . . . . . . . . . . . . . . . . . . . 197
`
`Figure E.2: Primary and secondary HiBall views. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 199
`
`xvii
`
`META 1009
`META V. THALES
`
`

`

`LIST OF ABBREVIATIONS
`
`1D
`
`2D
`
`3D
`
`6D
`
`DOF
`
`EKF
`
`FP
`
`GPS
`
`Hz
`
`one-dimensional
`
`two-dimensional
`
`three-dimensional
`
`six-dimensional
`