Department of Computer & Information Science

Technical Reports (CIS)

University of Pennsylvania Year 1988

An Adaptive Tracking Algorithm for

Robotics and Computer Vision
Application
Reem Bassam Safadi
University of Pennsylvania

This paper is posted at ScholarlyCommons.

http://repository.upenn.edu/cis reports/735
 AN ADAPTIVE TRACKING
ALGORITHM FOR ROBlTlCS
AND COMPUTER VISION
APPLICATION
Reem Bassam Safadi

MS-CIS-88-05
GRASP LAB 132

Department of Computer and Information Science

School of Engineering and Applied Science
University of Pennsylvania
Philadelphia, PA 19104

January 1988

Acknowledgements: This research was supported in part by DARPA grant N00014-85-

K-0807, NSF-CER grant MCS-8219196, USPS 104230-87-H-00011M-0195and U.S. Army
grants DAA29-84-K-0061, DAA29-84-9-0027. Any correspondence regarding this paper should
be sent to Kwangyoen Wohn, GRASP LAB.
 UNIVERSITY OF PENNSYLVANIA
THE MOORE SCHOOL OF ELECTRICAL ENGINEERING
SCHOOL OF ENGINEERING AND APPLIED SCIENCE

AN ADAPTIVE TRACKING ALGORITHM FOR

ROBOTICS AND COMPUTER VISION
APPLICATIONS

Philadelphia, Pennsylvania
December 1987

A thesis presented to the Faculty of Engineering and Applied Science of the University of
Pennsylvania in partial fulfillment of the requirements for the degree of Master of Science
in Engineering for graduate work in Electrical Engineering.

Dr. Kwangyoen lbohn, Supervisor

 Table of Contents

Abtract ..................................................................................................iii
Chapter 1

Introduction and Background .......................................................... 1

Chapter 2

Compariaon of tracking Algorithms ...................................................... 6

Chapter 3

General Implementation of the Elected Algorithm .............................,.,17

Chapter 4

Tailoring the Elected Algorithm to the PVM1-Ba~ed

Robotics and Computer Vision System ....................
. ................. 27
....
Chapter 5

Conclusion and Recommendatiom for Future Work ........................... . 34

References ...................................................................................................... 36

Appendix I

Basic EQuations of the Simulated W i n g Algorithms ......................... 39

Appendix I1

The lkacking Program ................................................ . ............. . 42

 Abstract

We provided a vision-controlled robotics manipulation system with a robust, accurate

algorithm to predict the translational motion of a 3-D object; hence, makiig it posaible

to continuously point the video camera at the moving object. The real time video images

are fed to a PVM-1 (a pyramid-based image processor) for image processing and moving
object detection. The measured object coordinates are continuously fed to our algorithm

for track smoothing and prediction. In this study, we examined several tracking algorithms

and adopted an optimal a - B filter for tracking purposes and the a - /I - 7 filter as part of

the initialization procedure. The optimum gains for these 6lkm are based on the M i n g
Index principle which in its turn is based on the measurement noise variance and the object
dynamics. We derived an expression for the noise variance corresponding to our application.
As for the object dynamics, we developed an adaptive method (using the a - /I - 7 filter
mentioned above) for inferring object dynamics in an iterative learning process that results
in an accurate estimate of the W m g Index. The accuracy of our algorithm r e h that

of the Kalman filter but is much simpler computationally.

 CHAPTER 1

Introduction and Background

A machine vision system is often considered as part of a larger system that interacts
with the environment. The input to such a system, is an image, or several images, while
its output is a description that must bear some relationship to what is being imaged and

must contain all the needed information to carry out a designated task. In other words, the
objective is not to obtain any description of what is imaged but one that allows us to take

an appropriate action. Since building a Wversal' vision system is still at the early stages

of development, researchers have been addressing themselves to systems that perform a

particular task in a controlled environment or to modules that could eventually constitute
a generai purpose system [I].

The work here is tied to one of those particular t d a , namely, real time analysis of images
in a dynamic environment. Under this category falls the topic of motion analysis.

It is known that there are two major obgtacleer in computing the 3-D parameters of

rigid motion from the retinal optical flow. One of these is the high dimensionality of the
parameter space; the other is the nonlinearity of the constraint equatione. It has been shown

in [2] that these ahortcominge may be eased by following two strategies. The h t , and the
most important of these, ia to employ t r a c h i , thereby exploiting the temporal behavior
of a moving object. The second ia to employ stereoscopic imaging. The results showed that
tracking the object of interest is advantageous in both monocular and binocular imaging
situations.
1
 The use of a tracking filter is dasirable under these circumstances in order to increase the

accuracy of the image- based measurements of the position and orientation of a 3-D moving

object. Our goal has been to find the most suitable tracking filter and tailor it to an existing
vision-controlled robotics manipulation system that has the capability of performing object

detection in real time. Thia capability is provided mainly by the Pyramid preprocessor
[3], [4]. We will focus our attention here on tracking the translational motion of a 3-D

rigid object. Ttacking and prediction of this motion can be used to continuously point the
video camera of the robotics system on the moving object. An extension to the problem of
tracking the rotational motion of the rigid object is straightforward. The following sections
give an overview of the Pyramid preprocessor and the existing moving-object detection

algorithm.

1.1 The Pyramid Machine

The Pyramid Machine is a preprocessor dedicated to producing a multi-resolution repre-

sentation of an image; specifically, the $yramid liken mpresentation. -ids in general,
are data structuree that provide successively condensed representationa of the information

in the input image. According to [3]there is evidence that the human visual system usea
a form of multi- resolution representation wbkh supports the concept that image process-
ing at multi-resolution levels is very efficient and robust. The most obvious advantage of

pyramid representatiom is that they provide a poeeibility for reducing the computational
cost of various image operations. Many basic image operations may be performed efficiently
within these pyramid structures. The first requirement for a multi-mlution image pro-

cesaing system is that it be able to perform a pyramid transform such ae the FSD (filter,
2
subtract, and decimate transform), to decompose the original image into a set of different

reeolutions. The Pyramid machime is capable of constructing a complete low or bandpass

pyramid from a 256 x 256 image in 1/30 of a second (one frame time). The present design
uses a single filter and decimate stage. During pyramid conatmction data is recirculated
through this module for each pyramid level. A separable, five tap filter is used in the pyra-
mid construction. The input data and output pyramid levels are represented with 8 bits
per pixel, while internal computations are performed with 16 bit arithmetic.

When the system is used aa a video preprocessor, it continuously procerraes incoming

image data, transforming it to a more suitable representation for further processing such as

eliminating certain epatial frequency bands or computing local energy measures at different
scales, storing the results in a memory frame store. These d t s can be accessed simulta-

neously by the host computer for further processing. In this manner, the hoat processor (an

IBM RT in our case) is relieved from performing extensive computations. For example, for
a 256 x 256 pixel image, updated 30 times per mend, the data rate is 2M pixels per second.

By representing the video image in a suitable format, the hoat can perform real time or

near real time image proceesing operations by selectively limiting the processing to 1000 to
20,000 pixels per second, rather than 2M pixels/second. This proved to be sufficient for

performing numerous image processing tasks, e.g. real time motion detection and tracking,

and course-fine pattern matching for robotics guidance [3].

1.2 The W t i n g Motion Detection Algorithm

The tracking filter (that provides tbis system with prediction capability) usca an existing
motion detection program which detects object motion wherever it may occur within a large
3
field of view. The following is a brief description of how this is accomplished 1.11.

Let I(T) be the image frame at time T . In the first step, a difference of two consecutive
images is obtained D(T) = I(T)- I(T- 1). Difference values that are not zero indicate that

a change has occurred in the original sequence. In the second step, the difference image is
d e c o m p d into spatial frequency bands through the conatmction of a Laplacian pyramid

(See Figure 1.1). A particular band paee level is then selected for further analysis. For

example, I& 1 could be chosen to generate a second series of low paes versions G,I Lo! where

n ranges from 1 to 4.

G4 which contains only 16 x 16 samples (and yet represents image changes correapondiig

to that within the original image) is examined by the host. (It is worthwhile mentioning

that a typical high performance microcomputer (e.g., an IBM-RT) can examine only, on
the order of, 16x16 samples per one h e interval [4].) If a change ia detected in G4,the

host examinea a 16 x 16 subarray of GScentered on the change detected in G4. A detected

change within the new subimage directs the host to examine a 16 x 16 subarray of G2.This
procedure allows the system to rapidly home in on a small object anywhere within the field

of view, since G4,which is a 16 x 16 array, contains the entire field of view of the 256 x 256

original image, but a 16 x 16 subarray located in G1 covers only a small area of the original
image [4].

1.3 An Adaptive Tkacking Filter

We conducted a general study on the tracking filters and chm one of these filters for
our application. Among the filters that were examined are: The Kalman, Wiener, rr - /3
and a! - ,fl - 7 Elten, and the Two-Point Extrapolator. The criteria followed in our decision
4
 F = FILTER
D = DECIMATE

Figure 1.1 An FSD Pyramid Tramform (from (31).

Goo:The origin$ image,r t p m t e d by 256 x 256 a m 9 of pixels
G,,: The low psss resnit where the hiat index indicates the number
of filter stepe, the second index indicates the decimation steps
F: A convolution with an FIR low pam 6lter
D: Decimate operation, every other row and cohunn is discarded
to obtain G,+,
L,: Ban* or Laplam pgramid kvel obtained by subtracting two
consecutive Ganssian pyramid levela as ahown
making will be discussed in the next chapter.

We adopted an optimal a - B filter for tracking purposes and the a - 8 - 7 filter ae part

of the initialieation procedure. The powerful performance of this filter (which redm that
of the Kalman) liw in the method that produces the filter gain values. The derivation of the
optimum filter gains is based on the k k i n g Index which is proportional to the position
uncertain$ due to object accelerations to that due to measurement errors (in the detection
scheme) 151.

We derived an expression for the noise variance corresponding to our application. As for

the object dynamics, we developed an adaptive method (using the cr - 6 -7 filter mentioned
previously) for inferring object dynamics in an iterative learning process that reaultn in an

accurate estimate of the Tracking Index.

The work presented here was developed on an IBM AT; and the d t a were tested using

the position data of the moving object obtained from files produced by the Pyramid/IBM-

RT system. The following chapters give a more detailed account of our work.
 CHAPTER 2

Comparison of Tracklng Algorithms

As we observe the position of a moving object over time using any kind of measuring
instrument (a video camera, radar, etc.), almost always, the observations are cluttered by

noise, errors and i n a c w i e s . The primary function of the trackiig filter is to accept

the noiey position data at its input and provide smoothed object position and velocity
estimates at its output. These values are used for controlling the orientation and position
of the camera as well as predicting future object poeitions. In addition, the smoothed

position and velocity estimates can be used for track correlation and association purposes
in a multi-moving-object environment.

In smoothing and prediction, two set3 of equations govern the whole technique. The k t
set of equations is the differential equations which describe the process (object motion).

The second set relates the parameters being measured to those to be estimated (61. In
addition, smoothing and prediction are related in a recursive manner, i.e. a predicted value

depends on the last smoothed value, and a smoothed value t h into account the last

predicted d u e . As an example, the prediction and smoothing relationships for an optimal

mean-squareerror (MSE) estimation procesa (Kalman filter) for object tracking is given by

l51,[71:

prediction: 2(k + 1lk) = #i(k lk)

smoothing: 4(k + Ilk + 1) = 4(k + Ilk) + K(k + l)[z(k + 1) - h2(k + Ilk)]
6
where,

k = kth time interval

?(k + 11k) = predicted state value at time k + 1

P(klk) = smoothed state value estimate at time k

i(k + Ilk + 1) = smoothed state value estimate at time k + 1

z(k + 1) = measured state value at time k +1
K(k + 1) = gain value at time k -t1

t$ = state transition matrix h = 1 when z represents pition, O when z represents velocity

or acceleration.

A state could be position, velocity, or acceleration. Here, the obeervation or measurement

is modeled by the actual object position plus an additive noise component:

where the measurement noise uncertainty n(k) ia assumed to be a zero-mean, white

stationary random pmces8.

The problem then is narrowed down to obtaining the "best* estimate of i ( k ( k ) . There

are two solutione for extracting the k t " smoothmg estimate. The h t , is the Least
Squares method, the other is the Minimum Variance method. Generally, the Least Squares

method (the one under consideration) takes a fitting function and fits it to the data. The
method calculates the residuals (differences between the obsewations and the fitting func-

tion), squares them, ad& them, and produces a certain vdue. The Leaet Squares method
7
in its most general form, then minimizes that value, for any arbitrary number of dimen-
sions. The Mimum Variance method is similar to least squares, but is more generalized.
Minimum Variance take every o k m t i o n and weights each precisely according to ite mer-
its. For example, poor observations have a large variance while better observations have
a smaller variance. Typically, observations are weighted by the inverse of their variance,
hence poor obeervations are suppressed, while good observations are boosted relatively (61.

2.1 Cornpartson of 'hacking Algorithms

A comparative study of five important real-time tracking filters was conducted in 181.

It compared these filters in tracking accuracy and computer requirements (memory and
execution time).

The filters considered in this study were: the Kalman filter, simplified Kalman filter,

a - fi filter, Wiener filter, and the two point extrapolator. The gain vectors of the firat

three filters were assumed to be calculated in real time. The last two filters (Wiener and

two point extrapolator) were both examples of stored gain vectors. The following is a brief
overview of each filter.

In the K h a n filter, a model for measurement error has to be assumed as well as a

model of the object trajectory and the disturbance of the trajectory (81. The Kalman filter

can in principle, utilize a wide variety of modele for measurement noise and trajectory
disturbance; however, it is often sssumed that theee are described by white noise with zero
mean [9]. This becomes a requirement for the filter optimality and introdurn the need to
have two augmented state variables in order to whiten the object maneuver and adapt it to

the theoretical framework neceasay to make the filter optimal [8].

8
 If the maneuver is assumed to be white, no augmentation need be performed (resulting
in what is known as a simpiified Kalman filter). Here it is assumed that the change in
velocity of the object is uneorrelated between samples, i.e. white. Ruthermore, if thia filter
were restricted to modeling the object path as a straight line and if the measurement noise

and maneuvering noise were modeled as white gauasian with zero mean, the Kalman filter
equations reduce to the Alpha-Beta filter equatiom with the parameter alpha and beta

computed sequentially by the Kalman filter procedure [9].

The Wiener filter differs from the Kalman filter in that its gain vector being equal to the
steady state gain vector of the regular K h a n filter and is calculated off-line and stored
in memory. This results in considerable computational savings in addition to makiig it
simpler than any of the preceding filters. Because it has constant gain, the Wiener filter

requires no auxiliary equations to be solved and requires very little computer memory.

Thia filter is adaptable to a variety of moving objects and can track both maneuvering and

nonmaneuvering moving objects well. This is because its gain is derived from the Kalman
filter, which accounts for the statistics of object maneuver directly [8].

The a-8 filter coma in many different varieties. Some are deaigned to provide the beat
transient following capability for a constant velocity object, while simultaneously providing

the best minimum variance estimate of position and velocity [lo]. Other a-@ filters that
have been designed utilize the object maneuver statistics [I 11. If the performance criterion

ia dynamic minimieation of the total mean-squared filtering errors, the filter then takes
the form of the Kalman filter. if the design objective is to minimize the tracking errors
against a general class of trajectories, the corresponding a-8 filter takes even a different

form [s]. The a-8 filter considered in the simulation study [8] was designed to minimize the

mean-squared error in filtered (smoothed) position and velocity, under the assumption of a
9
constant velocity object motion.

All of the mentioned filters are recursive fading memory filtera. The simplest type of

filters that can be implemented is the 'almost memoryleas" two point extrapolator. This
filter uses the last data point to determine object position and the last two data points to
determine object velocity. Because this filter is essentially memoryless, its performance in

tracking maneuvering and nonmaneuvering objecta is quite as bad 181.

The result of the simulation study conducted in [8],showed that the most sophisticated fil-

ter, the Kalman filter, is the m a t accurate and the m a t costly to implement. Furthermore,
the Kalman filter, the simplified Kalman filter, and the Wiener filter generally performed
within 20 percent of each other (in terms of execution time and memory requirements). The
a - /3 filter performed on the average about 50 percent worse than the Kalman filter with
the greatest degradation occurring for maneuvering objects. The two point extrapolator,

uniformly performed more than 70 percent worse than the Kalman filter.

As for implementation requirements, they increased in the following order: two point
extrapolator, Wiener filter, a - @ filter, simplified K h a n filter, Kalman filter. The com-
plexity factor between successive filters was about two to one.

An attractive filter would be one that can achieve the performance of a K h a n filter,
both in the transient and eteady state periods; yet be easily implementable in real time.
Also, it will be very desirable for such a filter to have the capability of tuning itself to the
sensor and moving object characteristics.

In addition to the optimal transient and steady atate performance, we would like to have
a simple filter that can achieve this performance regardless of object dynamics. It is unde-

sirable to have solutions (for filter gains) that entail m i v e relationahips of considerable
10
co~ltpkity;it would be ideal to obtain a closed form solution and eliminate the recursive

process altogether at Ieast for the steady state (even if it were simple as in [12]), without
any degradation in performance. Nor would it be desirable to have a simple closed form

solution that can only provide optimal performance for a particularid object motion be-

havior (as in (111 where it is assumed that object accelerations are exponentially correlated,
and (101 where the results apply for constant velocity objects only). This means that in

order to reduce the implementation cost, it is desirable to have constant gain vectors. This
will result in considerable savings over 6lters that calculate the gain vectors in real time.

h m the results of the simulation study, a probable filter that fits most of the above
descriptions would be the Wiener filter. However, the performance of this filter im the
transient state is not optimal, because its off-line computed vector is the Kalman steady

state gain vector. Using the steady state gain in the transient state will give erroneow
predictions.

2.2 The Elected Filter

A recent study (51 provided optimum parametera for the a=@filter which results in o p
timum tracking in both the transient and the steady states. With these parameters, the

a-8 filter achieves the performance of the Kalman filter without increasing its complexity
or implementation cmt.

The above mentioned study introduced an optimal filtering solution for the object track-
ing problem which depends on a parameter dehed as the W.aclting Indexn which is pro-

portional to the paition uncertainty due to object accelerations to &at due to measurement
errors. Upon evaluating this parameter, the optimal transient and steady state gaina are
11
specified. The atering procesa is as follows 161:

Prediction:

Smoothing:

where,

z(klk) is the position estimate at time interval k

v(k1k) is the velocity estimate at time interval k

z ( k ) is the noisy position measurement

a is the position tracking parameter

@ is the velocity gain tracking parameter.

T is the sampling period

In modeling the object motion, a one dimensional, linear, time invariant, ideal model is
used:

where a ( k ) is the moving object state vector at time k; 4 is the state transition matrix;
w(k) is the unknown object maneuver/state transition matrix. 9 ia the acceleration state
12
transition matrix (for a second order model $ = [:TI TIT;for a third ordv model

$ = [bT2 T 1IT. A two state model which includes all possible maneuver accelerations
[5] is sufficient and will be considered hereon; that is

The model in our application is based on the assumption that, without maneuvering (or

with smooth maneuvering in a small time interval), the moving object will generally follow
a straight line constant velocity trajectory.

The noisy object measurements are modeled by the actual position of the moving object
plus an additive noise component, i.e:

where for a two state model

and the measurement noise uncertainty is assumed to be a eero mean white stationary

process. The values of a and @ are determined by the following parameters:

T: The sampling period.

a,, : The measurement noise standard deviation which is determined from the object de-

tection scheme, i.e. the measurement process.

13
a, : The maneuvering accelerations standard deviation. This parameter is related to the
object dynamics.

A typical maneuverung accelerations probability density [7] is shown in Fig. 2.1hthis

figure, A denotes the maximum acceleration which the object can have. Values of the
density between no maneuver (a = 0) and maximum maneuver (a = &A) are non zero

because the moving object may not be accelerating at the maximum rate. The object has
a probability PI of accelerating at thia mrurimum level (either plus or minus), a probability

P2 of not accelerating at all, and an assumed uniform probability distribution of amplitude

(1-(2P1+ P2))/2A of accelerating between -A and +A. The acceleration variable, therefore
has zero mean and variance (A/3)(1 + 4P1- P2).

The tracking index ia provided below in term of the above parameters and also in terms

of the steady state gains '

a and ; along with the corresponding optimal ?j relationship.

As for the optimal a* relationship, it had to be derived and will be given in the following

chapter.

p* = 2(2 - a') - 4 @ 7

The optimum transient Mter gain parametera are based on following recursive approxi-
mations:
14
Figure 2.1 Typical Pmbabiity Demity of Object Acceleration.
 a ( k ) = o ( k - 1 ) + Ga[a* - ~ ( -kl)]; Ga = 1 - e - l f k m

@ ( k )= b(k - 1 ) + G B ( T - /3(k - I ) ] ; Gp = 1 - e-'/'~

where,

cr(k) and B(k) are the gains for the present state

a(k - 1 ) and B(k - 1) are the gains for the previous state
a* and 8' are the steady state optimal gain values, ae defined on the previom page.

K, and Kg are the first order time constants of the corresponding gain excursion from
+

it8 initial to its optimal steady state value. It has been shown in [5] that these time constants

are strictly a function of the trackiig index.

However, (51 does not provide an expression for the relationship between the tracking
index and these time constante. It does, on the other hand, provide the graphs for the
normalized time conatants vs log(A2), as shown in Fig. 3.

Finally, it k worthwhile to note that for small a, the cr - @ relationship mentioned

previously approaches the classical relationship [S]:

When the filter parameters are adjusted according to the above equation, the filter pro-
vides the best transient following capabiity for a constant velocity object, while simul-
taneously providing the best minimum variance estimate of position and velocity of any
15
fixed parameter Wter [lo]. The following chapter will discuss implementation details of the
elected filter.
 CHAPTER 3

General Implementation of the Elected Algorithm

The cr-8, a-@-7filters, and the Two Point extrapolator have been implemented in the
simulation program. The equations for each filter are provided in Appendix I.

These three filters were implemented as an illustrative tool during our study. Then the
a-@ filter was chosen to perform the tracking, while an a-8-7 filter was implemented in
the learning procedure devised for inferring the dynamics of the moving object and hence

optimizing the a-8 filter used for the actual tracking.

The following sections will consider both IYters since both are used by the simulation

Pww"l.

3.1 Filters Initialization

The t r d i index, A, plays a major role in determining the transient and the steady
state g a b of the a-/?and a-b-7 filters. It is a dimensionless parameter proportional to the
ratio of the position uncertainty due to object accelerations (maneuvers) and to that due

to detector measurement errors [5], i.e.

17
 position uncertainty due to acceleration
tracking indez a
position measurement uncertainty

It ia defined by three primary object trackiig modeling parameters: track period (time

bet ween position samples), object maneuverability (characterized by object accelerations),

and measurement noise (51:

T, the track period always has a known value. h for determining a, and a,, one would
need to have an a priori knowledge of the defection mechanism and the object dynamics

qectively. Obtaining the values for these parameters will be discussed later on, but for
now we will assume that these values are available to ua and hence we have a value for the
tracking index A. Once A is known, we are able to calculate the filter transient and steady
state gains.

Initially, the filters gains are assigned to the following values:

For the a-b filter:

For the a-8-7filter:

These values were obtained from the initiation of the smoothing process (shown in the
following section).

The program then calculates the transient gains at each sampling interval until the op-
timum steady state value, corresponding to that gain, has been reached.

The transient gains for the gain parametera a, @,and 7 are based on the reclvsive

approximations that were given in Sec. 2.2 and are repeated here for convenience (with the
addition of the third state parametens, i.e. acceleration):

a(k) = a(k - 1) + Gala* - a(k - l)]; Gu = 1 - e-'ILa

P(k) = P(k - 1) G@[B'- B(k - I)]; Gp = 1 - e-"'~

+
7(k) = ~ ( -k 1) + G,[y - ~ ( -kl)]; -
G, = 1 e-'/'~

where,

a(k), @(k), and 7(k)are the gains for the present state

-
a(k - I), @(k I), and 7 are the gains for the previous state

a * , B+, and 7' are the steady state optimal gain values.

k, , ka, and k7 are the &st order time constants of the cormponding gain e x m i o n

19
from its initial to its optimal steady state value. It has been shown in 151 that these time
consfants are strictly a function of the tracking index. However, [5] does not provide an
expmion for the relationship between the tracking index and these time constants. It does,

on the other hand, provide the graphs for the normalid time constants versus log@), as

shown in Figure 3.1. (The normalized time constant values were numerically approximated
by wing the KaIman filtering process).

In order to make these graphical relationships available to the program, one could divide
the t d i n g index range into subranges and provide the normalized time constant values

for each subrange endpoints (nodes) and estimate the values within the subranges, through
interpolation. This is required because the program will need these time constant values to
calculate the gains, and it needs to do so for any tracking index value.

The most suitable interpolation method for this purpose is that of Cubic Splinea [13j.This
is one of the most common piecewise polynomial approximations. It uscs cubic polynomials

to interpolate between each successive pair of nodes. There are several reaa011s for choosing
this type of interpolation, among them:

(1) The oscillatory nature of hi$ degree polynomials used to approximate an arbitrary

function on a closed interval, and the fact that a function over a small portion of the interval
can include large fluctuations over the entire range, restrict their use when a smoother
approximation is desired.

(2) With a simple piecewise linear interpolation, there is no atmmnce of differentiability

at each of the subinterval endpoints, i.e., the interpolating function will not be smooth at

these points.
Figure 3.18 Fimt-Order Time Constatfor a-@ Filter ( h m 151).

Figure 3.lb First-Order Time Constabfor cr-p- 7 Filter (from 151).

(3) Since the cubic splines interpolation uses a cubic polynomial at the subintervals, it
will not only insure that the interpolant is continuously differentiable, but also that it has
a continuous second derivative on that interval.

A final note on Cubic Splines is that either one of the following set of boundary conditions

should be satisfied:

(1) Free boundary: The second derivative at the interval end points are zero.

(2) Clamped boundary: The k t derivative of the interpolant at these end points are
equal to the function first derivative at these points.

For simplicity, the free boundary condition has been used in the program.

Now, we d d b e the process of obtaining the optimum steady etate gains. For the
a-fl filter, the relationship between a and the tracking index had to be derived from the

following:

The derivation gives the following result (by eliminating p):

which has the following solution:

 Initialize:
obtain parameters
T,a,,, a,,, a,
k=O

I Cakulote tcantient gaina for I

Fignre 3.2 The a-j3 Filter Gains Calculation Procedure.

 The second solution will give a < 0; therefore, it is ignored.

As for the a-&r filter, the relationship between a and the tracking index was derived
form:

The derivation gives the following result (by eliminating and 7):

We mhe the above equation numerically for a wing the Newton Raphson method [13]
which givea the positive real root that we are seeking (which lies between 0 and 1.)

Fig. 3.2 shows a flow diagram for obtaining the atens gains.

3.1.2 Initiation of the Smoothing Procese

Each filter uses a different initial value for the smoothed eetimatea; the procedure for

obtaining heee values waa shown in (51 to be MSE optimal.

The a-8 filter uses a two-point measurement process for the initial position and velocity
estimates:
 The a-8-7 filter uses a threepoint measurement initiation process:

where z(i) is the measured position at the ith sampling period.

3.2 Case Study Simulation model

A case study was used to aid us in the understanding of the filters behavior. It also aided

us in testing our developed techniques as in the caee of the learning process, we developed,
to be used for determining a, of the moving object. When we chose the simulation model

for this purpoee, we had two objectives in mind. The first was to have a model that tests
the robustness of each filter; the second was to have a model where the primary tracking
modeling parameters could be calculated in closed forms. The latter, helped us in perform-

ing the simulation under a controlled environment which is an important requirement for
testing the filters and developing other techniques.

The chosen simulation model consists of sinusoidal oscillations both in the X and Y

directiom. The frequency of the Y millations was chowm to be an integral multiple of

that of the X direction. Therefore, the motion in the X-Y plane will be cyclic (forming
Liajous patterns) with sinusoidal velocity and acceleration in each direction. This model
will test all three filters for maneuver following capability and prediction accuracy. Given
below are the model equations:
23
where f and are the amplitude factore for z ( t ) and y(t) respectively.

An approximation to the maneuvering standard deviation is given by [IS]:

i.e., it is ' of the maximum acceleration value. This is to be expected since the probability
3
density function takes the form shown in Figure 3.3.

As for the sampling interval T, it could range from 20 to 50 samples of a full cycle of the

fastest sirmsoid, i.e.

The measurement noise which is awumed to be white gauaaian, has a standard deviation
equal to a small fnetion (A- $)of the maximum sinusoidal amplitude, i.e.

Substituting the above d u e a of T, a,, and a, for the tracking index:

24
Figure 3.3 Probability Density Function of the Acceleration in the Simulation
Model (where A b the maximum acceleratirn, from [IS]).
3.3 Simulation Resulta

To test which of the three filters has the best performance, we calculated the standard
deviation of the difference between the predicted and model d u e s . Table 3.1 shows these

standard deviations for some of the runs. As expected, for this continuous acceleration

model, the a-8-7 Eilter had the e m r smallest standard deviation.

Except when the tracking index is relatively high, the program starts the filtering procees
by implementing the transient gains. Had not this been done, the steady state value of or
(corresponding to a moderate-blow tracking index), which in this case could be excessively

small, would have been used to perform the tracking during the transient state. The

latter would make it difficult for the filter to catch-up with the next measured position.
The transient gains in the initial state proved to be very useful since they provide larger
gain values (and then fall down to the optimum steady state dues.) Thb means that

initially, it is more important to catch-up with the next position rather than to filter out

the noise. With each predicted position, the filter gains are further reduced (to allow for
more smoothing of the measured position and to get a better accuracy) until the steady
state value, corresponding to each gain, has been reached.

It was mentioned earlier that in our application, the moving object will not perform
severe maneuvers; thus, the superior maneuver following capability of the cr-/3-7 filter will
be somewhat wasted. This was one of the deciding factors for choosing the a-B filter over the

a-8-76lter. In addition, it was shown in [5] that the accuracy of the prediction/smoothing
25
 Two Point Ehtnpol;rtor a-p-7 Filter

Table 3 . 1 Error Standard d e v i a t i o n s - p r e d i c t e d

p o s i t i o n values form model p o s i t i o n values.
process improves as a decreases. Since for a given A, the a-fl filter provides a smaller

optimum a than the one provided by the a-@-7filter (for the same A - as can be seen from
Fig. 3.4), we preferred to use the a-j9 hlter.

In the next chapter we will discuss two major remaining points. The first is how we
provided the a-p filter with an adaptive learning capability. The second, is how this fitter

was tailored for our application.

F i y e 3.4 Optimal Position 'Ihckhg Gain a V e m W i n g
Index A (from [S]).
 CHAPTER 4

'hiloring the Elected Algorithm to the PVM-1 B a d

Robotice and Computer Vhion System

In the previous chapter we wumed that the three fundamental tracking modeling pa-
rameters a,, a,, and T were available to us and we deferred the discussion on how we obtain

these parameters to this chapter.

In order to make our filter practical to use, it would be necessary for us to provide the
filter with these parameters. Determining the first parameter, a,, depends entirely on tihe

moving object dynamics; the second parameter, a,, depends on the method that gene-
the object position valuea. The third parameter, T, ia simply the sampling period, i.e. the

time between mcceesive position samples. This paremeter is always known and in our case

it is the reciprocal of the frame rate (i.e., & sec).

The following sections will explain the procedures for obtaining the two parameten a,

and a,.

4.1 Determining a,

a, is a quantity that reflects the uncertainty of the moving object position due to its
acceleratiom. The degree of accelerations may vary from one moving object to another.
How do we determine a, for any moving object that might be of intereat to us? The idea
of a "learning" tracker evolved when we tried to answer this question. This means that
27
prior to performing any track'mg, an approximation to a, will be cdculated off-line using

position data obtained as a typical test run. Then the cr-/3 filter will use this value for future
tracking.

We begin our explanation of the process by introducing the underlying basic concepts

behind our method:

Let a be a random variable that could take any value in the range fA; where A is the

maximum acceleration that the moving object undergoes. Let us assume that we have N
consecutive position samples of the moving object. We can calculate the acceleration at

each point using the two preceding position points in addition to the current point, i.e.

where

T is the sampling period

z(i) is the poaition value at time interval i

Note that our notation z(i) represents a general position point value, not necesrrarily the
X-coordinate; i.e. s ( i ) implicitly indicates X or Y. a, is derived for both directions X and

Y; each has its corresponding a,, i.e. a,, and a,, . We will use the notation a, to represent
either direction.

The variance for this random variable is given by:

28
which is estimated as:

The resuiting a, value would have been used to calculate the tracking index for the

tracking filter had it not been derived from the a; values, because the a; values were derived

from the unreliable noisy object position data. However this d u e could be das an initial
approximation to the actual oa.We can obtain a better approximation if we use a &red
version of the noisy data points. Here, the a-#?-7filter comes to play.

The a-fl-7 filter is run iteratively with each iteration using the smoothed position values

that the filter calculated during the previous iteration, That is, at each iteration, the filter
produces the object position values had there been a lesser amount of noise present in the
data (than that present in the data of the previous iferation). The quality of filtering-out
the noise depends on the values of the filter gains, a,j9, and 7. These gains are calculated

from the tracking index which in its turn is calculated from T, a,, and a,. Let us assume

that on is known at this point. We how the value of T and we have an initial approximation
to v,. When the a-8-7 filter is run on the noisy position data for the hat time, it uses
the initial approximation of a, (which we derived above) to calculate an initial value of
the tracking index. Then it calculates the corresponding gain values and usea them to
calculate the predicted and smoothed position, velocity and acceleration values, For the
second iteration, the filter uses the smoothed acceleration values from the first iteration
to calculate a new improved approximation for a, and the process is repeated again. The
29
next iteration will produce even better values because the newly cdculated a, is a closer

approximation than its previous value. The whole proce-w (see fig. 4.1) is repeated until
the newly calculated a, asymptotes to a d u e that is closest to the actual 0,.

We tested the above procedure using the simulation model (discwed in the previous
chapter). The resulting a, was identical to the theoretically calculated value (as in sec

3.2) without the presence of noise. As we increased the noise level, the deviation from the
theoretical value increased.

a, is a measure of the level of the uncertainty in the object position due to certain e m m

introduced during the p i t i o n measurement proms. Therefore, this quantity is entirety

dependent on the measurement method. In our case, we use the existing moving object

detection algorithm that was discussed in Chapter 1, to supply our frackiig filter program
with the moving object position; hence, u, corresponds to the errom introduced in this

detection algorithm.

Recall that detecting the moving object may occur at any one of the five pyramid levels,

(once an object has been detected, the algorithm calculates the center of the object and
passes that as the position value). Each pyramid level has a corresponding a,. For example,

one does expect the noise level in the lower mlution image to be greater than that in a
higher resolution image. A safe assumption, (that was later proven to be correct), was that
the noise level increases by the same factor at which the image resolution decreases, e.g.
the noise level in a 32 x 32 resolution image is approximately 4 times that in the 128 x 128

resolution image. Then it is clear that once we obtain a, for any one of the pyramid levels,
30
 cddmk a. fmm I rat w pazttoa dua panu

I * - - c . M a u a', 8',1'

A
'-+
cdcul.u new a.,. a., u a q rmoolhcd vcdmtim nha

cakulale v.., 0..

' The pmcrdure lor cdculaing (b. l n r i a r gain .h& ore r t h l dmm i Fi8. 3.2
with an ulditimd pis 1.
"Nomewrcl~tdb.ds.b.urrnur+(b.wcolc11~dY.POibl.

fim4.1 C a k u h k Rocsrtore for a,

it will be a matter of trivial scaling that will provide us with a, for each level.

The measurement noise present in any of these levels is mainly due to quantization noise?

which ie carried over to the calculation of the object center.

Let us consider on for the 256 x 256 original image. We will perform our derivation for

a one-dimensional model since, in our case, a, in the X-direction ia identical to that in the
Y -direction.

Our model is shown in Fig. 4.2. Let L be the length of a stretched wire. Let X I and xz
be the p i t i o n of the end points on the X-axie; and let z1, and 2~~be the positions of the
end points after quantization. Let c be the difference between x, and x (i.e., x, = x + c).
This is a random variable whose probability denaity function (pdf) is uniformly distributed
between +$, where A is the quantization step (see Fig 4.2b).

We would like to derive an expression for on b a d on the variance of the estimate of the
, represent this quantity. We proceed aa
object center (here the center of the wire). Let 6

follows:

Thus we can express ,&,, in terms of the actual center of the object with an additive e m r
factor i(cl + c2), i.e.:
Figure 4.2a Mode1 Used in Calculating tbt -ion Noise. L Here i the
A c t d Length Before Qurntbg.

Figure 4.2b P r o W i t y Density b c t i m of the Quantization Noise.

 The expected d u e of P, is:

therefore,

hence,

Thia gives us the following result:

 Hence, the value of a, for the 256 x 256 original image is A where A = 1/256. As for
SJg'
the other pyramid levels, a, is simply a 2,' multiple of that in the original image, where

n is the image level. That is, a, for the second pyramid level is
%; for the third level,
a; etc.

A final note is that if the moving object is being detected in the third level, for example,
our tracking algorithm uses the an that corregponds to this level to calculate the t d i g
index. It is a simple matter for the detection algorithm to pass the pyramid level value

since it is a known quantity.

These results were tested on position data files that the detection program produced
(from the Pyramid Machine - IBM R!I' setup). Our tracking algorithm performed best
when we used the proper a,, , i.e., the one that corresponded to the pyramid level where the
object was detected.

A typical image frame, obtained by our syatem, is shown in Figure 4.3. This ia the
original 256 x 256 image which is a part of a series of frames from which the position data
of the detected moving vehicle was obtained. For this particular frame, the proper o
. is a
which corresponds to the fourth (last) pyramid level. The vehicle image can be detected

at this level because it occupies approximately two pixels out of 16 (in the fourth level), as
seen from Figure 4.3.
Figure 4,3 A Single h e h m a Series of Fhunes Obtained by the
Pyramid Macbhe (from whkh the poeition data of the
detected moving vehicle waa obtained).
 CHAPTER 5

Conclusion and Recommendations for

Future Work

We studies several tracking algorithms in order to provide a PVM1- based

robotics and computer vieion system with an adaptive tracking capability. The
examined fllters were: The Kalman, Wiener, a-B and a-8-7 filters, and the Two
Point extrapolator. The decision criteria followed in chooslng the appropriate
algorithm was based on two objectives. The flret was to obtain high performance
accuracy in both transient and steady state track periode; the second was to
chooee a tracking algorithm with low real-time implementation coet.

The comparative etudy led us to chooeing an optimal a=@fllter that realizes

the accuracy of the Kalman alter. The optimallty of the adopted fllter stems
from the method used in producing the filter gains. Thie method is based on the
@TrackingIndex" parameter which is proportional to the ratio of uncertainties
due to object acceleration8 (a,) and that due to meaeurement nobe (a,).

In order to determine a,, we developed an adaptive method for inferring

object dynamics in an iterative learning procese. We ueed an a-8-7filter for
t h b purpoee. The gain parameters of the latter filter are also determined by the
tracking index; hence performing optimum smoothing. The reeulting smoothed
acceleration values are used to calculate the acceleration variance. The latter
calculation is an integral part of this learning proceea.
34
 As for a,, we learned by examining the existing moving-object detection algo-
rithm, that the present measurement noise is mainly due to quantization effecta.
Hence, we were able to derive an expression for a, which inherently includw
the quantization noise effects as they are introduced through the detection al-
gorit hm.

Having u, and 0, as known quantitiee, we were able to use an algorithm as

eimple as that of the a- B fllter, without increaeing its complexity and implemen-
tation cost, to realize the accuracy of the Kalman filter. In addition, providing
our system with an adaptive- learning tracker introduces the poseibilitiea of
using it in numerous robotics and computer vision applications.

We recommend the following for hrture work:

1. Provide the a-p fllter with optimum on-line adaptive learning capability.

Thie is useful in mult i-mwing-object environment where a priori information

on the object dynamics is not available.

2. Extend the tracking algorithm to have the capability of tracking an ob-

ject in a multi-moving-object environment. It has been shown in [la] that the

tracking index plays a mdor role in the probabilistic poeition data -to- moving
object association problem.

3. Combine the tracking algorithm with feature extraction capability [17]

to single out a particular moving object out of the eame aeeemblage of moving
o bjects.

4. Use the tracking algorithm in conjunction with binocular imaging to com-

pute the 8-D parameters of rigid motion.

35
 REFERENCES

[ I ] Ballard D., Brown C., Computer Vi~ion,chap. 1, Prentice-Hall 1982.

.
[2] Bandopadhay A., Aloimonous J , "Active Navigation: 'hacking an Environmental Point

Considered Beneficial,"IEEE Motion Workshop,1986.

[3] Van der Wal G., Sinniger J., 'Real time pyramid transform architecture," SPIE Vol.
579 Intelligent Robots and Computer Vision,1985.

[4] Anderson C., Burt P., Van der Wal G., 'Change detection and trackiug using pyramid

transform techniques" ,SPIE Vol. 579 Intelligent Robots and Cornput et Viaion, 1985.

[5] P. Kalata, T h e tracking index: A generalized parameter for cr-8 and a-8-7 target
trackers," IEEE Iltaw. on Aerospace and Electronic Systerna, Vol AES-20, March 1984, pp
174-182.

(61 Morrison N., Tracking and Smoothing.In Eli Brookner (Ed.), Radar Technology, Dedham,

Mass., Artech House, 1977.

[7] Brookner E.,The Kdman Filter.In Eli Brookner (Ed.) Radar Technology, Dedham,
Mass., Artech House, 1977.

[8] Singer R., Behnke K., " Real-time tracking filter evaluation and selection for tactical
36
application^,^ lEEE Ram. on Aerospace and Electronic Systema, Vol. AES-7, No.1, 1971.

[9] Ziemer R., h t e r W.,Principles of Communication: system^, Modulation , and Noise

, chap. 4, Houghton MiWi Company, Ma., 1976.

[lo] Benedict T., Bordner G., 'Synthesis of an optimal set of Radar-while-scan smoothing,"

IRE Trana. on Automatic Control, Vol. AC-7, No.4 (July 1962, pp. 27-32.

Ill] Fitzgerald R., 'Simple h k i n g Filters: Closed-Form Solutio4 IEEE Trana. on

Aerospace and Electronic Syatem, Nov. 1981, pp 781-785.

[12] Acharya R., 'Dynamic Scene Model: An Estimation Theory Approach," IEEE Conjer-

ence on Pattern Recognition and Image Proceaaing,Las Vegas, Nevada, June 1982.

[13] R. Burden, J. Faires, Numerical Analysi8,third ed., Prindle, Weber & Schmidt publish-

era, MA., 1985.

[14] Skolnic M.,Introduction to Radar Systema, second ed. McGraw Hill, N.Y., 1980.

[15] Schleher D.,Artomatic Detection and Radar Data Proceaaing, Artech House Inc, Ded-

ham, MA, 1980.

[16] R. Fitzgerald, 'Development of practical PDA Logic for multitarget tracking by micro-

processor," Proc. 1986 American Control Conference, June 1986, pp.889-898.

37
[17] Gorin Allen, 'Aspect- Based aircraft classification from Dynamic Imagery,"lEEE Con-
ference on Pattern Recognition and Image Procesa:ng,Las Vegas, Navada, June 1982.
 APPENDIX I

Basic Equations of the Simulated Tracking Algorithms

Prediction:
z(k + 1Jk)= z(k(k)+ Tv(k(k)

Correction:

(definitions will follow the a-fl-7 filter equations)

(2) a-8-7Filter

Prediction:
Correction:

z ( k l k ) is the position estimate at time interval k

x(k + Ilk) is the position estimate at time interval k t 1 given k samples (measurements)

v(klk) is the velocity estimate at time interval k

a(k lk) is the acceleration estimate at time interval k

a ( k ) is the position tracking parameter at time k

p ( k ) is the velocity tracking parameter at time k

~ ( kis) the acceleration tracking parsmeter at time k

T is the sampling period

z(k + 1) measured polsition at time k + 1

(3) Two-Point Extrapolator

Prediction:
x(k + l ( k )= z ( k l k ) + Tv(k1k)
Correction:
 APPENDM I1

THE TRACKING PROGRAM

PROGRAM t r a c k ;
I * This program performs tracking by using a Two Point
Extrapolator, an Alpha-Beta Filter, or an Alpha-Beta-Gamma
Filter. The tracking may be performed on variations of the
simulation model or on a binary file of position data points.
The adaptive-learning capability is also available as an
option. 1 *
(::(INST
max length = 150; (* maximum number of data points for use as
a fading memory filter. This limit is
for simulation purposes only.+)
m color = 1;
p r -color = 2;
model color = 3 ;
[Tks following a r e i:onstants related to the menu o p t i ~ r i s+ )
+

csme-opt tor calculating maneuvering sigma estimal,c . k )

(*
cmse-opt = 1 ;
( * catd opt for continuous acceleration tracking d e r n ~ ~* )
catd opt = 2;
( * spopm-opt is for tracking using series of positions
obtained by the Pyramid Machine. * )
spopm-opt = 3 ;
exit-opt = 4;
(* sigma-n 0 is the quantization noise in t h e origirial 256x256
image 1 *
sigma-n I) = 0.2C14;
TYPE
string - t , y p e = stringll27 ;
array-._type = array[l..max-length] of real;

VAH.
11 The following arrays store the filters gains f n r t,ht? x a n d
y directions. *)
gain .x, gain_-y : array r2. . 3 , 1 . .3j of array -type;
gain-opt--x,gain-opt - y : array12. . 3 , 1 . - 3J of real ;
gain--type : array[l . . 3 ] of string-type;
(* The following arrays are used to indiclte il a p-tri-if:~~l~ir
gain of the fading memory filters has reached ste,-1Jy st;lC,e
or not. * )
steady _state_ x ,
steady_state._y: array[Z..3,1..3] of boolean;
( The following are first order time coristarits 1
+. r Ltj
the filter gain excursion from its initial stat#-2 LCJ its
optimal state value. * )
k--taw-x : arrayL2..3 , l . .3] of real;
k t a w - y : arrayf2..3,1..3]of real;
( 6 The f~llowing variables are satandard devialions for
maneuvering accelerations and measurement noisc. 4 )
 sigma-a-.x, sigma--a. y ,
s i g m a _ n . - x , sigma-n-y : real ;

t r a c k i n g - i n d e x - - x , t r a c k i n g - . inde:~:-;. : r e a l ;

c The f o l l o w i n g v a r i a b l e r e p r e s e n t s C11e '~!;,)r . I I ill

~ 1 t*\,rt;J
w h e r e t h e o b . j e c t is b e i n g d e t e c t e d . Tht. v l l u ( . I u:-
a s s i g n e d by t h e u s e d . Normally, howcvcr, t i II : r i 11
be p a s s e d by t h e e x i s t i n g Mcjtiun i k t e c t i o n 1 2 r t j g c - t n 1 t l izl
values f o r t h i s variable a r e 0 . . 4 k )
Pyramid--level : i n t e g e r ;

( The t o l l o w i n g a r e t h e m e a s u r e d , p r e d i c t e d ,
k i smr-1 ]tiic<i
o b . i e c t p o s i t i o n a n d v e l o c i t y f o r t h e x a n d y d i r e t ~ l - i t - l r l a+ )
m x, m y : a r r a y _ t y p e ;
P. x , P-Yl

s _ - x > S-_Y,
m.-v-.x, p-v-x, s_-v_ x ,
m v . y , p v - y , s v y : a r r a y r l . . 3 ] of a r r a y t y p e ,
s-a-x, p a x ,
s_a . - Y , p a Y : a r r a y - t y p e ;

I The f o l l o w i n g a r e t h e s i m u l a t i o n model p o s i t . i o n ~r:.ill,res. t 1

model - x , model.-y : a r r a y - t y p e ;

(* T i s t h e s a m p l i n g i n t e r v a l between t i m e k a n d k t 1 'l'k~
E:
sampling-factor reduces t h e sampling r a t e by Llle arl$-8t-i~it-
s p e c i f i e d by t h i s v a r i a b l e . . # )
T : real;
s a m p l i n g -f a c t o r : i n t e g e r ;

( e The following v a r i a b l e s arc E I S S C ~ iCa t c d rri L k i Ill..

c o n t i n u o u s a c c e l e r a t i o n t r a c k i n g demo, whi c:l~ uses Liic
simulati(>n model. * )
x-_mag, y--mag : r e a l ;
w : real;
m : integer;
n _ - s a m p l i n g , n--measurement.-noise : i n t e g e r ;
phase : r e a l ;

( * miscellaneous * )
k, ctr, c t r l , ctr2 : integer;
max-runs : i n t e g e r ;
s : string-type;
option : integer;
delay-factor : integer;
filter-option : integer;
p t s f i l e : f i l e of c h a r ;
f ile.-name : stringl-121;
num. of - p t s : i n t e g e r ;

i:*I n c l u d e t h e C u b i c S p l i n e i n t e r p o l a t i o n p r o c e d u r e . t ,

C$I c s - _ e x t r a p .p a s t
FUNCTION Gauss : real;
( R e t u r n s a gaussian r a r ~ d o mvariables by summing 1 2 :;arnpl+-a ~ * t
u n i f ormlly distributed random v a r i a b l e u b t a i r l s d 1L.C,III t 11,.
prede f i n e d Random f u n c t i o n .*1

V ar
temp : real;
i : integer;
Begin ( * gauss *1
tcrr1p : = U . O ;
f o r i : = 1 to 1 2 do
t e m p : = t e m p + random;
Gauss : = temp - 6 . 0 ;
End; (.* gauss -V 1
PROCEDURE get-position( 1 : integer);

!t This procedure reads the the next, ~ s b . i e c t p n i , i t i c ~ r l ( s, IIIJ

\ . J ~ ~ . I I . .

y ) i r o m the specified file. Then it, checks t o s,+.~. 1 I i L i ,: -!

valid point ( based (2n how kar t h e new p o s i t it In i5 i 1:. &in I IIS-.
previous one I . If the point is inva Lid, I Glt F - L - ~ I C ~L ~ I A ~ C
r.al~i..~laiesan approximation to what t11e prt)pr:r. v~ L I 2~ L.ki( l d '{I

have been using linear int,erpolati~~n N,_tCc: t t t t , L i r~ t 1 *r.g&

skips sampling factor-1 points, in e f f e c t I i t.1~~2

sampling rate by sampling.-factor + )
Const
pos-dit f l c t o r = 5 ;
v31-
position_difference : integer;
temp-x, temp-_
y : char;

Begin ! t get-_pc)sition4 )
position _difference : = pos-dif -f actor * sampling i a c t ur;
for ctr : = 1 to sampling-factor-1 do
read(ptsfile, temp-x, temp-y) ;
read( p t s f ile, temp-x) ;
read(ptsfile, temp - y ) ;
if ( 1 2) then
begin
m-x[l] : = ord(temp..x);
m-y[lJ : = ord(temp-y);
if ((abs(m .x[1] - m-x[l-11)) > position_dilt?,.r-nt~e I t h~_tn
m-x[1] : = 2Km-x[1-I] - m-x[l-21;
if ((abs(m_-y(l1- m y[l-11)) > p o s i t i o n c l i l Lk~entcrl:r~l_t~r

m-y[1 J : = 2*m-y[1-1 j - m-y[1-2 J ;

end
else
begin
m--xr 11 : = ord (
temp.-x) ;
m-y[L] : = ord(temp-y);
end ;
End; ( +get -position*)
PROCEDURE calc-sigma-atfirst-time : boolean);

i k This procedure c a l c u l a t e s an estimate 0 tl~..: m:irtc:~i*/l~ring

standard deviation value. When f i r s t t i m e i s I tl~c
p r o c e d u r e u s e s p o s i t i o n p o i n t s d a t a f o r m a t e s L riu 1i l l 2 t . ~ )
obtnir~ the acceleration values. Otherwise, il.. w i l l u:;t: ttlt,
smoothed a c c e l e r a t i o n v a l u e s o b t a i n e d v i a t h e 3 l p h a - h c L : ~ - g s ~ r ~ ~ r ~ a
filter t I

VAR
v x, v y : array type;
sum a x , sum. s _y ,
s u m . s q u a r e d .a_.x, sum--squared-a -y : real ;
i integer;
:
junk f i l e : t e x t ;

Begin (*talc sigma _a*)

sum a x : = 0 ;
sum a y : = 0 ;
sum-squared -a -x : = (3 ;
sum - s q u a r e d a y : = 0 ;
i f (first-time) then
begin
i : z 1;
f o r i : = 1 t o num-of-_pts do
g e t - p o s i t i o n [ i );
f o r i : = 3 t o num o f - p t s d o
begin
v _ x r i - I 1 : = (m_.x[i-11 - m x l i - 2 1 , '1';
v - - x r i l : = ( m . . x [ i l - m - x [ i - 1 I 'I';
s - a x r i - 2 1 : = ( v - x r i l - v - x r i - 1 I ),IT;

v-y[i-I] : = (m-y[i-11 - m - y [ i - Z J j , : ' I ' ;

v _ y [ i ] : = (m-y[i] - m-y[i-13 )/T;
s-a--y[i-21 : = ( v - y [ i ] - v - y r i - 1 J 1 / T ;
end ;
end ;

f :i: Calculate t h e estimate of the man.euvering star~dard

deviation *)

f o r i : = 1 t o num.-of - p t s - 2 do
begin
sum-a-x : = s urn_-a--x + s -a -x j i ] ;
sum-squared-a-x := xl j 1 I ;
sum- s q u a r e d - a _ - x + s g r ( s- :i_
sum.- a _y : = sum-a-_y + s-a--y [ i J ;
sum-squared-a-y := sum _ s q u a r e d - a _y + ~ q r,.;( _-i y 1 i j !;
end ;

s i g m a - a-_x := of- _ p t s --
s q r t fs u m - _ s y u a r e d _ . a - _ x / r ~ u m
s q r ( s u m a x'nurn Q L ~ I S ) ) ;
sigma -a--y : = s q r t (sum-squared ..a yJnum of - p t s
s q r ( s u m - a y/num of p t s ) 1 ;
 writ,eln;
w r i t e l n ( ' E s t i m a t e d m a n e u v e r i n g standard d e v i s t i c l i l i n Lh12' 1;
w r i t e l n O x direction is : ' sigma-a-x);
w r i t e ln;
w r i t e l n [ 'Estimated m a n e u v e r i n g standard devia t i o r ~ i l l t h u ' ;
w r i t e l n l ' y d i r e c t i o n i s : ' , sigma a 5 7 ) ;
w r i t s 1 n:
reset(ptsf ile ) ;
End;( + talc-sigma-a*)
PROCEDURE Get-option;
( r T h i s procedure displa;yrsthe menus a n d ~ b3 i tr 1 s t h k i :Ic. 15: I !:,-I
options. I *
Var
answer : char;
parameter : integer;
Begin ( % g e t option*)
answer : = ' y ' ;
clrscr;
b l r i t e l n ( ' 1 . Calculate an estimate of manel.lverit~c:5 1 t x t ~ c i x r ~ l
d c v i s ~ tor!
i ' ) ;
writ,eln(' from a given test run. ' 1 ;
writeln;
writein( ' 2. (~ontinueousacceleration t r a c l . i n g dr?ni,-)' I ,
writeln;
writeln( ' 3. Track using series of positior~soLlt aic1~3t-l
tl1c:
1-11.

' ,.
;
writeln( ' Pyremid machine' ) ;
writeln;
writeln( '4. Exit' 1 ;
writeln;
write( 'Please Enter optZion number: ' ) ;
readln(option);
if (option = catd ~ p t then
)
begin
clrscr;
while ( answer= ' y ' ) or (answer=' Y ' k do
begin
writ-eln(' The following parameters havo tlls 1u:~ing
1 1 - 1 1

valucs: ' ) ;
writeln;
writeln(' 1. X-Y frequency factor = ' , m);
writeln;
writeln(' 2. Radial frequency = ' , w:S:3),
writeln;
writeln(' 3. Phase difference = ' , phase:S::3);
writeln;
writeln(' 4. Number of samples of a fu1.L c.il:la vf
the i - a s t c r ' i ,
writeln( ' sinusoid = ' , n- sampling ;
writeln;
writeln(' 5. "SNR" = ' , n-_measuremen t no i st3 ;
writeln;
writeln(' 6 . Display delay ( m s ) = ' d e l a y i : - t c C ~ - ~ r ) ;
writeln;
write(' Do you like to change any of the p A r - J n l c t t ~ r s
( y , ; ~ l ) :' j ;

readln ( answer t ;
while (answer='y') or (answer='Y1) d(>
begin
writeln;
 write ( ' Which parameter d o :y.l2~1 1 i k.t-. t 6- 'tiingl;
I1 ' 1,
readln( parameter ) ;
writeln;

-
if (parameter=l) then
begin
write( ' X-Y f reqi-xerlcy t ~ c : t 1.1 t- ' ) ;
readln ( m ) ;
end
else
if (parameter=2) then
begin
write ( ' Radial f r e q u irll ,, = ' 1 ,
read( .w ;
end
else
if (parameter=3) then
begin
write( ' Phase dif'-ff?rt-:rll~t;.= ' ;
readln( phase ;
end
else
if (parameter=4) then
begin
write(' Number of
samplus = ' ) ;
readln (n-sampling) ;
end
else
if (parameter=5) t h e n
begin
write('"SNR" z ' I ,
readln ( n - m e a s u r c n i c - . r ~ C
c ~ ~ - ~ i . - , + >;
end
else
if (parameter = 6 L1lt:n
begin
write( ' D i s p l a y (1e.lsy
(rnsj: ' J;
delay i at: t ~ )) r;
readlr~(
end
else
writeln( ' I n v a l i d
optic~a
writeln;
w r i t e ( 'Do you like t o change anotht-.r
Iy,'n): ' 1:
readln(answer);
end ;
end ;
end
else
begin
clrscr;
 writeln;
write( 'Please e n t , e r test-run file n a m e : 1 ,
readlnt f i l e name I ;
assign(ptsfile, file-name);
writeln;
write( ' How many measured points d~?-3s thl-: t i L C i'-'(.~ntairl" ' ) .
readln(nrxn-of-pts ) ;
reset(ptsfi1e);
writeln;
( t( The following value will be normally p s a a t = ~ ~ L,yl Lilc
existing Motion Detection Program. t )
write( 'Enter The pyramid level at w h i c h tllc c~b,j~?c:t was
d ~ t l - . 't ,- t 2 < 1 : ' ) ;
readln(pyramid-level);
clrscr;
writeln( ' The following parametxrs tlsvf.: Lilt2 icr.11L I L J L I I ~
va1ut:s: ' ) ;
wr'iteln;
writeln( ' 1 . = ',
Sampling rate reduced by -3 l'--l~_+tor
sampling factor) ;
writeln;
writelno 2 . Display delay (ms) = ' , cl(-:lsy fact-or);
writeln;
write( ' Do you like to change any of the psr3rneLcrs
l-t(y r 1 j . ) ;
readln (answer);
while (answerz'y') or tanswer='YY)do
begin
writeln;
write ( ' Whirh parameter do you l i k . . \ t.r_, i : k ~ ~ l l g ~
(1 2 ) : '1;
readln( parameter) ;
writeln;
if (parameter = 1) then
begin
write( 'Reduce the sampling r a t e b y
a t a c t t ) r ~ f :' ' ) ;
readln(samp1ing-fact0
end
else
if (parameter = 2 ) then
begin
write( 'Display delay i nis ' I .
readln(de1ay-f actor) ,
end
else
writeln( ' Invalid option. . . ' ) ,
writeln;
write('Do you like to change a n o t h e r
(y/n): ' 1 ;
readln(answer1;
end ;
end ;
(option = cnise-opt) then
 begin
filter-option : = 3 ;
T : = 0.0:33:3*samplin.g .factor;
calc- sigma pa( true) ;
end;

if (option ( > cmse--opt) t h e n

begin
clrscr ;
writeln;
writeln( 'Which of the f r3l lowing t'iltt,rs yt 1 1 1 id 1i L
;I( , ~ i x
:
LlJ lJ,5L ' ),
writeln;
writeln( ' 1 . Two point extrapolator' 1 ;
writ,eln('2. Alpha-bata filter' ) ;
writeln('3. Alpha-beta-gamma f i l t e r ' ) ;
writeln; write( 'Please enter o p t i o n numt+;t:: ' I ,
readln(fi1ter-option);
end ;
End ; ( *get _ o p t i o n * )
PROCEDURE calc-gains ;
( * This procedure calculates the optimum stea4:v. s l 3 t 2 [ c t;-~irl-, 1 ,r
the Alpha-Beta, and Alpha-Beta-Gamma f i l t e r s t i

Procedure calc-alpha-2(t-i:real;
var opt -gain: real ;
( This procedure calcu1~-tes the optimum steady 5 L:it t- l i p l l ~ t ,,L-
the Alphs-Beta filter. .+f
var
b: real;
root : r e a l :

begin iWcalc alpha - 2 ' g

b : = 0.25*(t i + 8 ) * t , _ i ;
root : = 0.5-*(-b+sqrt(sqr(b )+4:kb)f ;
if (root>O)and (roottl) then
opt -gain : = root;
end; t *talc alpha-2* )
{ t--i : real ;
Procedure calc. alpha_-3
var opt -gain : real ) ;

( t' This procedure calculates the nptimum steady st-at.c -1.lpha lor
the Alpha-Beta-Gamma filter . The Newton-Ftaphsor~mc t l l ~ ~is~ lt l s t c j
here for solving the cubic equation. * )

cons t,
to1 = 0.0001;
var
b , c , d: real;
p a , p : real;
f _ _ p , f .d p : real;
temp: r e a l ;
continue: boolean;

begin (*talc.-alpha-3*)
b : = 0.25*t-i*(t_i-16 ) ;
c : = 0.25*t-i*(48-t-i);
d : = -8*t-i. 3

p@ : = 0 . 5 ;
temp : = sqr(p0);
continue : = true;
while continue do
begin
temp : = sqr(p0);
f-p : = pO*temp + b*temp + c*pO + d ;
f - d - p : = 3*temp + 2*b*p0 + c ;
P : = PO- f..p/f-d-P;
if (abs(p-pO)<tol) then
begin
opt.-gain : = p ;
continue : = false;
end
 else
po : - p ;
end ;
e n d ; f 'tical c.-a l p a - 3 % )

Eegirl ( %ale g a i n s * 1
i f ( f i l t e r option = 2 ) then
begirl
c a l c - - a Lpha-2 ( t r a c k i n g - i n d e x _x, g a i n opL x 1 2 , l 1 ;
gairl_opt_xL2,21 : = 2 * ( 2 - g a i n - o p t - x l t : , l I ) 4
*
s q r t ( 1 - g a i n ctpt : - r ( l ; l , I l ~ ,

calc - a l p h a _2 ( t r a c k i n g - i n d e x - . y , g a i n - o p t j.. 1 2 , l 1 i ,
gain-opt-y[2,2J := 2 * ( 2 - gain o p t - y l 2 , l ) )-
* s q r t ( 1 - gain-opt-;lZ,l I ;
end
else
begin
c a l c - - a l p h a . - 3 ( t r a c k i n g - - i n d e x . - x , g a i n - o p t - xi 3 , l 1 ) ;
g a i n _ o p t _ x r 3 , 2 ] : = 2 :* ( 2 - g a i n - o p t x / 3 , l 1 ) - 4 .
+ s g r t ( 1 - g a i n cjpt :A( ' 3 , 1 1 1 ;
gain-opt--x[3,3] := s q r ( g a i n -opt-x[ 3 , Z I 1 ,
gain opt x [ 3 , l ] ;

c a l c - - a l p h a - - 3( t r a c k i n g - i n d e x - y , gain-upt-i. 1 3 , 1 j ) ;
gain.-opt-.y[3,21 : = 2 * ( 2 - gain-c3pt_yl3,1]) - 4
* sqrt(1 - g a i n o p t _ y [ 3 , 11 ) ;
gain--opt.-yC3, 31 : = s q r ( g a i n - - o p t - - y [ 3 , 21 I /
gain-upt-yl3,ll;
end ;

w r i t e l n ( 'optimum a l p h a x = ' , g a i n - o p t - _ x [ f i l t e r _ + ~ p t i i ,1nI ,

:5:3,;
w r i t e l n ( ' optimum a l p h a y = ' , gain-opt.-y[ f i l t e r o p t i s r r , 1 I
: 5 : :3, ;
readln ;
End ; t k c a l c - - g a i n s *
PROCEDURE initialize-filter;
( * This procedure calculates the maneuvering, n(.$is-r:. A~I~J-AYC~
deviation and the tracking index. f )
Const
In 10 = 2.30258;
Var
log.-index x ,
log-index-y : real;
i , ,i : integer;
Begin ( * initialize-filter + )
if (option=catd_opt) then
begin
sigma -a-x : = 0.707*sqr(wl*x-mag;
sigma-a-y : = 0.707*sqr(m*w)*y.mag;
sigma-n.-x : = x-mag/n-measurement-nr-7i~-~;
sigma-n-y : = y-mag/n-measurement-noise;
T : = 6.283/(n-sampling*m*w);
end
else
begin
if (option=spopm-opt) then
begin
write( 'Enter maneuvering standard c l e v i 3 t i(>rl in
the x direction: ' ) ;
read.Ln(sigrna_a-x ;
writeln;
write ( ' Enter maneuvering s t a n d a r ? (3,-.J i - A ( ic-~ri i n
the y direction: ' ) ;
readln( sigma-a_y1 ;
T := 0.0333*sampling--factor;
end ;
{* The standard deviation value o f the q n r i r ~ li a c ~t, i r l t l
noise is (2*pyramid--1evel)xsigma-n-0 + 1
sigma-n-x : = exp(pyramid--level*ln(2 ) ).#sigma o C) ;
sigma-n-y : = sigma--n-.x
;
end;

tracking_index-x : = sqr (T1*sigma-a-x/sigma--nnnnx;

tracking_-index-y : = sqr(T)*sigma-a--y/sigma--n-y;

if (filter-option < > 1) then

begin
log-index-x : = ln(sqr( tracking-.index-x) ) ,,'111 10 ;

log-index-y := In( sgr( tracking.-index-y 1 );lrl 1U :

 if (filter_option=2) then
i : = '7L
else
j. : = 3 ;
for j : = 1 to filter option do
begin
if ( log index -x > 0 then
steady-state x r i , j1 : = t r . u < -
else
begin
steady-state_x[i , ,j J :: f : i l Z , c .
j l : = cs extr2pc i - f ,
k-_taw-xli,
gain -typeI .i1 , 1 i-+g i r ~ d e : ~) ;
end ;
if ( log -index.y > 0 ) then
steady-state-y [ i , j 1 : = trus-
else
begin
steady_state_y[ i , j 1 : L P s l .=r:.
k-taw-yui, jl : = c s - e x t r - i p t i 1 ,
gain t y p e [ . j \ , Log i n d e x y ) ;
end ;
end ;

gain.-x[%,l,ll : = 1.0; gain_y[2,1,11 : = 1.0;

gain-x[2,2,1] : = 1.0; gain-y[2,2,11 : = 1.0;
gain-x[3,1,1] : = 1.0; :=
gain--yr3,1,3.] l.(l;
gain--x[3,2,1] : = 1.5; :=
gain--y[3,2,1') I .5 ;
gain-.x[3,3,11 : = 2.0; gain-y[3,3,1) : = 2.0;
end;

for i:= 1 to 3 do
begin
if (option=catd-opt) then
begin
model-x[ i J : = x_mag * sin( w*i:*T ) ;
model-yti] : = y--mag cos(m*w*i'KT+pllase) ; *
m-x[:i'l : = model -x[il + sigma-n x E. Gauss;
m-y[i] : = model-yEi] + sigma-n_y t Gauss;
end
else
if (option=spoprn-opt) then
get-position(i);

end ;

( The following are the initial estimates #>f the Tt~v k'c~iilt
extrapolator, Alpha-Beta, and Alpha-Beta (4nmmcl 1 i l t i : r * s ,
respectively. *)
 if t iiltcr-option=l ) t h e n
begin
s-x[l,l] : = m-xr3-j;
s-yCl,Il : = m-yr31;
s-v-x[l.l] : = (m-x[3]-m_x[21 ) / T ;
s-v-yC1,lI : = I m - ~ ( 3 1 - m y[2])/T;
end
else
if i l t e r _ o p t i o n = 2 ) then
( f
begin
s_x[2,1] : = m-x[3] ;
s_yC2,11 : = m _ y r 3 ] ;
s - _ v - x [ 2 , 1 1 : = (m-x[3]-m-x[21)/T:
s . _ v - ~ [ 2 , 1 1: = (rn-y131-m-_yr21 j,'T;
end
else
begin
s _ x C 3 , 1 1 : = m.-x[3];
s - y r 3 , l j : = m-y[3];
s-v-x[3,1-j : = (3*m-x[3]-4~*m-x[2)+m-:.r[l J ),/(2+-T);
s-v-y[3,1] : = ( 3 * m - y [ 3 ] - 4 * m - y [ 2 ] + m _ y / l 11 i i i ( 2 $ T ] i
s-a--x[l) : = (m-x[3]-2*m-x[2]+m--x[I] ),/sqr(T1 ;
s-a-y[lI : = (m-~[3]-2*m-~f2]+rn-y[l]),sqr('I'i;
end ;

End; (*initialize-filter:*)
PROCEDURE Calc-trans-gain( f i l t e r - . i d , g a i n - i d : i n t e,.gt:l ,
~ ; o f > r d i n ~ t .. e ~ f h a r ) ;

(* T h i s p r o c e d u r e c a l c u l a t e s t h e f i l t e r g a i n s t a l p h a , tc Li.1, <iii:.l
gamma 1 d u r i n g t . h e t . r a n s i t i o n p e r i o d . I t r e t u r r t s 2 t . . r t . ~ e~ 3 l a c ~ : .
when t h e s t e a d y s t a t e , o p t i m a l g a i n v a l u e h a s b e c r i rc:a!:llcJ,
f a l s e o t h e r w i s e . :%)

Corlst
error-percentage = 0.1;
B o g i ~( ~
Xcalc -t,rans-gain* )
i f (coordinate = ' x ' ) then
begin
g a i n - x [ f i l t e r - - i d , g a i n - _ i d k]
, : = g a i n - - x l f i l l,oc i l l ,
g a i n - - i d , k-11 + ( 1 - e x p ( - l / k _ - t a w - x r f i l t e r - i d ,g ~ i ni d 1 I I
tr ( g a i n _ - o p t- x [ f i l t e r - i d , g a i n _ _ i d l - g a i n t i l t c r ill,
g 3 i u i d ,k- 1 ] ) ;
i f (abs(100*(gain_x[filter__id,gain_id,kj-
gain--opt -xrf i l t e r - - i d .g a i n i d j I /
gain-opt- filter i d , g a i n i d ] r .
e r rt L I Lllcn
s t e a d y - s t a t e -x[f i l t e r - i d , gain- i d ) : = t r w 2 ;
end
else
begin
g a i n - y r f i l t e r - i d , g a i n - i d , k l : = g a i n - y [ f i l L + - ? ri l l ,
g a i n - - i d ,k - l ] + (1-exp(-l/k_-taw-y[filttr i 4 , g a i n i d 1 r )
+- ( g a i n . - o p t _ y [ f i l t e r - i d , g a i n i d ] - g a i n - y 1 f i l t ~ l ri d ,
gilicl i d , k- 1 1 1 ;
i f ( a b s (lOO*(gain-y[f i l t e r _ i d ,g a i n _ i d ,k ] -
g a i n _ o p t _ y [ f i l t e r _ - i d ,g a i n i d ] i i
gain-opt-y [ f i l t e r - i d , g a i n - . i d I ) <
error-percentage) then
s t e a d y - s t a t e - y [ f i l t e r - i d , g a i n - i d ] : = LI-ue;
end ;
End ; ( f c a l c - _ t r a n s - g a i n * )
PROCEDURE initialize;
Begin ( * initialize
gain-type[l] : = 'alpha';
gain-type[2j : = 'beta';
gain-typeC31 : = 'gamma';
delay -fact.or : = 0;
m := 2;
w := 6.28;
phase : = 1 . 5 7 ;
n- sampling : = 20 ;
n -measurement-.noise :
sampling_f actor : = 1 ;
- 256 ;
x-mag : = 1 2 5 ;
y .mag : - 75;

get-option ;
End; ( * initialize ;+')

PROCEDURE add-measurement-noise;
(* This procedure adds noise t o the ideal model .Lr-,itntiun. 'I'he
noise is white zero mean. * )

Begin
m-xlkl : = model.-x[k] + sigma.-n-x * Gai~ss;
m-_y[k] : model--yrkl + sigma-n-y :# Gauss;
End ;
PROCEDURE p r e d i c t - l ( k : i n t e ~ e r ) ;
I f The p r e d i c t i o n i n t h i s p r o c e d u r e i s based o n t h i t elf thz-: 'I'675-,
Point Extrapalator. * )

B e g i n ( '*predict.-1.r:)
p _ - x [ l , k ] : = s _ - x [ l , k - 1 1 + T*s_v-x[l , k - 1 1 ;
P ~ C l , k l: = s - - y [ l , k - l ] + T * s . v - y [ l , k - 1 1 ;
p-v_xll, k l := s--v-x[l, k - l ] ;
P - v - Y I ~ , ~: =~ s - v - y [ l , k - 1 1 ;
End; ( *predict-1 r )

PROCEDURE s m o o t h - l ( k : integer);

(* The s m o o t h i n g i n t h i s procedure i s b a s e d on tkial ,-)i t h e Two

Point Extrapolator. * I

Begin (*smuoth-1%)
s - x [ l , k ] : = m-x[k];
s - y [ l , k l : = m-y[k];
s - v - x l 1 , k I : = (m-x[k]-m-x[k-11 )/T;
s - . v - y r l , k ] : - (m-yCk1-m-y[k-l])/T;
E r t d ; (*smooth-.l*)
PROCEDURE predict_2(k:integer);
('+ The predicition in this proc:edure is based on that L~ the
Alpha-Beta Filter. * )
Begin t +predict--:!r:)
p - x i 2 , kl : = s-x[2, k-I] + T*s-v--x[2,k-I];
p_ Y I 2 , k j : = s-y[2,k-I] + T*s-_v-y[2,k-ll;
p - v - x [ 2 , k ] : = s-v-xr2,k-11;
p _ v - - y l 2 , k l : = s-v-yr2, k-11;
End ; ( rpred ic t - - 2 ' k )

PROCEDURE smooth-2 (. k: integer) ;

(* The smoothing in t.his procedure is based on t h a t , ,.)1' 1111;- A.lpl-la-
Beta Filter. , # )
Var
dif f ererice.-.x : real ;
difference-y : real;

Begin (*smmoth-2r)
difference-x : = m-x[k] - p-x[2,k1;
difference_y : = m_y[k] - p-y[2,k];
s_x[2,kl : = p-x12,kl + gain-x[Z,l,k]*difference x ,
s - y C 2 , k J : = p-yt2,kl + gain-y[2,1,k]*difference-y;
s-v-x[2,kl : = p-v-x[2,kl + (gain-x[2,2,k]/T)+differe11~:e x ;
s _ v _ y C 2 , k I : = p-v-y[2,kl + ( g a i n - y [ Z , Z , k ] / T ) ~ d i t f ~ r : ~ -y ~; ~ r ~ ~ ~ ;
End; (*smooth-2*)
PROCEDURE predict_3(k:integer);
( t- The predicition in this filter is based 011 I hx t t ht-: A lphl
Beta-Gamma Filter. 3 )
Begin ( *predict-3*)
p-xf.?,kJ: = ~._~[3,k-ll + T*s-v_x[3,k-l1 + U . ! l t j q r ( ? ' , t s -J x l k . 1 1 .
~ - ~ [ 3 , :k= l s-y[3,k-l1 + T*s v-.y[3,k-Il + C ) . ~ + S ~ Y ( T 1)1+j 'E 1J.q 11;
p_v--xr3, kl : = s _ v _ - x [ 3 k-I]
, + T*s--a_-x[k-l
J;
F.-v-Y[Y ,kl : = s_v_y[3,k--ll+ T*s-a-y[k-I];
p-a-xrk] : = s-2-x[k-l];
p _ a - y / k l : = s a y[k-11;
End; ( *predict-3*)

PROCEDURE smooth_3(k:integer);
(* The smoothing in this filter is based on that; of t,he Alpha-
Beta-Gamma Filter. * )
V a r
difference-x : real;
dif f erence-y : real;
Begin ( *#smooth_3*)
d i f f erence_x : = m.-xlk] - y - x [ 3 , k] ;
difference--y: = m-y[kl - p--y[3,kl;
s - X I - 3 , k l : = p--x[3,kl+ gain--xr3,l,kl*differenr.e_;,.
s-yC3,kl : = p-y[3,kl + gain--yC3,l,k]*differenc'cCy;
s_-v-xr3,k] : = p-v-x[3,k] + ( g a i n - - x [ 3 , 2 , k ] / T ) * d i f ~ c r c r 1 1:,; ~c
s - v - Y ~kl ~ , : = p-v-Y [3,k] + (gain-y[3,2,k J / T )k d i t i c r t - . n ( . t . . y ;
s-a_-x[kl : = p-a--x[kl + gain--x[3,3,k]/(Z*sqr(?') ) + d i tLf~:r:?~ic+t3x ;
s-a-y[kl : - p--a-y[k] + g a i n - y [ 3 , 3 , k ] / ( 2 * s q r ( T ) ) * d i ~ F ' ~ ~ r5,;
~11~~~
End; ( Ksmmoth-3*)
PROCEDURE get-statistics(x1, yl, x2, y2 : array_-typl;~;
(:I: This procedure calculates the standard d e v i a t i t - ! n s 1r.f the
predicted and model values in both the x and the y dirir!: t . i l ~ a s .
:*: )

Var
zzave._x,zzave_y,
zave-.x, zave--y : real;
i : integer;
sigma-x, sigma-y : real;
n : integer;

Begin (,*get statistics*)

if (option<> catd-opt) then
n : = num-of ~ p t s
else
n : = m*n.-sampling-1;
zzave-x : = 0; zzave-y : = 0;
zave_x : = 0 ; zave-y : = 0;
for i : = 2 to n d a
begin
zave--x : = zave-x + (xl[i]-x2Ci.l);
save.-y : = zave-y + (yl[i]-y2[i]);
zzave-x : = zzave-x + sqr(xl[i]-x2ril);
zzave--y: = zzave-y + sqr(yl[i]-y2[ij);
end;
zave_x : = zave-x/n ;
zave-y : = zave-y/n;
zzave._x : = zzave_-x/n;
zzave_y : = zzave-y/n ;
sigma-x : = sqrt(zzave-x - sqr(zave_x));
sigma-y : = sqrt(zzave-y - sqr(zave-y));
clrscr;
writeln;
writeln(' The following are the values of the standard
deviations of ' ) ;
writeln;
writeln(' the predicted and model position values:' ) ;
writeln; writeln;
writeln('Standard deviation in the x direction: ' , :,igm:i_x
:kt: 3);
writeln;
writeln( 'Standard deviation in the y directior~: ' , s i g r n a - ~ ,
:5::3,);
readln;
clrscr;
End; (*get-statistics*)
PROCEDURE drawptx-temp, y-temp : real;
color : integer);
Var
x, y : integer;

Begin (*drawp*)
if ( o p t i o n = c a t d . - o p t , ) t h e n
begin
x : = round(x_temp)+l60;
y : = round(y_temp)+100;
end
else
begin
x : = round(2240Jcx--temp/255) - 950;
y : = round(300.%y_temp/255) - 100;
end ;

draw(x-2, y , x + 2 , y , color);
drawtx, y-2, x, y + 2 , color);
E n d ; ( *drawp* )
BEGIN (*Track*)
initialize;
initialize-filter;
while (option < > exit-.opt) do
begin
if (option <> cmse-opt) then
begin
graphcolormode;
palette(2);
textcolor(2 1 ; write( ' R: predic+tcdl 1 ;
textcolor(3) ; write( ' y : modcl ' ) ;
textcolor( I ) ; writeln( ' t i : rnc?asurt:l-l' 1 ;
end ;
if (option=catd-opt) then
max-runs : = man-sampling+l
else
max-runs : = num-of-pts;
for k : = 2 to max-runs do
begin
delay(de1ay-factor);
if (filter_option=l) then
begin
predict-l(k);
drawp(p-x[l,kl, p _ y [ l , k f , p r color);
end
else
if (filter_option=2)then
begin
predict_2(k);
drawp(p--x12,kl,p-y12,kl,
y r .colclr ) ;
end
else
begin
predict_3(k);
opt 1 thcrl
if (option <'/ cmse--
drawp(p_xC3,kl, p - y l . 3 , k I ,
pr- '20lt~r ) ;
end;
delay(de1ay-factor);
if (option=catd-opt) then
begin
model-x[k1 := x--rnagt s i r i ( w t ( k + 21 +TI;
model-y[k] : = y mag *
i:(ss(rn*wv(k+2)t
'rt p ~ l ;~ l ~ ~
end
else
get-position(k);
if (option = catd--opt)then
begin
 drawp(mode1-xrkl, ~ n v d c l. y l k l ,
fn<Jdc1 ~2<-> 1t-! I? ) ;
add_measurement..noise;
delay(de1ay-factor);
end ;

if ( f ilter_--option
0 1 ) than
begirl
if f ilter-option=2 ) t-lien
(
ctrl : = 2
else
ctrl : = 3 ;
for ctr:! : = 1 to f i l t - e r . o p t i c - ~ r qlcl
~
begin
gain_x[ctrl,ctr2,k j : =
gain-opt xi ctrl , c t r 2 J ;
gain_y[ctrl,ctr2,k] : =
gain-opt-y [ctrl, c t r v ;
end;
if not(steady-state-x[ctr1,11:tr2]) then
calc-trans-gain(ctrl,ctr2,'xJ)
else
ctr2,k]
gain--x[ctrl, := gai n-opt-x[
ctrl,ctr2];
for ctr2 : = 1 to filter--optiond(s
I c:trl,c t r 2 J )
if not ( steady--state--y
then
calc-trans-gain~ctrl,~t~r2,~y'~
else
gain_y[ctrl,ctr2,kl : =
gain_opt-y j ctr1 ,ctr2);
end ;
if (filter-option=l) then
I k)
smooth--1
else
if (filter_option=2) then
smooth-2 ( k )
else
smooth-3 ( k ) ;
end ;
readln;

if (option = catd-opt) then

g e t - s t a t i s t i c s ( m o d e 1 - - x , model-y. p - x [ f ilter o p t i c r n l ,
p _ y [ f ilter-option 1 )
else
get-statistics(m-x, m-y, s- filter-option],
) ;
s-ylf ilter__option]

if (option = catd-opt) then

get.-option
 else
if (option cmse-opt) then
begin
reset(ptsfi1e);
calc-sigma_a(f alse 1 ;
initialize_f ilter;
option : = exit-opt;

graphcolormode;
palette(2);
end;
If (option C > catd_opt) then
close(ptsfi1e);
END. (*Track*)