USING NICROPROCESSORS V3 71 KLMN FILTER REL AND A MEW ... · eficiency, and portability are emphasized in the software design. 4. The program was tested, using simulated trajectories,

RD-RI67 71 KLMN FILTER REL TINE TRCKING USING NICROPROCESSORS V3

AND A MEW LANGUAGE FOR NILITRY SOFTMRRE(U) NRALPOSTGRADUATE SCHOOL NONTEREY CA J A FERREIRA 27 MA 6

UNCLASSIFIED F/G 9/2 M

w I.L

IL6 La ILO11111 ii - lion-?

11.5 EA

-I~r~r CHAR

**~* 3311111

4--' ~ , *'t!j r

NAVAL POSTGRADUATE SCHOOLMonterey, California

Go

DTICOELCTE

MAY 29 1986

THESISKALMAN FILTER REAL TIME TRACKING USING

MICROPROCESSORS AND A NEW LANGUAGEFOR MILITARY SOFTWARE

by

Jose Alberto Fernandes Ferreira .

L...March 1986

Thesis Advisor: H. A. Titus

S Approved for public release; distribution unlimited

EUY ASFCAINOHIS PAG

REPORT DOCUMENTATION PAGE

Ia. REPORT SECURITY CLASSIFICATION lb. RESTRICTIVE MARKINGSU NCLASSIFI ED______________________

12a. SECURITY CLASSIFICATION AUTHORITY 3. DISTRIBUTION /AVAILABIUTY OF REPORTApproved for public release;

2b. OECLASSIFICATION /DOWNGRADING SCHEDULE distribution is unlimited.

4, PERFORMING ORGANIZATION REPORT NUMBER(S) 5. MONITORING ORGANIZATION REPORT NUMBER(S) ..

6.. NAME OF PERFORMING ORGANIZATION 6b. OFFICE SYMBOL 7a, NAME OF MONITORING ORGANIZATION1(if applicable)Naval Postgraduage School j62 Naval Postgraduate School

6 c. ADDRESS (City, State, and ZIP Code) 7b. ADDRESS (City, State. and ZIP Code)

* Monterey, California 93943-5000 Monterey, California 93943-5000

Sa. NAME OF FUNDING iSPONSORING 8 b. OFFICE SYMBOL 9. PROCUREMENT INSTRUMENT IDENTIFICATION NUMBERORGANIZATION (if applicableo)

Sc. ADDRESS (City, State, and ZIP Code) 10. SOURCE OF FUNDING NUMBERSPROGRAM IPROJECT ITASK IWORK UNITELEMENT NO. NO. NO. ACCESSION NO.

I TITLE (include Security Classification)

KALMAN FILTER REAL TIME TRACKING USING MICROPROCESSORS AND A NEW LANGUAGE FOR MILITARYSOFTWAARE12' PERSONAL AUTHOR(S)

Jose Alberto Fernandes Ferreira13 APePR hei 13b, TIME COVERED 14 DATE OF1 JPT 1f 1 ~np.aJ15 AECUT-

FROM ____TO 2_ 24 46 SUPPLEMENTARY NOTATION

17 COSATI CODES 1S. SUBJECT TERMS (Continue on reverse if necessary and identify by block number)

FIEL GRUP SB-GOUP Kalman Filtering; Torpedo Tracking

9 ABSTRACT (Continue on reverse if necessary and ientify by block number),A sequential Extended Kalman filter routine, implemented in the "C"

language, is developed for an embedded computer application. The design

takes into account the real time constraints imposed by the sampling time

of the data. Filter measurements consist of four (4) transit times gen-

erated by torpedo instrumentation in a TEST RANGE facility, and they are

nonlinear functions of the coordinates of the torpedo. Memory, speed

eficiency, and portability are emphasized in the software design.

4. The program was tested, using simulated trajectories, in an IBM-PC% and in a CIE 680/30 microcomputers. Limitations in handling small floating -

20 DIST R19UTION /AVAILABILITY OF ABSTRACT 21. ABSTRACT SECURITY CLASSIFICATION* UNCLASSIFIED/UNLIMITED 0 SAME AS RPT. 0DTIC USERS I Unclassified

22a NjAME OF RESPONSIBLE INDIVIDUAL 22b. TELEPHONE (Include Area Coe22c. OFFICE SYMBOL

H. A. Ti -4m - 62Ts00 FORM 1473,854 MAR 83 APR edition may be used until exhausted SECURITY CLASSIFICATION OF 7HIS PAGE

All other editions are obsolete uN A IFIEUNLSSFE

SKCUflITY CLASSIFICATION Of V041S PAGS (Wham DA Maege.4

19. ABSTRACT (cont'd)

point numbers degraded the performance of the algorithm in the

CIE machine which could not track 1/4-G maneuvers.

Acee!;siin 7or

N T 13 GT VrIDTI' VTA-

By-

SECUMlY CLASSIFICATION OF THIS PAGE(When Does Entoeg

................................................

o - -. . -.' -. . . - -T " . S'Fv <- -F-

m - -r r : - . ~ d fq m r r i~~ri j -r- j r-f~r

Approved for public release; distribution is unlimited.

Kalman Filter Real Time Tracking UsingMicroprocessors and a New Language

for Military Software.

by

Jose Alberto Fernandes FerreiraCaptain, Brazilian Air Force

B.S., Instituto Tecnologico do Aeronautica, 1977

Submitted in partial fulfillment of therequirements for the degree(s) of

MASTER OF SCIENCE IN ELECTRICAL ENGINEERINGand S.-

ELECTRICAL ENGINEER

from the .S

NAVAL POSTGRADUATE SCHOOLMarch, 1986

AuthorsLI 4MnJose Alberto Fernandes Ferreira" >

Approved by: -H.A. 'Titus, Thesis Advisor,..

Alex Gerba, Second Reader

H.B. Ri&s, Chairman, Dep-mnt a.

Electrical and Computer Engineering"

J.N./Dyerl Dean of Scienceand Engineering .

3 :-'

.~~~~*5~~ 5'.- ° .S

ABSTRACT

A sequential Extended Kalman filter routine, implemented

in the "C" language, in developed for an embedded computer

application. The design takes into account the real time

constraints imposed by the sampling time of the data. Filter

", measurements consist of four (4) transit times generated by

torpedo instrumentation in a TEST RANGE facility, and they

are nonlinear functions of the coordinates of the torpedo.

Memory, speed efficiency, and portability are emphasized in

the software design.

The program was tested, using simulated trajectories, in

an IBM-PC and in a CIE 680/30 microcomputers. Limitations in

handling small floating point numbers degraded the

performance of the algorithm in the CIE machine which could

not track 1/4-9 maneuvers.

er..

4°

'"¢ :- ' .z :),) : ., "" . . . " " .. " " -' .. .. " '"".. . " ..... ' "" """

* * * * * ft" . .. * * . '-*-ftf--t-f. -*.* *.*.*. . -ft*"*ft ft".** * * '. .. I ... . .

TABLE OF CONTENTS

page

I. INTRODUCTION . . . . . . . . . . . . . . . . . 8

II. PROBLEM DEFINITION . . . . s ..... 9

A. RANGE TRACKING GEOMETRY .. . . . . . . . . . 9

B. GENERAL EXTENDED KALMAN FILTER EQUATIONS a a 11

C. APPLIED EQUATIONS .............. 13

D. THE SEQUENTIAL EXTENDED KALMAN FILTERAPPROACH .. . . . . . . . . . . . . . . . . 17

III. THE IMPLEMENTATION PROBLEMS .. . . . . . . . . 19

A. THE HARDWARE AND THE SOFTWARE PROBLEMS . . . 19

B. THE FAST APPROACH . . . . . . . . . . . . 20

C. ODDS AND EVENS OF THE C LANGUAGE . . . . . a 20

IV. THE REAL-TIME KALMAN FILTER PROGRAM IN C .. . . 23

A. THE INITIAL APPROACH . . . .aaaa 23

B. OPTIMIZATION OF EXECUTION TIME a. ... 24

V. TESTING . . . . . . . . . . . . . . . . . . . . 26

A. CHECKING TRAJECTORY FOLLOWING PERFORMANCE a 26

B. CHECKING EXECUTION TIME REQUIREMENT ..... 27

VI. COMMENTS AND CONCLUSIONS ............ 118

APPENDIX A: PROGRAM DESCRIPTION .......... 122

A. OVERVIEW . .. .. .. ... .... 122

B. DETAILED DISCUSSION ......... 124

APPENDIX B: ALGORITHM FOR THE KR.C PROGRAM .. . . 132

APPENDIX C: SETTING UP FOR EXECUTION ....... 136 :4

A. USER'S NEEDS ............ 136

5i

.j.

• - - I I ..... 2

B. PROGRAMMER'S NEEDS . . . . . . . . . 137fr

APPENDIX Ds COMPILING, EXECUTING, TRANSFERRING FILESAND PLOTTING . . . . . . . . . . . . . . 139

A. RNNIN THEPROGAM O THEIBM-C . 13

A. RUNNING THE PROGRAM ON THE CIEM-P . . 139

APPENDIX E: KR.C PROGRAM LISTING - IBM-PC VERSION . 145

APPENDIX F. LISTING OF CIEKRI.C - CIE VERSION . 173

APPENDIX G: PLOT.C LISTING - PLOTTING ROUTINE . . .194

APPENDIX Hi LISTING OF THE BATCH FILES - IBM-PC . 210

APPENDIX I: SIMTERM - PROGRAM DOCUMENTATION '. . 2112

APPENDIX Js ASSEMBLY LANGUAGE ROUTINES - IBM-PC .*232

LIST OF REFERENCES . . . . . . . . .. .. .. .. ... ~

INITIAL DISTRIBUTION LIST . . . . . . . . . . . . . .243

6

rl N;I17. 7.

ACKNOWLEDGEMENT

I am deeply indebted to my wife, Anne, for her love,

patience and understanding during the preparation of this

j work. I would also like to thank Professor Hal Titus for his

professional guidance. I would also like to express my

gratitude to Professcor Alex Gerba, Mr. Robert Limes and

Mr. Albert Wong for their help and sugestions. k

7

I. INTRODUCTION

The problems of torpedo tracking have been studied, at

the Naval Postgraduate School, by several researchers, who

have implemented simulations that would allow analysis of

tracking algorithm performance using the Extended Kalman

Filter C13. However, those studies have been limited to

simulations done on large mainframe computers, without

trying to adapt them to the real time processing system at

the torpedo range.

The objective here is to develop a version of the

Extended Kalman Filter, adapted to the constraints of

real-time processing, by generating a program in the C

language. Also it is designed to run in the microcomputer -

C.I.E 680/30, currently utilized at the Naval Torpedo

Station, Washington.

The presentation herein tries to follow as much as

possible the chronological order, describing the problems in

the order of appearance and the way they were solved.

..................-- -

IL-- ,

I. PROBLEM DEFINITION 00

," A. RANGE TRACKING GEOMETRY ri n

i Bets of hydrophone arrays are distributed in line along

r ~the extension of the test range. Each individual array ...

consists of four independent elements, placed at a distance

"d" apart, an the vertices of a cubs, thus defining an

orthogonal coordinate system in which the measurement of the

transit times are made. Figure 1, shows the geometry.

Four transit times, To, Tx, Ty 9 Tz, corresponding to the

four hydrophones in the array are measured and the

positional information is computed from the measured transit

times. The torpedos are equipped with sonar transmitters

which are synchronized with a master clock, allowing new

data every 1.31 seconds, with a range accuracy between 3 to

30 feet.

When multiple arrays are used, the signal from the

closest hydrophone is defined as the basis for the time

measurements and for the range calculations. If the computer

time were not critical, an 18 hydrophone signal should be

processed.

9

r- V"? . ' I.,

'I Z

Tx/

T/dA I

~HYDROPHONE (-/2-d/d/2)-/,/2/F

/d

HYDROHYROPHON Y

d' (-d/2,/d/2,d-d/2

HYHYDROPHONEX

(d/ 2, -d/2--d/2)

Figure ls Geometry of a tracking array.

10

B. GENERAL EXTENDED KALMAN FILTER EQUATIONS

A summary of the filter and associated equations is

given in this section, following the order and thefi

conventions in C13.

1. Observation Equations

For the three-dimensional problem, in which the

z-position is considered invariant, a state space consisting

of the throe positions, X, Y and Z, and the two velocities,

V and V define the torpedo motion. Thus, the discretex y

linear estimation and the nonlinear observation equations

are given by

X(k+l) - .X(k) + r*W(k) (2.1)

Z(k) - h(X(k) ,k) + V(k) (2.2)

whores I and r are constant matrices;

h is a nonlinear function of the state X;

W(k) is the plant excitation noise;

V(k) is the measurement noise.

The plant noise and the measurement noise are

assumed Gaussian distributed with zero mean, and

uncorrel ated.

The last assumption implies that

ECW(k).W T (J)3 Q'(k)

TErv(k).V (j)3 R(k)

11j

wherso ., for

0.O for kj;

2. Linearized Equations

Linearization of the observation equation is

obtained by using the Taylor Series to expand about the

prediction of the trajectory f or the interval.

ZWk - H(k) . XWk + VWk (2.3a)

whersi H(Ic) = A ~ =X~/-I (2.3b)

X(k/k-1) is the estimated value (predicted) at time

k. given the measurements until time k-I.

3. State Error Vector

XWk XWk - XWk

4. Predicted State Error Vector

X~k/Ic-1) - X(k/k-1) - XWk

.15. Covariance of State Error

PWk - ErX(k/k).X (k/k)]

6. Predicted Covariance

P(k/k-1) -EEX(k/k-I).X (k/k-I)3

7. State Excitation Matrix

T TQWk r(k).ECW(k).W (k)J.r Wk

S. Measurement Noise Covariance Matrix

'C RWk EEY(k).VT W3)

12

I.

9. Kalman Filter Equations

P(k+l/k)-.P(k/k). T+ G(k) (2.4a)

8(k)-P(k/k-I)HTk)EH(k)P~k/k-l)HT(k)+R(k) (2.4b)

P(k)-mI-S(k).H(k).P(k/k-l) (2.4c)

X(k+ I/k)-IF. X (k/k) (2.4d)--

Z(k/k-l)-h(X(k/k-1) ,k) (2.4e)

X (k/k)-X (k/k-l) +8(k)CZ (k)-Z(k/k-1) ] (2.4f)

C. APPLIED EQUATIONS 113

In order to apply the Extended Kalman filter to the

torpedo tracking problem it is necessary to define the state

vector and the matrices H, §, r, W, q, and R.

1. Transit Times

1 ) 2 +

T (k)-C(xd/2) (y+d/2) 2 +(z+d/2) 2 j/2 +v(k) (2.5)

T (k)i ' lx+d/2)2+(y-d/2)2+(z+d/2) 2 1/2+v(k) (2.7)x VEL

T (k) i L(x +d/2) (y+d/2)2 +(z-d/2)2 3t/2 +v (k) (2.8)

where: dm30 feet, is the distance between two hydrophones

in a given array;

VEL=4860 ft/sec, is the average sound speed at Dabob

Bay.

These equations must be linearized to apply Kalman

13

Filter Theory. The linearization is done, in this case,

about the predicted trajectory, which is being generated by

the filter algorithm.

The values of transit times are corrupted by noise,

v~k), which is assumed Gaussian with zero mean. The

positions x. y, and z are all functions of the time

interval.

2. State Equations

The plant state equations are defined by

x (k+1) xi(Ii)+TV (k)+gl

xx

y(Ic+l) - y(k)+TV (k)+g (2.9)

V (k+1) V (k) 4gy y 14

Lz~k+l) J Lz(k)+g,

where. x~k)9 y(k) and z~k) are the position coordinates at

time t(k);

V x(k) and V Y(k) are the x and y components of the

velocity;

the depth z(k) is maintained constant and any

velocity in z is assumed random, Gaussian,

uncorrelated and with zero mean;

T is the sampling interval T-1.3lsec;

91 9 g2 g and g~are excitation terms, included to2'. g3 g 4

take into account random variations in speed (~)

14

heading (y and depth (n), which are assumed toOt

be Independent,, zero moan, rates of change.

Typical torpedo maneuvering parameters are

ai22*/sec; Ov 2-E~rv2t t t

-36 ft/sec1 ; ECY 223t t e9t

1~n f t/sec; at 2 EEC4-

In state form

X~k+l) - .X(k) * -'W~k) (2.10)

1 TO0O0O

0 100 0

1~ 0 0 1 T 0

0 00 1 0

TR/2 0 0

T 0 0

r 0 T2 /2 0 (2.11)

0 T 0

.0 0 T

(v (V/V t )6,02 V y2 b 2(2.12a)t

15

, (Vy/V)t + V 2 2 (2.12b)

#Aar - VXVy C it/Vt- (2. 12c)

_ is a symmetric matrix given by

qO qI q2 q3 q4

q5 q6 q7 qS q9

G qlO qll q12 q13 q14 (2.13)

q15 ql6 q17 q1S q19

q20 q21 q22 q23 q24

whor qO,(T2/2). ; ql=(T3 /2) As. ; q2=(T/2) 2 A.., ;

q3- T /2) A. .; q4-01 q6-T 2 ;

q7-(T/2)....; qST 9A..; q9-O;

q12,,(TA/2) 28; q13m,(T/2) a 1; q14-n0; h;

q18-T2 ) q19-0; q24=T261;

Z(k) ,, T Ck) T (k) T (k) Tz (k)]T

IT 0 0 0

R(k) - 0 0 IT 2 0Y

0 0 0 IT2

L ~z

18 7

Typically TT6 2 a 10-1 SE.C.ST T TyC 2

hO hi h2 h3 h4

h5 h6 h7 hS h9

HUE) - hlO hil h12 h13 h14

h15 h16 h17 h18 h19

h20 h21 h22 h23 h241

whores hOin(x'(k)+d/2)/DEN1; hlinO;

h2-(Y'Ck)+d/2)/DEN11 h3-0O;

h4-(z'(k)+d/2)/DEN1; h5-(x Ck)-d/2)/DEN2;

h6-0; h7in(Y (k)+d/2)/DEN21

h1301 h9-(z' Ck).d/2)/DEN2;

hlO-(x Ck)+d/2)/DEN31 hl-0n;

hl2-Cy'-(k)-d/2)/DEN31 h13-01 -

hl4in(z*Ck)+d/2)/DEN3; hl5-Cx Ck)+d/2)/DEN41

h16-0; hl7m(Y' Ck)+d/2)/DEN4;

hIS-O; hl9m(z Ck)-d/2) /DEN41

and DEN1-E(x*(k)d/2)2n4(y'(k)+d/2)2.(Z,k)+d/2)231/2

DEN2uC('(k)-d/2)l4.(y'(k)4.d/2)2.(Z-(k)+d/2)231/2

DEN3-E(x (k)4d/2)2 +(y' (k)-d/2)2 +(Z Ck)+d/2)2 j'"2

DEN4-C (x Ck)+d/2)2+(y (k)+d/2)2 +CZ (k)-d/2)2 ,1/2

17

D. THE SEQUENTIAL EXTENDED KALMAN FILTER APPROACH 113

This approach assumes that all four transit times

measurements are statistically independent of each other. In

this way one can process the transit times sequentially and

independently. The result of each computation step is used

immediately, as soon as it is obtained. Thus more accurate

estimates of the filter states are obtained.

This procedure also circumvents the matrix inversion

when determining the filter gain, and allows one to

disregard any particular time measurement that was found

invalid, and not the total four times for a time slot

(To1.31sec).

Erroneous transit times are detected by implementing a

three sigma gate using the covariance of the measurement

noise (R) and the covariance of the estimation error

(P(k/k). For i=1 to 4

gate(i) = 3*( /(P. maximum)/4860 + R

It can also be used to minimize position errors during

high dynamic maneuvers, by repeating the calculations for

the same time interval until the transit time residual error

falls below a preset value.

.

i~ . .

1 I I. THE IMPLEMENTATION PROBLEMS

A. THE HARDWARE AND THE SOFTWARE PROBLEMS

Since the objective is to use the developed software for

torpedo tracking at the NAVTORPSTA, in Keyport, Washington,

there was no other choice but to implement the program to

run on the C.I.E. 680/30 microcomputer, and to implement it

in the C language.

Two problems immediately arose.

First, there was only badly written and incomplete

documentation. The manual was found difficult to read and to

understand. In addition, there was no information about the

computer hardware.

Secondly, poor software was available. The CIE 680/30

is a Unix based machine, designed for business applications,

working with the Regulus operating system. As far as known,.

there is no software available (specifically written for the

CIE) on the market.

As a result of these problems, there was no full-screen

editor available for the C.I.E machine, no communications

program, and no built in graphics capability (although the

first screen that appears after booting the system shows the

message "CIE GRAPHICS SYSTEM #7").

19

II,." =

. . .... ... ,

B. THE FAST APPROACH

In order to be able to write a reasonably long and

complex program, in a reasonable time, debug it and make it

work, it would be necessary to have a good compiler, a good

full screen editor and good documentation on software and

hardware.

It was then decided to make use of the known software

and hardware information available for the IBM-PC

microcomputer. It was not difficult to find Public Domain

Programs able to communicate with Unix based machines, to

emulate the VT-100 terminals, and to transfer files. Also it

was not difficult to find a powerful easy to use full-screen

editor for the IBM-PC.

The program was then written, trying to keep the C code

the most standard possible, using the Lattice C compiler.

It was executed first in the IBM-Pc, then transferred

(source code) to the C.I.E using a public domain program

(SIMTERM). It was then adapted, making minor changes, and

recompiled on the C.I.E to generate the executable code.

C. ODDS AND EVENS OF C

The C language is not very high level but it is a very

powerful language, allowing bit operations, low level read

and write, system calls and easy interfaces with machine

language. The general mathematical expressions can be kept

20

.4-.*****.*.I-*-*.*'*'-'***-**-**.**-.-t'.** 4 *.*

the same way as in Fortran, but some now operations,

.1.

functions and expressions are also used. Ne

Incrementing a variables

"i-i.l;" can be "i +" or " i", depending if you

first use the variable (++i) and increment it or

vice versa (i++).

Incrementing i by Js "1i=Jj"

Do loop, testing the counting variable after using its"for (i 0;i<imax;i++)""

Blocks are delimited by "(

Every program, subprogram, functions or procedures are -"

seen as FUNCTIONS in C, including the main program. This is

indicated by the calling statements as ins

main() .... for the main program that is not

passing any arguments;

plot(xy) .... for a routine that passes X and

Y vectors for plotting, and so on;

C uses passing by value approach. So the value of the

variable "x" is copied in and used inside a function. Arrays

are equivalent to pointers and thus they can return new

values from functions.

There are automatic, local, global and external

variables. There are character, integer, long integer,

string, floating point (double and single precision),

string, and enumeration data types. Short integers have

standard size. Double precision floating point allows one to

21

"o 5

• r'ro' , .,, i r du r .. r P , r. *r V' V u . -', - "' . ;-. * -. '. .. I--- . , . ". - .

represent numbers in the range + 101 in the IBM-PC

(Lattice C compiler) and in the range 10 + 4 in the CIE.

There are IF , IF THEN ELSE, WHILE, DO .. WHILE, and CASE

statements.

There are no input and output statements as in Fortran

or other languages. Input-output is done by standard

functions, which must be carefully used.

Header files and # *include " statements are used to

hide from the user definitions of parameters that are

machine dependent or contain environment-specific

information. "#def ine" statements are used to define

constants and macros and to hide machine dependent code

segments.

Reference [43 is the definition manual of the language.

The other references for the C language were used primarily

to look over the example programs, and functions.

Iw

0q

22

. . . . . . .... .. .

-4 .% - S - - - a S - -, 7 - T- W,

IV. THE REAL-TIME KALMAN FILTER PROGRAM IN C.4.

A. THE INITIAL APPROACH

The first idea was the simple translation of the program

described in reference C13 to the C language, without

worrying about specific problems that would arise when

trying, later on, in a more realistic approach, to simulate

as close as possible the actual running conditions.

The initial program had a structure similar to the

Fortran program in C13, that is, a main program, the actual

Kalman Filter, and several functions to calculate the

trajectory, to add, transpose and multiply matrices and

to multiply two vectors. All the floating point operations

were done in double precision and the matrices were defined -'-

as two dimensional arrays.

The initial approach failed in many aspects.

It did not have enough speed, producing, on the average,

one estimate every 22 sec, in an IBM-PC without math-

coprocessor (8087), running at 5 MHz. The same program

generated a new estimate every 5 sec in an IBM-PC with an

8087 chip. It did not have the main frame graphics

capability, readily available and, finally, due to the way

the C language passes parameters to the subroutines, the

matrix routines worked correctly only if matrices were

defined as square matrices.

23

. -. 7-.

B. OPTIMIZATION OF EXECUTION TIME

Although the C.I.E microcomputer runs a Motorola 68000• ,. ..,

cpu at 8 MHz, and the IBM-PC runs at 5 MHz, executing the

same program in both machines showed a difference of 10% to

20% in execution time. In this way, optimizing the execution

in the IBM-PC (with 8087) would be a valid approach since it

would allow a safety margin, and would require almost the

same effort.

Optimization of execution time led to a

1. Transforming all the matrices from bidimensional to

unidimensional arrays (or vectors). That saved memory space,

and forced the access to the elements to conform with the

way these are stored (C stores array elements by rows), thus

speeding up retrieving when needed.

2. Eliminating functions, actually by having everything

in the main program.

3. Eliminating all the unnecessary operations. That

involved restructuring the program to avoid loops when

possible and reasonable, avoiding multiplying by zero,

avoiding adding zeros, and avoiding repeating calculations

when the result was known to be constant.

4. Avoiding double precision where possible, and when

improvement of the execution time could be achieved without

compromising the results. The Lattice C compiler, in the'%

IBM-PC, is defined for precision (all the floating point

24... ~i .-

. . . . . . . . . . . . . . . . .

calculations are done in double precision). Then, if single

precision is desired, you lose in both, precision and speed.

The CIE machine calculations uss the Motorola "Fast

Floating Point", a format which is not compatible with the

IEEE format. The IEEE floating point values lose some

precision due to the fact that the calculations use the fast

floating point routines and a translation to and from the

FFP format E103. So, the FFP format was used, in the CIE.

5. Writing single precision routines to avoid calling

standard mathematical routines, which are all double

precision, in C.

6. Reproducing more closely the way the final program

would have to work, when in the real time application.

7. Testing the program for several values and

parameters, to determine bounds that would allow results of-

good precision without increasing the execution time. It was

found, for example, that the results are highly sensitive to

the way the columns of the matrix H are calculated, thus,

requiring calculation of H using double precision.

B. Measuring execution time of tasks inside the

program.

9. Developing software tools to allow for interactive

execution and checking of correctness of the results.

25;.-oy

V.

V. TESTING

A. CHECKING THE TRAJECTORY-FOLLOWING PERFORMANCE

The trajectory following performance was verified for

the following trajectories ,

1. Straight line parallel to the array axis, and

y-coordinate - 0.0 , single array tracking;

2. Straight line parallel to the array axis and

y-coordinate = 2121.0 , single array tracking;

3. Straight line, 45? inclination relatively to the

array axis, single array;

4. Straight line parallel to the array axis, long run,

multiple array tracking;

5. Straight line and circular maneuver, short run, slow

turn (10.Odg/sec , 1/4-8), single array tracking;

6. Straight line and circular maneuver, long run, slow

turn (10.Odg/sec , 1/4-G), multiple array tracking; -

7. Straight line and circular maneuver, short run, fast

turn (20dg/sec , 1-8), single array tracking, adaptive

maneuver implemented with 4 iterations allowed:

B. Straight line and circular maneuver, long run, fast

turn (20 dg/sec , 1-6), multiple array tracking, adaptive

maneuver implemented with 4 iterations allowed;

26

P J.

TABLE 1 , and TABLE 2 summarize the results in terms of

maximum absolute error for the IBM and the CIE microcom-

puters, respectively. For the single array cases, only the

region inside a 3000 feet radius around the center of the

array being used is considered in determining the errors.

The corresponding plots can be found in the following

pages. The plots were obtained with the value 10 for the

elements in the diagonal of the matrix R (the Error

Covariance Matrix) for the IBM-PC microcomputer and with the

value 10 for the CIE. The plots are numbered sequentially

and contain headings to identify and relate them to the

Table 1 and Table 2. The actual time is obtained by

multiplying the time scale by the length of the time slot,

1.31 seconds.

B. CHECKING THE EXECUTION TIME REQUIREMENT

The execution time in the IBM-PC, was verified by

checking the printouts containing the values of the internal

computer time for a cycle. Later, an "waiting loop" was

introduced to generate an interval of 1.31 sec between each

data input (that is, reading files TRJ.DAT and ZIC.DAT).

Since the "reading internal time" was not incorporated ".' . .. -u

in the CIE program version (CIEKR1), and since that proved

to be a faster machine, the time requirement was just

checked against a chronometer for 150 iterations.

27

,i2 ..

TABLE 1

IBM-PC MICROCOMPUTER

I RUN I MAX. ABS. PO8. ERROR I TRAJEC I ARRAY 1 INITIAL Ia i a I I I

# 1 X.ERR I YERR ZERR I TYPE I TYPE I CONDIT._.', . . . .I a £ I

II I I I 3000I11 0.025 1 4.5 2 1 linear I singl 1 y = 0

1 I I Vx -50I I Vy= 0

I I x -3000 1''2 I4.5 15.4 2.5 1 linear I singl1 y 2121 1"'

I I Vy= 0

I II x -3000I3 1 6.0 6.0 2.7 Ilinear I singl 1y =-30001

---- 33.8

I II Vy- 335.81

I I x -34000!4 0.2 5.4 1.8 1 linear I multp 1 y -6050 1

1 a a Vx -84.4

I I Vy- 0

a circl a x -3000i5 1.0 4.3 5.6 1 l adg/s I sing y = 0

I to=40 a ' Vx-50 Iaa ta70 a Vy 5.

a irl a x =30000"

117 3.3 1 1.8 I l adg/s I multp 1 y =0

Ito =40 IVx=-50 1-1 tf-70 1 Vy= 5

a circl 1 x a5000a.- ,7 2.4 7 a3.7 20dg/s singl y = 0 a

ato 35 Vx=-a2.5- -

t f=45 1 Vy= 0

SIcircl I x -2000018 22 5.2 12 20dg/s I multp 1 y = 0

a -- -- -- -- a- -- a- ------------------------------

a2a8a=a2 I a

1 7133 128 I ltf/=1 m0tp y = 0 I -

a a a a ':a a aa aL TI'./ a a...a

a-----a8

a aaaa----------

a aa- t 23 .x 9.. . . .

TABLE 2

CIE MICROCOMPUTER

1 RUN 1 MAX. ABS. POS. ERROR I TRAJEC I ARRAY I INITIAL 1---- ---------------- I

# X ERR I YERR I ZERR I TYPE I TYPE I CONDIT.

I I I Ix 3000 11 0.031 4 1 1 1 linear I sing1 I y - 0

I I i I I I I Vy= 0'a I Iv.... .

1 x "3000

2 1 7.1 10 1 1.2 1 linear I singl I y -2121 "aIa a a a Vxi-33.81

I I I I Vy= 0 I:'

I I I I x =3000 I1'3 3 3 1 0.9 1 linear I sing1 I y -- 30001

I I I I Vx =-33.81'-i Vy= 33.81 1- '

4 0.2 3.5 I0.7 Ilinear 1 multp 1 y =6050I I Vx--84.4'.

I circl I x -3000 1""-- 5 1a--- - --- --- a --dg/s -singl a y = 0-

t o=40 1 Vx=-50

1 tf=70 1 1 Vy= 5 1

a circl a x i0000:1 3 I 1 3 1 --- lidg/s multp I y = 0001

I Ito=40 I Vx=-50 I-":1 tf =70 1 I Vy= 5 I-

I circl I x =5000 1a-7 ---- - a--- a --- a 20dg/s singl a y = 0-- -"

I I to =35 :IVx=-92.51 .-.1 t4-45 1 1 Vy= 0 "'

I I circl I x 200001 m

8 1 0.2 1--- --- 1 20dg/s 1 multp I y = 050

IIto =120 I Vx=-92.5I La ta 130 a 1 Vy- 5-.-

a - - - - - - - - - - - -- - - - - - - - - - ----------

29 ,

a a a.aa a a aa aia,..a a aa aC rC~ a X .ij%'.

a a.a a

-c c* - 6- 1. 1. C- W: 7- W., IL - . 1.7 07. V

64.

CD* C_;

.4-~~CD

-4

11-4

CO-.Rcr

0- F-

39-

ara fa sngl.ra. Iiilcniin x-00ft4

ft, x-50ft/sc, V-0 f/seS

30-

C".a

-74.

C%3,

CU'.

cm,

- m

c3

cm

cm

cm r

I=rCnC C cS! 0! c!CS

C%3 __q

Figur 4..

IB-C Etmt ftrpd oiindrn

straiht rn inthe rea o a snglearra. Iita

condiions x-300 ft y-Oft. x--50ft/scV- tsc

31

S--S

4_ -nc4 .c -4

4-; '4%

2..

-- !%-~mq

* V.

c0-4

cm!4

cm

cz-4

0? crz 4 c-!

LU % %, = _ .-.-

Figue 5. IBMPC Eror n trped postio durng

staih rn In te ra f inl ary. Iita

codtint -00 t yOfv x-5 t/wVyU4ts'

z3

I.j

CD--

CD4

I~x .4- o

tn CD

-o"

-o

cm4

ca -. c! c L! % o cC%3 C3 c% __4 4 co cm c

Figue 6.IB-C Erri topd det duigas agh

ru n h ae o snl ary.Iita cnitos

x=00 ft, y- t4x-5 tsc V- tsg

34~

JA

4_

?p

FRN RR-" .. _W __ ..

CD2-4- -raQ

0--4

-44C 7 CI

0r%

I-.. Cli

CD,

j~~~~r - CS;2COI~C~ .E-4- -1 ,d L

P. Figure 7. IBM-PC -Variance Of Position error during astraight run in the area of a single array. Initialconditionss x-3000 ft, y-O ft, Vx--50 4 t/suc, Vy-O ft/sec.

35

C%2 -.4D

Cn ~ I

- a

00

L" C-;

CD-

Cm2

q c C" V2 C%2 __ -_4 C D

Figure S. IBM-PC -Variance of position error during astraight run in the area of a single array. Initialconditionsi x-3000 4t, yin0 4t, Yxin-50 4t/sec, Yy-0 ft/sec.

* 36

L-4-4 D

:w - .9

strigh ru n te ae f a sige ary nta

3-37

- -S - - -Ca

ILNil!i.

-4e

*a .5 l %2 C2 C"_4 -f _4 C

Figure 10. IB-C Fle esiaeo mensur err

duigasrih-u nteae fasnl ara. Intia

codtins -00 t yOf, x-5 t/eVy-ftsc

Lf38

61r

7 '? F r - -.- .- . - - ..- r'Fr rr - --x r r* . -. * -. --. -~ ~>zr

p.-.

- U..-4

CON

-

.. - :

Figure 11. IBM-PC - Filter estimate of mean-square error ;

' ~during a straight run in the area of a single array. Initial i

conditions# x-3000 ft, y-O ft, Vx--50 ft/sec, Vy=O ft/sec.

1239

~~~~~~~~~~~~~~~.. .............. ..-........ , . ... ,.. ...-...--.---. '..-.-..."-c.. -'....-.-.-..-..'-- - '.-2----

* . ' .*.*.,*.** ... '-.t~. %2

- f- -

Fiur 124 IB-C Fle etmt 4mansur ro

404

-% -

IM

- -

0r%

:1C

SIa _

-. a

%" %

ISm

' ;-U--1

Figure 13. IBM-PC - Real time plot of estimated trajectory

of the torpedo in the area of a single array. Initialconditionss x-3000 fts y-2121 ft, Vx--33.8 ft/s, Vy-O ft/s.

41

WN

Cm4

CJI-

CS!

-4

CO.4-4

&W M66CS

424

II~.

=_ CD

7 -

SI;

&ro xS00fy221f, .- 3. #/,V- ts

43.

C%2.

LLm

-- =Zap

CDa

cbe.

.- ~ ~ C CFC3I

Figure 16. IBM-PC -Error in torpedo position during astraight run In the area of a single array. Initialconditions. x-3000 ft, y-2121 f t, Yx--33.8 ft/s, Vy-O ft/s.

* 44

cmI~

CS2eg

W21 CVo

454

4-,

C-24

DM-A

= - IT

464

_ _ _ww - KY=-

-CTFM RAK-f~w~vw.wNWU M,_

-E!

c* A

9na

- AA

- -'k

C21

U-.; -' -4

Figure 19. IBM-PC -Filter estimate of mean-square error

* during a straight run in the area of a single array. Initial

A conditionsi x-3000 it, yin2121 it, Yx:-33.8 ft/s, Vy-O ft/s.

* 47

cm -

-cc'.

c-Io

CID

-c

C DI tw-~

9wa

-aurn;

aw %. C1. -cr AD c cI.CZ IC Lr; -- i-C4 C27 I1 cs

Figue 2. IB-PC Filtr etimae o mea-sqare rro

duigasrih u ntearao igeary nta

codtos x30 t y22 t x-3. ts-V- ts

c-48

-7

.:r:

CI

co

ci .

UW-

'?W' ~ ~~D~ .N~F54WXM ~ ~ *~ ~ 1 ~~ ~ ~k

I'I.

CDIgCD ~5-

'C

I cm

CN2

U,J

=1 -=

oftetreoInteae fa igeary Iiilcni

050

"7 T-V .-" -7 V

C".

'a, -4

CD i...~

S.' C! a

U. /

Fiur 2. IB-P Tu ta~ctr o te opeo n/hare of a ige ary Iiilcniiossx30 t

(-00 ft-4-38f/ec Y3. tsc

a-u /51

CIA

'-I4

- - S

'.3 C=:* 5O4

Figure~~~3 244B-C Esiaeo oreopstindrn

straigh run inthe are of sinle arr. iiilcniin

arm x-00 ft Y-00 ft Vx-3-8 t/s V-338 f/9

52.

tu4

Cvj," e_ .

am

=-!4

PIL

16n.

CC

Lr ,,I;

_ _ - . -t/s.

.- - ,I :--.

Fiur-5. IM-C -,rrr intrpd positio duiga-

-. 4'

....... .........-..

,= -..-. ;::53

,= = ._, _., ._, , ... .:W Y

4 c1%

CDJ

Lr!-

strigh ru n.h&re fa snl ra. Iiilcn

dto s:r x_00_qy-00 t x-38f/s y3. ts

%4 .- %k

~ r~.54

-Q

--E

CS;

cma

CD.~

Cm

Cm "~

cl-4

-44

Fiur 2. IMP ro n treo dph drn

strigh ru intheare ofsinle rra. Iitil cndiion

1 -00fY-00f,-.-384/,VY3. ts

I55

.......... . -,'

4 -. _ -

0-4 g -a

5-p

p 0%%

U'o%

a, "a. IC%

IN,

- _6-4W

Figure 29. IDM-PC -Filter estimate of mean-square errorfor a straight run in the area of single array. Initial con-

ditionss x-3000 ft, Y--3000 ft, Vx--33.8 ft/s, Vy-33.8 ft/s."

56 4

Car

CS;

CC!

4- 4-

- S.

C-: Lr c"! r! -4

Lrl. Lru qw C21 C

Figure 29. IM-PC Filtr estmate mea-qae4ro

for~ ~ ~ ~ ~ a tagtrnII- raofsnl ra.Iiilcn

diins -300fy.00f.V-3. ts y3. ts

57_

2 irk

'-I;

cV~j

I- Ln

\ ..- cc.

585

Maoa

4-4

CW,-I

C 4S!_

CS!-. CS ! CD

59-

NEEN WEE W~w~jxFW_ 773" Kw - IMANA x T-JK ~r%.WXrj17.Vr ~ I

1.- T--K %x Y T

0_ 406oa

F.-A

* - Im D

:msL9_!

/vjCD

qts4

/ 0_4 c.; __ ,2 C; Cl 1 1 ,

Fiur 32 B-C Esiaeo'oreopstindrnstaih ru'hoghmlileary. nta.cniin

armsx-3000 tq -605 4t Vx-84. fto. V-O t-.

U-. 60

5' S%

U-. .5

CID'

AMR

Jo.

co

2--C

-------- -

_~~c _ _ _ I I

Figure 33. IBM-PC -Error in torpedo position during astraight run through multiple arrays. Initial conditions

* arms m-34000 f t, y-6050 ft, Vxin-84.4 ft/s, YyinO ft/u.

61

CS'..

CD.'

0;.

S.D.

9A Fas

__ D ~-Ow qcr 4w e*'.

U-31 -31 U1j 5$

,7z4 ci

Fiue3.IMP ro n opd oiindrn

strigh ru thoug mutipe aray. Iitil cndiion

aresx-3400 t, y6050+tVx--4.4 t/s Vy- +t/. 4

62.

CS;~

CDa

W 9-0

CS!

* X 0% -* C

0; Ca z Cw I

Fiur 35 B-C Err i oreo dph drn-

arms -40 t -00f..x-44f/,V fta.

In Ca

U., N.

-_4

JM64

04

9--4

U-..

Coo)

1c

'Ln

'-4o

I In

.l * ci CS! 0% c! -I! U" _ .

-- I co cm -o cm cmcm

Fi gure 36. IBM-PC - Filter estimate of mean-square error* for a straight run through multiple arrays. Initial condi-* tiort x-34000 ft, y-n6050 ft, Yxi-94.4 ft/s, Vy-O ft/s.

64

.......................... ... .... ... ...

1~ -4-b

r 0%

01

U-2 '" - C z

Figure 37. IBM-PC -Filter estimate of mean-square errorfor a straight run through multiple arrays. Initial condi-tionsi x-n34000 ft, yin6050 ft, Vx--84.4 ft/s, YyinO ft/s.

65

."'.".4-

( -4

,U--.

a srih *u nteaeas igeara.Iiil l- ..

_Q

_n --

C30 tw. .-CS!

0%3 Lel gw 4w CV1 % C%3 _4 _ co c

Figure 36. IBM-PC -Filter estimate of mean-square errorfor a straight run in the area of single array. Initial con-ditions: x-34000 ft. y=6050 ft, Yx=-4.4 ft/s, VyinO ft/s.

66

ICY

-4w D

0.3

CD

04D

C%3

04

UII

'CD

Figure 39. IBM-PC -Real time plot of estimated trajectoryof the torpedo in the area of a single array. Initial condi-tions. xin3000 ft, y0O ft, Yxm5O ft/s, Yy-5 ft/s.

67

W31*

In CD,

ml* C%2cv, _ ly,

-%; CS;

Figue 4. IB-PC Tru traectry o th topedodurng.

10 dgs maeuve in he aea o a snglearra. Iita

codiins -30 iy- tV--0ftscV-5i/sc

684

'I-%

c": c qrcs

CS! "? L! CI co m cv .i

CVIA c~a _4 c cm

Figre 41. IB-PC Esimae f orpdopostin drig10~~~~~ dgsmnue nteae o igeary ntacondtiosi -300 f, yO f, Vx-50ft/wcVy- ftsec

a-69

CD -4

w~j, CLP 0

CF:

CO

COL

%* CDU.30C

-- 4 ('M -4O.

4r

CD3

c~i

-D

CD4

------ -S

U-.L! I

U; C- - C"

Fiue 43 B-P ro i opeopstind-iga1

dg/s manuve in he rea of a sinle rray Intia

conditions -300f.y.fV-50f/eV- tsc

~zJ~71

CX,

WO%

* aa

U-b4

. . . . . . . . .

7.4

w CD

0-d -I-ICO A

C.-

U-S CID

Fiue 5 IMPC Tuetajcor f h tred urn

a 0 d/s mnuvr trog ulil ary. nta

condtiost x100 fty-Oft. x--0 ftsac Vy- ftsec

=473

-4

CV!

CS -D-44W3

744

4.00 CO

- I-P

r~~~~ ----. M

CD '..;

M~~~ ~ ~ a,.. SI CD CI

Figue 47 IB-PC rro in orpeo psitin dringa 1

dg/s ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ * maevr truhmlil ras nta odtos

X-100 ft Y- ftVx-50 f/sec VY5 '4.e,

75-

4-4

0-4

044

C.,

__D CD3

VIA -_ -P c

Fiue 4. IBMP - Ero in topd poito duin a1

dg/s ~ ~ ~ ~ U manuve thog mutpearas niilcndtos

x-00 ft, y-O ft Vx-5 -tsc -y- -ts

- -- - - -- 4 4 C76C

CS;A

CIDis-iCD

* b.~.4. 00

'0 - CD01 CS

CD~

Figue 49 IB-PC rro in orpeo psitin dringa-1

dg/s manuve thoug mutipe arays Intia coditons

x-100 ft y- ftVx-50 f/sec Vy5 ftse4

774

44

croS- C.%__

-44

'-CD

c-ii

-4E

C" C! X! C7 OT/4V C"CC C4 c-D

Figure~~ 50 IB-C Tu taetr o h opeodrn20 d/ maevri hSrao snl ra.Iiilcn

ditinssx-500 f, yO ft Vx-92. ftsec Vy- fts-c

/7

.y U0 .1.-

E/ -4,..

c- Am

U-c

Ln'L

Lr31

U--

K'-

Figur 51. BM-- -- Etiat -f topd poiin uig-

_ %. _ I I I I IY%

Figure 51. IDM-PC -Estimate of torpedo position during a20 dg/s maneuver in the area of a single array. Initialconditions x=5000 ft, yO ft, Vx-92.5 ft/sec, Vy-O ft/sec.

79 '5

.72>

Y ~ ~ ~ .:1 7 -77

Cw%.

c1~i

-m

ca

cor

coc

or: C' :

Figure 52. IBM-PC -Error in torpedo position during a 20dg/s maneuver in the area of a single array. Initialconditionss x-Z000 ft, yin0 4t, Vx--92.5 ft/secs Vy-0 ft/sec.

s0

1

C%2'

-o

-4

I-.;

cm!C U7 C%! 0 _D %9: r% CI

Figure 53. IBM-PC -Error in torpedo position during a 20dg/s maneuver in the area of a single array. Initialconditionsm x-5000 ft, y-O ft, Vx--92.5 ft/sec, Vy=0 ft/sec.

B1

-4-

E-0

C2

l

U-Icv O

_I c:CD 0

-LIP

CII

CXD

ba.

C_2

cza

Co2

"I

'C3

X . i

Figue 55 IB -PC eal imePlot of etimted rajeto.of ~ ~ ' th opd hogh mlil ras. Iiilcni

tions: x=20000 it, y=O it~-, x -25f/eV = ft sc

C-.- a83

CS;'

CS!

c-I ~tv

_ "%

_ -9--' ID

~CD

4C;=!

i -r

844

6-s

PQa- -

9--4

a-.1- CS!

__4 -4 CD CD CD K2D-

Figure 57. IBM-PC -Estimate of torpedo position during a20 dg/s maneuver through multiple arrays. Initialconditions: x-20000 ft, y-0 ft, Vxi-92.5 ft/rn, Vy-0 ft/s.

85

r-kw

cm

- .,.--

0--4

.-.,.-..

CD3

-D

CS!4

__2 0%_ 4 a CD - -D

Figure 59. IBM-PC -Error in torpedo position during a 20dg/s maneuver through multiple arrays. Initial conditions:xin20000 ft, y-O 4t, Vxin-92.5 ft/s, Vy-O ft/s.

8

CID.

fma

Cs;-

CD 5

Col. K" __

Figure 59. IMP ro ntreopsto uiga2dg/s~~~~~ ~ ~ ~ ~ maevrtruhmutpeary.Inta odtos

x-00 tyOf, x-25 ts y- ts

87 %4

40 4

CS!.'S *

:w C% CO cl- U-b w C21 _4 C

__4 __ a 0 S S ; C 6 M I

f.4

%e

-

I CO

C%U-. I

CDa

OD CD CD C

Fiur 6.- - Tru trjctr of th topd in the

area of a single array. Initial conditions s x-3000 ft, y-Oft, Yx--50 ft/sc, Vy-O ft/sec.

89

-1 -61 j 1- - 7 =

cr% IN

co'

C.. ..

./I I

/ 5

U.. *.I..

90

.-

,. .,;5-5.1 , _

- !!

-' 90

-, ,.. .

-' .-.- . - . . . - .a..-j ..- -. .:.-. .- ., .'., L.C .:.-.-" .. " ' . .., '.h,: -'- -- - . - '

* --' ~ ------ -a .

C-4-

_ _ _ _ _ _ _ _ _ - CD

clk co cW11 C% L-4 m

C4 _ _ _ .. z I

I-J.

Figur 63. CI ro ntreo oiindrn

stagtrni heae fasnleary ntac n i io s x__3000 ___t, _____ it,________________________________

913

C..,

-- 6

_ _ _ _ --°%

-.

o-4Q

-M

C -

Fi ure-. CE -Err i opd oiin drn "

C=

~L

c-: cla c4 m" n *=

Figure 64. CIE -Error in torpedo position during astraight run in the area of a single array. Initialconditionss xin3000 ft, y0O ft, Vxin-50 ft/sec, Vy=O ft/sec.

92

-C-

CP OP.

Fiur 65. CI ro ntreodet.uigasrih

run s in te ae f a snl rry tA odtos

x-00 -t4- t x 5 t/aV- tsc

934

C-14

4-4

C

=0

CVVW

. . .. .

%- oz 4 -__

Fiur 6. CE -t. Varian of poV tio Lrrr-urig1

straight run in the area of a single array. Initialconditionst x-3000 ft, y-O ft, Vx--50 ft/sc, Yy-O ft/usc.

94

,~ ~ ,... - .. ' .. .., . L- L-. TV;. .3 1-- -Slug", ,7w, ... - .- . , . .

e

3 - - 4

P-4

%%"mc

•.-.. -4

zo-

I,

I I

-- % C %-- -- - -4

• I _-

"I' - "

U-," I

Figure 67. CIE Variance of position error during a

straight run in the area of a singlo array. Initialconditionst x-3000 ft, y-O ft, Vx=-50 ft/sec, Vy=O ft/sec.

95

I

9.

AD-AI67 871 KALMAN FILTER REAL TINE TRACKING USING MICROPROCESSORSAND A NEN LANGUAGE FOR MILITARY SOFTHARE(U) NAVALPOSTGRADUATE SCHOOL MONTEREY CA J A FERREIRA 27 MAR 86

UNCLASSIFIED F/G 9/2 ML

hmohmmmmmh

hhhhhmhmmmmhm

( 7919''

.

L.2 1(11f. 1111 .6

MICRIICOP' CHA T1

OdLlu..

I- W1W , ? w MI%7-_ L - -N .

/ Cm

- ICO L06

CD 0a Cm C

Fiue 8 CI Vaiac ofdpherrduigasrih

run in te ae-fa snl rry nta odtos

x-00 ft y- tdx-0f/eV- tsc

*II -96

-4

OM tea

C%63

4 'A

LW-V

4! -

CS;A

OD Mrs

-,

Figure 69. CIE -Filter estimate of mean-square errorduring a straight run in the area of a single array. Initialconditions, xin3000 ft. y0O ft, Vxin-50 ft/sec, Vy=O ft/sec.

97

Lk'

|Oo

US;

- L

C~CD

-O --

.-- o

codios x-00fyOft x 0f/a ,. yO ft/ea

-,-"

.-. ,.':..:

I... -

• : -.:-

". .. . . . . - . . . ,,..S

-"v .,'". " - -'.,J- " .'. ',_ -~ "-- - .--.- ' '. - V -''

YAM

4C

2=~

c-Jm

Ln -4 m~

LawI

du-in a tagtrnIIh rao igeary nta

cadtoesx30 iyOi, x-0ftsc y- tsp

I 99

C..,

* ~.

-~ b~.c~JC~, - C-.., -

us Cd~ -~ c'aL~.

C-

-5'. .5,.

~5 *5,*

-4

~~1

3- C-.,

LfI~.

LmJ 0%=3- I 5.*

-4 -

-"5C%3

0%

43-L=J Im~

-I

* S S p 5 5 5 5 C...,

-I -4-4-4-4 -4 -4-4-4-4 -4

(J CJ C%3 C%3 C~4 C~ C~ C%3 C%3 C-%I C~

S.

~- 9

Pigure 72. CXE - True trajectory of the torpedo in the-' area of a single array. Initial conditions a x3000 ft,

yin2121 ft, Vxin-33.9 ft/sec9 VyinO ft/sec.

.5.

5,

100

psi

<NS.~.**** ~ ,~ .'Sq

~5 * '. ~ 5*~5 -. *555*5 ~ S 5-.' * . , 9-

-I

.P IF

D-' -

c%

/ -6

*33m

.3.3i

C"! S! C; CW__ 31/ D

Figur 73 I.siae ftreopsiindrn

straight run in the area of ~ingearyntilcniinav x-30 ft a11fs-338f/,V- ts

U,101

Loa

_ _o _ _ _ -.

=

- co

_% %

.. ' U---"-U-" ... ,.

-'. .; C, ,2,.

-I- - -,-o4 -- u ""

Figure 74. CIE Error in torpedo position during astraight run in the area of a single array. Initialconditionst x-3000 ft, y- 2 12 1 ft, Vx-33.8 ft/s, Vy-O ft/s.

102

A....*

-U- ~ -U, -* * * -

.041 or-,

It

---

cm,cm!.

- ,..,.-

igreI 75. Ci

.- ;.

staighu run Ci - are of a sintio duria

conditionso x-3000 ft, y-2121 ft, Vx--33.8 ftls, Vy=O ftls.., .

103I

-i ,I

Lm.- -

., , , .- ., , 'j . . . -., . .. - .. .._ . - ' .. ' .. .. ' ..-_ . .. ' , . ' . ' .. . , ' , .. ,' ., . ' . ._ . . . ,=_ _ .' _ - .

U-31

-0-4

cr C":

-- 4~ ~ __co cm C

Fiur 76 CI-ro ntreodet uigasrih

1044

C-5

.. ~C=

Figue 77 CI Filer etimte o mea-sqare rro

duigasrih u nteae fasnl ara. Iita

codtonsx300fy211f, x-38 ts y- ts

L=105

.4o

IJ.

It,

t.4°

c--p

,.I -

4--4-

.4n -%4_ _4 -- _

I,.J._-- .

•2---

Figure 7e. CIE Filter estimate of mean-square errorduring a straight run in the area of a single array. Initial

conditionss x=3000 ft, y-2121 ft, Vx-33.8 ft/s, VyO ft/s. -#

106,."

-- 4

LmCL

I-V.U-A

-- M .7

....

cv I c , a'•--''.

7.7 ,,. -T.'-.N > ,w .wa

- /'

-/- " -I.

a -

I I

'a I

,, ,a-..

44

CS v" ii iS I

'a S;

- .-...

Figure 80. CIE -Tru trajectory o. the torpdo in the.area o a single array. Initial conditions x x-3000 ft,

Ym--3000 ft, Vx(I-33.8 4tlsec, Vy-33.8 ftlsec. ...

10,

,-.4- -

~o a-. ~ - _ .- , -.-

,,.

,," _ -,.-

$

le|

I

,,,, S.'..S - *N

.5 - w

a. - -i

* - "-

//I-. * S.-

]- , , ,,S

.: ,-.'..

,-. 109 "'-'

S °- .

*5 ..

1-1-

---------- --

_ _ _ _ _ _ _ _2

Lfl

1110

**'~~~~~~~-1.~~~w m d WV 7 91,-~~W~ ' ~-, - ,.

WIN

------------- _ ___ __ __ _

.4-

'-I o

__ _ _ __ _ _ __ _

'-3i

LfCD

p ~- M

Uam

~. -=-S;

staih run in th are of a ige ra. ntalc

tionst ~ ~ ~ U- x-00fy-00 t x-38f/s y3. t

L'

--

CD

_ 1=-S; ,w Cm.6

o= ,,o ,=,, u-, ., . 0,%.€ ,-

-- ; I z I I I I I I a I I

• Figure 84. CIE -Error in torpedo depth during a straight .,

i ~run In the area o4 single array. Initial conditionst x-3000.-,i"' ft, y--3000 ft, Vx--33.8 ftls, Vy-33.8 ftls. "

" 112""

! -S:. .

0.e

-C%-

I~a

0%a

'a:

Ci.

CM Cw cm 0%

Figure 85. CIE -True trajectory of the torpedo in theduring a straight runthrough multiple arrays. Initial condi-tionss x-n34000 #tq yin6050 ft, Vxi-684.4 ft/sec, Vy-O ft/sec.

113

-M -41

0~ eel / -

SICID

'pO

C.451.4

me IOp

93- Cdcmi

I-I 00-- Fr

C%-A C1.11 C

Figur/ 8U I siae ftreopsiindrnstagt rn thog utpe ras ntilcniinarex300 t -65 t x-e. t/ V- ts

11

.o -

4- - CID

.__ ____ __ - W-,|

0% o

CO CS • CS! -- D

Figure 87. IE Error in torpedo position during a

straight run through multiple arrays. Initial conditions 'arms x-34000 ft, y-6050 ft, Vx--84.4 ft/s, Vy-O ft'/s.

115

-" m-. ...

- -- r -"- -. -.- '--.- - r ,-- r . r r4W/W.J..W.. , , - -"-"

"'

4C.

"°"

NmI - M .., .,

I II I I ! I

WQ%.a.

.

Coo'

Wa C

C%2.

Figure 88. CIE -Error in torpedo position during a )straight run through multiple arrays. Initial conditions ..

are, x-34000 ft, y-6050 ft, Vx=-84.4 ft/s,,Vy-0 ft/s.

116--"-"

16

V-4CO* CS

Ln CS;~

=C

CO

10 CID,

CS!4

U-. CD

-47

.........-.-....-.... ,-.,

.. .,-

VI. COMMENTS AND CONCLUSIONS

There are slightly different versions of the program,

for the IBM-PC and for the CIE microcomputer. However, the

Sequencial Extended Kalman Filter program presented have V

many common characteristics. The execution time per cycle, -

in both machines, is less than the sample interval for the

data. The program is written in the "C" programming

language, and it is meant to be reasonably portable. Double

precision calculations are widely used, and allowed in many

cases to bound errors without repeating the calculations for

the same time interval.

Calculations are performed using an adaptive Q matrix. .

That is, the matrix 0 is recalculated each iteration, m.

instead of using a constant value. The trajectory simulation

routine was implemented using geometrical approach. The

tangential speed is assumed constant during the whole run.

The Kalman Filter can track through single and multiple

arrays. The algorithm always uses the coordinates of the

closest array in the calculations, for the multiple array

case. There is essentially, in either CIEKRI and KR, only a

main program without subroutine calls. The bidimensional

matrices were transformed and are used as vectors. Most

loops were eliminated to keep the execution time within the

required limits. '..--

118

.~.%...* S5*.. <..

The CIE version can also run in the IBM-PC but the IBM-

PC version runs only in the IBM since it calls assembly

language routines to generate graphic output. The IBM

version is slower than the CIE version and the IBM-PC

microcomputer must run with its 8087 math coprocessor chip

to satisfy the speed requirements. Both machines were

configured with hard disks, and the data files were read

from and written to the hard disks. Both, the IBM-PC and the

CIE versions, will run on the IBM-AT.

There is almost no documentation or software available

for the CIE microcomputer. The lack of documentation, the

lack of a good compiler, a full-screen editor, and the lack

of graphics capability were major problems in using the CIE.

The improvement in the execution time was more

noticeable in the CIE than in the IBM. This is probably due

to the presence of the math coprocessor chip, 8087, and (or)

due to a better compilerin the IBM-PC. The use of the "-f"

("fast floating point format") option, when compiling code

on the CIE, seemed to help both, the speed of execution and

the precision of the results.

The programs were tested for torpedo speeds from 20

knots (3Z.8 ft/s) to 55 knots (92.8 ft/s) and turn rates of

up to 20 dg/sec, or maneuvers of up to 1-G. The magnitude of

the position errors for linear trajectories were maintained

less than 10 ft in either computer. The CIE could not track

any of the circular maneuvers (run numbers 6, 7 and 8 in

119

......". ."...-,.

u~~~~~X - - _ . - _ - . . L - , -; - -_ -. . . . -. . . - -= -

Table 2). In the IBM-PC, keeping the maneuver within the

tracking radius of a single array (+ 3000 ft around the

array center), the magnitude of position errors were

maintained less or equal to 7 feet for an 1-8 maneuver and --

less or equal to 22 feet for the multiple array case.

The high error obtained for the circular maneuvers in

either the single or multiple array case reflects the

algorithm inability to follow fast turn circular

trajectories. In [11 such high errors were reduced to values

comparable to the errors found in the straight runs, by

adding the adaptive Q matrix to PKK to produce PKKMI and

repeating the calculations for the same time interval. This

was called "adaptive maneuver". The "adaptive maneuver" as

implemented (see segment 2, in the algorithm, in Appendix A)

was found effective for the IBM-PC when the value 10 was

used for the diagonal elements of the matrix R. However, to

keep the execution time per cycle within 1.31 sec, the

number of adaptive maneuvers performed had to be limited.

Almost no improvement in reducing tracking errors was

achieved when the value used was 10. For the later case a

great deal of degradation in the execution time was observed

and the time interval was exceeded even when just one more

iteration (optimum maneuver) was executed. The procedure did

not work on the CIE, which could not handle numbers less

than 10 reliabily. The "adaptive maneuver" feature was

120

.-7.

utilized only for the runs 7 and 8 (only 4 iterations were

allowed) and have produced, for the 1-C maneuvers, results

which are comparable to the results for the 1/4-6 maneuvers.

The single array cases showed smaller errors, meaning

probably that the technique used to switch arrays can be

improved.

An interesting point is the behavior of the curves for

the diagonal terms of the matrix PKK. The curves PKKX vs

TIME SLOT, PKK Y vs TIMESLOT and PKKZ vs TIMESLOT, which

are the mean square errors should grow proportionally to the

square of the position errors. These are depicted in the

curves X ERR vs TIME-SLOT, Y_ERR vs TIMESLOT, and Z_ERR vs

TIMESLOT. When such does not happen it means that the

filter model used is not a correct description of the

process. When the value of the diagonal elements of the

matrix R were 10 only the qualitative behavior of the

mean square errors agreed with the expected. When 10 was

used, the quantitative behavior of the mean square errors

were about the expected.

Additional tests should be performed using trajectories

generated from actual torpedo runs on the Dabob test range.

As a future step, there is the need to adapt the program to

provide it with the correct interface formats to access the N

real data, before running it in the real-time conditions on

the range.

. . ._ . .•., • . . • .. i• > _ > .' , .

APPENDIX A

PROGRAM DESCRIPTION

A. OVERVIEW

There are at this moment, three versions of the Extended

Kalman Filter program. In either case the simulated

trajectory must be first computed and the hydrophones array

times, corresponding to the simulated positions, are stored

in a file in order to be read in. All three programs write

the files XKK.DAT, PKK.DAT, and XDIF--.DAT to the disk.

To avoid delays due to data writing and retrieving from

disk drives, the data files can be written to and read from

an electronic drive built in the RAM memory. There is also a

utility program - PLOT.C - which allows fast graphic

analysis after the run.

1. The CIE1.C Program

This was the first program to run in the CIE

microcomputer. It reads "clean" (without noise) trajectory

data from the file TRAJ.DAT and writes 3 columns,

corresponding to the coordinates x, y, and z, to the disk.

The code to generate Gausian noise is contained in the main

program.

CIE1.C generates 3 data files, XKK.DAT, PKK.DAT, and

XDIFF.DAT, containing filtered data to be post-processed.

XKK.DAT, contains 4 columns, corresponding to the time slot

and the filter estimates, X(k/k), for the coordinates x, y,

122

~..................................

... . . *.J*I .I l d

and z in that order. PKK.DAT contains 4 columns with the

corresponding values of the time slot and the error

covariance matrix for the coordinates x, y, and z, in that

order. XDIFF.DAT contains 4 columns with the corresponding

values of the time slot and the errors for the coordinates,

x, y, and z, in that order.

2. The CIEKR1.C Program -.

This program runs in the CIE and provides input and

output similar to the KR.C . It is able to simulate only

straight line trajectories, due to the lack of an "arc tan"

function in the CIE's C Compiler, and at present does not

access the computer internal clock.

3. The KR.C Program

This program includes a set of assembly language

routines for interfacing with the keyboard, display, and

output ports of the IBM-PC/AT. It also includes a plotting

routine, written in C, which calls the assembly routines to

generate the frame, to print updated values, and to draw the

estimated trajectory. Another assembly language routine was

added to allow access to the internal clock in the IBM-PC.

See references [73 and [8] and Appendix C for details.

The Kalman Filter section of KR.C needs the files

TRJ.DAT and ZIC.DAT, generated by the trajectory simulation

routine, for inputs.

The trajectory simulation generates transit times

data already corrupted with noise. That involves inclusion

123

I.,,

of the noise generation routine with the "clean" trajectory.

The clean trajectory can be either a straight line or a

circular curve.

The trajectory simulation routine creates the files

TRJ.DAT, ZIC.DAT, TRJ.INPq and PARAM.DAT.

The TRJ.DAT contains the data for the clean

trajectory, corresponding to the time slot, and the x, y and

z coordinates. ZIC.DAT contains the noise corrupted transit

times. TRJ.INP and PARAM.DAT contain information to be used

by PLOT.C program in order to identify and relate the

trajectory data and the parameters used in the Kalman Filter

to the graphic outputs.

B. DETAILED DISCUSSION

Since the programs evolved one from the other, only the

last one, KR.C, will be discussed in more detail. The-

routine that generates the trajectory and the tools

developed to facilitate the objective analysis of results

will also be briefly described.

1. The Kalman Filter Program - KR.C

The algorithm for the program KR.C is provided as

Appendix B and the actual program implementation is shown in

the Appendix E.

The capability of tracking trough multiple arrays is

implemented by simply defining switching points along a

coordinate axis, and comparing the estimated x-position

against switching values. The coordinates of the tracking

124-

124 .'"

'9 . . .-.. * .* .* - ** * *. * -. . 9. .-. * . 9... , -. - . . . . . . . .-.

array are then changed in order to always keep the closest

array tracking the torpedo. Figure 90 shows the coordinate

system for the multiple array tracking, the location of the

switching points and the hydrophone location matrix.

Tests performed using a straight line trajectory and

keeping the Q matrix constant showed no improvement over the

case where the adaptive Q is used.

2. The Trajectory Simulation Routine - TRAJEC

This routine was developed as an independent

simulation program, to generate trajectories, where circular

and linear segments can be combined to produce a desired

path.

Given the initial x,y,z, positions and the initial

speeds and assuming that the vertical speed is zero,-this .

routine generates noisy data to represent the "raw" data as

it would come from the hydrophones. TRAJEC finds the

position of a point in a circle after a time interval T,

given the tangential speed of the point, and given the

desired turn rate or given the centrifugal acceleration. It

is assumed that the tangential speed is constant during the

maneuver. Either the turn rate or the centrifugal

acceleration can be used to determine the radius.

Figure 91 represents the geometry of the problem. The

straight line shown is tangent to circle. A point is

initially moving on the line and at the time to it is

125

Y (f eet)

(-array(- 4array(-i .array-o array(-1 4array. -array-.

6 a 5 . 4 £ 3 . 2 a 1

6k -------------- *---I---*---I---*----*----

I I I I I 1

4 i X . I I I . I X(f eet)

6k sw4 12k sw3 16k sw2 24k swl 30k swO 36k

Coordinate System for Multiple Array Tracking Case

C HYDRO X HYDRO Y HYDRO Z HYDRO

I I I I,..i -- '-

x y z x y z x y z x y z

36000 6000 0 36030 6000 0 36000 6030 0 36000 6000 30

30000 6000 0 30030 6000 0 30000 6030 0 30000 6000 30

24000 6000 0 24030 6000 0 24000 6030 0 24000 6000 30

16000 6000 0 18030 6000 0 18000 6030 0 18000 6000 30

12000 6000 0 12030 6000 0 12000 6030 0 12000 6000 30

6000 6000 0 6030 6000 0 6000 6030 0 6000 6000 30

Hydrophone Array Location Matrix

Figure 90

126

.*. . . . . .. . . . . . . . . . . . . . *J..*

v Y IN

or.

ip.

0

Figure 91 1 The circular trajectory problem.

127

subjected to centrifugal force which makes it turn. The

point then describes a circular movement generating a circle

of radius R, and at the time t +T will be at the position

XqY. Assume V yand V xknown speeds, m - V y/V xis then the

slope of the line tangent to the circle that passes at the

* point xI, and has center at a,b. The angular coefficient

of the line perpendicular to the circle and passing at the

point xOy 0 is

(X0 -a) /(yo b) I m ,so

X0 a -m ( y - b)

0 )

and R2 N x a)2 + (y -b) is valid for any point

on the circle. -

at x0 , 0 2 ( X- a )2+ (yo - b) 2

R2 (M2 1) (y b b) 2

-o b + R /sqrt(l + m 2 )

or biny0 +R sqrt(l+ M 2 )

and a Xe -- M* (Y0 b)

For any point on the circle

x - a - R cos

y - b- R sinG

From the Figure 91

128

~tan m

Gi vanV~ V 4 V msqrt(VI I

Given w 4 R V~ / %1

Given the central acceleration, g -~~ g V Vt

At the and of the interval, the position will be x,

y and the speeds will be given by

V - - V sinx t

V - V Coy t

These equations, when implumuntud, generate the true

traJectory.

The routine also computes the measurement noise

times. The Gaussian noise is generated by computing the

linear approximation of the inverse of the normal curve C93.

The input comes from a sequence of uniformly distributed

numbers generated by the equation below

r(I + 1) - r(i) * 317.mod(1)

The resultant sequence is zero mean and is scaled to obtain

the desired variance.

3. The Plotting Routine -PLOT

The plotting routine is contained in the file PLDT.C

(IBM-PC), and was written as an independent module to be

129 '

linked with BIOSIO.OBJ, and reads data from the files

XKK.DAT, PKK.DAT, XDIFF.DAT, TRJ.INP, and TRJ.DAT.

That is a menu driven program, which provides

fourteen (14) different options and was designed to simplify

the analysis of data generated in a torpedo run. The ploks

have their titles already chosen. The scales reflect the

maximum and minimum values. A reference number and a label

for SINGLE or MULTIPLE array cases is printed on the plot

for reference. Linear interpolation is used to interconnect

the sample data points. An optional plot, where the data

points are simply plotted, is also available. This last

option would allow the time to be associated to each point

on the curve.

In an attempt to provide the CIE machine with

similar capabilities, an ANSI driver was also defined and

tested successfully in the IBM. However, that provides a

very limited graphics capability, allowing only 80 columns

by 25 rows of resolution against the 640 by 200 in the IBM,

in graphics mode. The file KR1.C makes use of the defined

driver to produce the graphics presentation of the estimated L

trajectory.

For UNIX systems, like the CIE, one might want to

consider the "termcap" database of hundreds of encoded

descriptions for terminal control. However, except in

unusual circumstances, C's standard-library I/0 functions

130

"- " ". - "" * "-"**'4 4- ~ 4'' " -'' " ."- - .''" -.-. "4-.. " -.... ". """.-.. . . . . . ."-. . .' ' "' ' ""'" -

V A

provide excellent portability and a wide range of display

features C63

4. BIDSIO.ASM and BIOSIO.OBJ

Those files contain the assembly language code 10"

(public domain) and the object code (IBM-PC) to interface

the C programs with the keyboard, display, output ports and

to access an absolute memory location.

5. DOSTIME.ASM and DOSTIME.OBJ

These files contain the assembly language code and

the object code, respectively, that allow access to the

internal clock (IBM-PC).

6. The SIMTERM program

This is a public domain program written by Jim &

Eric Holtman that allows one to emulate an HP-like terminal

to a UNIX system. It is a self contained module that allows

one to connect the IBM-PC to the CIE microcomputer, directly

by using the RS-232 output on the IBM and one of four (4)

output ports on the CIE.

7. The KALMAN.H header file

It contains the definitions of the program

constants, initial values of parameters, and the definition

of the ANSI terminal driver, which can be used in the future

to generate rough plots on most of the nongraphic terminals.

131.

.. / .,..- . . , ,. . ... . *.*%,° *" . .*.-.- -.. - . .*,* .-.- .* . ... ".-.- .-..- -- -... .". , ,.."

APPENDIX B

ALGORITHM FOR THE KR.C PROG3RAM

*duf ine CONSTANTS 1* used to def ine constants *#def ine MACROS /* define macros */*include FILE.H /* include header file ~double xkkE2003; /* declare global variables *main() /* main prog. declaration *

f 1* block/function delimiter */* DEFINING FILES *FILE *fpxyzv *4openo; /* declares fpxyz an a *

* 1* pointer to a file ** /* yet to be open using*/

/* fopen() function */* DECLARATION OF VARIABLES *int i,j; /* long integers *short ii,jj; /* short integers *float x,y; /* single precision *1ndouble xx,yy; /* double prec. reals */* OPENING FILES *+Fppifopen('pkk.dat","w"); /* open file pkk.dat *

/* for writing *f pef open ('xdi ff. dat", "w") ; /* open xdi-Ff.dat *fpxyz-fopen("xkk.dat",'w'); /* open xkk.dat */* INITIALIZATION OF VARIABLES *

Initialize HYDRO[3Initialize gammaE3Initialize covwE3Initialize xkkmlCJInitialize xbE3 ,ybEJ ,zb[3

/* DATA INPUT -INTERACTIVE *menu: Put menu

Get inputsPerforms actions

Case statement1 Run trajectoryWrite run# and xkkmlE3 to trj.inpCall trajec to read trj.inp andgenerate noisy trajectory

2 Run Kalman filter3 Modify Parameters

a. Optimization criteriab. Single/multiple arraya. Initial x, y, zd. Initial Vx, Vye. Noise covariance

132

f. Error covarianceg. Redefine maneuver

- Turn rate based- 3 number- Start/end time

h. Main menu/* Run Kalman filter */

runs Get run # for referenceSave inputted data into filesOpen files trj.dat and zic.dat for readingInitialize QFIND blockInitialize hrow[]Initialize pdum]3Initialize rr[]Prepare screen for real time presentation

Calculate scalingDraw frame

Get time from internal clock/* START TIME LOOP */

Start time loop : for (kk=O;kk<itime;kk++)

Get hydrophone array coordinatesagain: Get time

If time interval < 1.31 goto againUpdate timeGet true times e true positions (noisy data)

Read dataE] from file trj.datRead zic[] from file zic.datSave true positions in true[J

adapt: /* GET HROW, CALCULATE GAIN, ESTIMATE *//* COVARIANCE OF ERROR SEQUENCIALLY *

js: for (imO;i<4;i++){

Calculate denom using double prec. sqrtCalculate hrowCO], hrow[2], hrow[4]Calculate gnum[], gdenomCalculate git] = gain columnCalculate covariance of error pit) :"

for (ii=O;ii<25;ii++)

calculate pdum["

calculate pit)Calculate measurement prediction

calculate zhat=denom/4860.calculate zdiffti]=zic[i]-zhat

Compute 3sigma gatep-max (pi O)],pi [ 12),pi [24])/* use single precision sqrt */gate=3.*(sqrt(p)/486O + rr[i])

Edit invalid time measurements

133

• h --. - . - , - . . - , . - . . . . . , . . . . . . . . . - - - - . . . , - . - , . . , . , ,. . - . , - . . - . ' - . - . j ' -

Set fl1agPut gain, giC3-0

Calculate estimate based on one ineas.

for (ii-O;ii<5;ii++) peitoxi Cii3-ixkkm1 Cii 3+gi Lii 3*zdiff Li

1* Update for next column *-For (ii-O;ii<5;ii++)

xkkmlEii3xitii3pkkmlEii 3pitii3

for (ii-5;ii<25;ii++)

ifiQ-3) goto 156/ * end of is loop *

/* Accept values as updates for time *

1561 D updates for the time intervalxkkmlC3-xiC3pkkmIC3-piC2xkk I 3xi C3pkkC3=pi E3

Recalculate time measurements and form(1) 1absolute value of residuals

Edit invalid times for adaptive maneuverroutine

Compute adaptive 0 matrixIf (icurve==1) compute adaptive 0else use constant Q

Check timeBy pass maneuver if no time leftCheck for valid measurementsBy pass adapt. maneuver if all invalidBy pass maneuver if not steady state

(2) Check average zdiffiJi vs. zdifavBy pass maneuver if avg.<zdifav/* Adaptive maneuver *

pkkml[3-pkktJ+q[3/* Reiterate same time slot *

goto adaptCompute measurement errors

xdiffE03-xkk[03-trued[03xdiff[13=xkkL23-trued[1Jxdi ff L23xkkC43-trued143

Compute switch to change between Q(3) constant : straight line trajectory

or 0 adaptive : circular trajectoryCalculate predictions for the next time

pkkml[3 and xkkml[3

134

Output results to the screenxlylz,v,VY,time valuesplot xkk[OJ, xkkC23

Output results to filmsxkkC2 ,pkktJ ,xdif4C3

) * and time loop /'Close files

) * and maino)*

135

APPENDIX C

SETTING UP FOR EXECUTION

A. USER'S NEEDS

This section describes the actual hardware and software

utilized to produce the results in Appendix H.

HARDWARE

The following describes the hardware utilized and the

way to interconnect the IBM-PC and the CIE microcomputer .

IBM-Pc : 2 floppy disk drives, 10 megabytes hard disk,

graphic board, graphic color display, serial port (RS-232),

B087 math coprocessor chip and Epson parallel printer;

CIE 680/30 : 20 megabytes hard disk, 4 serial output

ports, Amdek monitor and keyboard.

The computers were interconnected by cable, from the IBM

RS-232 output port to one of the 4 ports on the CIE.

SOFTWARE

The files and/or programs utilized are:

1. SIMTERM - communications program (in the IBM). Allows

transferring files between the IBM and the CIE.

2. CIE.BAT - batch file (IBM). Executes the necessary

sequence to interconnect the IBM and the CIE. Needs SIMTERM.

3. KR.EXE - executable file that contains the program to

simulate the torpedo trajectory and the Kalman filter

program (IBM). The trajectory routine is executed directly

from the KR.C program as a menu option and it generates the

136

* .**,.*. ~ -..- o-. -

data files "trj.dat", and "zic.dat". Then the trajectory and

time information from the files, "trj.dat" and "zic.dat", is

used by the Kalman filter algorithm to produce estimates. It

generates the files "xkk.dat", "pkk.dat", 'xdiff.dat",

"trj.inp" and "param.dat". At the end of the run, a total of

7 files would be generated.

4. PLOT.EXE - executable file that contains the plotting

routine (IBM). Allows posterior graphic analysis of the

results. Uses the information contained in the files

xkk.dat", "pkk.dat" , " x diff.dat", ,°trj.dat"I and "trj.inp".

5. KAL.BAT - batch file (IBM). Executes the necessary

sequence to run KR.EXE and PLOT.EXE.

6. KALMAN. H - header file (IBM and CIE). Contains

definitions used by CIEKR1.C , KR.EXE, PLOT.EXE, TR.EXE -

7. a.out - executable file generated from compilation of

CIEKR1.C on the CIE machine.

B. PROGRAMMER'S NEEDS

It is highly recommended to have a good full-screen text

editor, and the source code files included in the directory

of the IBM-PC. Source code files have extension ".C". A good

C compiler (IBM) is also recommended.

The software required is:

1. LATTICE C or MICROSOFT C compiler

2. FULL SCREEN EDITOR ;

3. SIMTERM- terminal emulation / communication program;

137

B-I..

1 7 ..'.Z7

4. SOURCE films u TR.C, KR.C, PLOT.C;

5. HEADER files 3 KALMAN.H, MATH-H9 STDIO.H;

8. ASSEMBLY LANGUAGE ROUTINES: BIOSIO.ASM, DOSTIME.ASM;

7. IBM MACRO ASSEMBLER

8. MICROSOFT LINKER.-

Using the Lattice C compiler requires the files LCI.EXE,

LC2.EXE, LCS.LIB, LCM5.LIB, CS.OB3, MATH.H9 STDIO.H to be

present. The assembly language routines must first be

transformed into object films by using the Macro Assembler.

The file TR.C must also be compiled using LCI and then LC2

to generate the TR.OBJ file. These are all linked to the

file KR.OBJ to produce the executable file KR.EXE

1383

APPENDIX D

COMPILING, EXECUTING, TRANSFERRING FILES, AND PLOTTING

A. RUNNING THE PROGRAM ON THE IBM-PC

This will assume one is running the Kalman Filter

program on the IBM PC/AT with a hard disk drive defined as

drive "c".. The system prompt is "c>", and the required

files are in the subdirectory named "KALMAN".

If you are not in drive "c", change to the correct drive

typing "c:" and hit "RETURN". The system will then answer

with the prompt "c>". Type "cd kalman" to access the

directory KALMAN

1. Compiling KR.C

This will assume you are running the Lattice C

compiler. Type "ce kr" to compile the file KR.C, using the

batch file "CE.BAT". The files KALMAN.H, DOSTIME.OBJ,

TR.OBJ, BIOSSIO.OBJ, LINK.EXE, CE.BAT, and KR.C need to be

in the same directory.

The Appendix E contains a listing of the file

CE. BAT.

2. Executing KR.EXE and PLOT.EXE

Type "KAL",in drive "c", to execute the batch file

KAL.BAT. The required sequence of commands is initiated and

can be interrupted by pressing "CTRL-BREAK" ("CTRL" and

"BREAK" keys at the same time). The Appendix E contains a

listing of the file KAL.BAT . -t

139

li " i..

- Y A..rnu 7 V 7.7v

IIb

The first file to be executed is GRAPHICS. It allows

dumping IBM-PC graphic screen to the printer.

Next the file KR.EXE, containing the trajectory

routine and the Kalman Filter program, is executed. Menus

give the indication for data input, to run the trajectory

routine, and to run the Kalman Filter. One normally defines

the parameters first (option # 3), then run the trajectory

routine (option # 1), and then run the Kalman Filter (option

* 2). .-.

After the run, the plotting routine ("Plot") is

called to produce the graphic output. Up to fourteen

different plots can be generated by making the desired

selection from the menu. The output to the printer is

obtained by pressing "SHIFT-PRTSC" ("Shift" and "PrtSc" at

the same time).

If a plot ends abnormally, for instance by the use

of CTRL-BREAK, the screen i.s left in the graphics 640 by

200 mode, then the command "mode bw8O" will set the display

mode to Black and White with 80 characters per line.

B. RUNNING THE PROGRAM ON THE CIE

Turn on the system with the floppy disk drive open. This

will force the system to boot and accept the internal hard

disk drive as the current drive. Turn on the terminal

display and when the cursor appears, press "ENTER". Follow

the prompts for date and time input.

140 .

* . * .~ i .. .. ,'. . .

Ul**~ -..-.

I..'.

After booting, the screen will show the message "CIE

GRAPHICS SYSTEM # 7" at the top, and "login : " at the

bottom. At this moment one should have the SIMTERM diskette

in drive A (the one at the left) on the IBM-PC. If the

logged drive is not A, one should type "a:" and press

"ENTER" to change to drive A . In drive A, one should type

"cie" and "RETURN" (ENTER). The batch file CIE.BAT will send

the correct command line to connect the IBM and the CIE.

Make sure the cable connection is already done.

The message "connection established" will then appear on

the screen. Press "ENTER" to get "login:". Log in using the

account "jose", and press "ENTER" for the password. The -

system prompt is "" Change directory using "cd work"

One is then ready to transfer files or to execute a program

in the CIE machine.

1. Transferring Files from the IBM to the CIE

To transfer a file use the redirection capability of

the Unix system and the command "cat".

Type "cat > filename.ext" and press "ENTER", at the

system prompt ("."). The cursor will stop at the begining

of the next line. "ALT-D" will dump a file to the

communication line. One will be prompted for the filename.

If, for instance one wishes to transfer the file

"kalman.h", which is in drive c: (IBM), to the CIE, given it

the name "kalman.h", the following sequence should be

performed, starting at the prompt "." : Type "cat >

141

"- ' - ' -" " " "J" ' ' J' ,' '- ",a': -s"..-. . + .. ,... ... ...-..... ,.,•........ . . , .., ,..

the name "kalman.h", the following sequence should be

performed, starting at the prompt "" 1 Type "cat >

kalman.h"; press "ALT-D"; type "czkalman.h" , when asked for

the IBM filename, and press "ENTER" to start the

transmission.

At the end of the transmission the computer bell

will ring, signaling buffer overflow. One should wait until

no more new lines are being written. Press "CTRL-D" to close

the file in the CIE. The prompt "" will reappear on the

screen.

The new file (kalman.h) will have to be edited on

the CIE to remove the inserted empty lines:

Type "ed kalman.h" and the editor will show the

number of lines in the file. Then, type "g/^$/d" and press

"ENTER". The number of lines in the file will be reduced to

the half. This will not affect the blank lines originally

in the program, just the ones inserted by the process of

transmission.

Type "w" to write the modified program version to the

disk, and "q" (quit) to leave the editor.

2. Compiling on the CIE

Type the command line "cc -f -v -L -7 ciakrl.c" to

generate the executable file "a.out".

where : "cc" invokes the C compiler;

"-f" option , informs the compiler to use "fast

floating point format";

142

~.q'~p -~ ~ 'A'.Nip !V -J ~ rwr ? -.F 7d01 v-xrpkw-a 717-r -40"*sWX

I-Lu option instructs the compiler to generate

pointers to allow long jumps, for a code~data greater

than 32 kbytus;

"8-7"4 option instructs the compiler to use the Unix

version 7 standard library.

3. Executina CIEKR1

Type "a.out" to start execution and then follow the

menus for inputs. Since the CIEKR1.C program does not

generate circular trajectories, the files containing the

trajectory data (TRJ.DAT) and transit time data (ZIC.DAT)

will have to be transferred from the IBM to the CIE, in

order to run the Kalman Filter for the circular trajectory.

Be sure to input the same data that-were used to generate

the circular trajectory on the IBM, and then select option

*1 (Run Kalman). For straight line trajectories, just follow

the menus.

4. Plotting the Results

Since there is no graphics capability built in the

CIE, the only way to obtain plots is to transfer the files

XKK.DAT, PKK.DAT and XDIFF.DAT, obtained on the CIE, back to

the IBM and then use the routine PLOT to generate the

desired graphic output. The procedure is described below:

"ALT-C", during the SIMTERM session, will open a

capture film. One will be asked the name of the capture, or

log film. The drive name should be specified.

143

For instance, assume one wants to transfer the file ,...,

XKK.DAT from the CIE to the IBM in order to use it later to

generate plots. Since, the routine PLOT reads specific

files, be sure to erase the old file XKK.DAT, in the IBM,

before copying the file XKK.DAT from the CIE, because the

same name will be used.

Type "list -n xkk.dat", at the prompt "%", without

pressing "ENTER". Pressing "ALT-C", will start the capture

mode. Then one will be asked the name of the log file. Type

"cxkk.dat", and press "ENTER", to open the file on drive C.

The command line "list -n ... " will reappear. Press

"ENTER" to start the transfer and the capture. Press "ALT-

C"at the end of the file listing, to close the log file. . -

After doing similar procedure for the files

XDIFF.DAT and PKK.DAT, leave the account on the CIE by

typing "logout".

Leave the SIMTERM program and drop the line,

pressing "ALT-F2". One will be back to drive A on the IBM.

Type "c:", to change logged drive to C

One may use his preferred text editor to eliminate

the undesired lines in the files XKK.DAT, XDIFF.DAT, and

PKK.DAT. The files should contain only four columns of

numbers, except for the first row, which is blank. Then one

can use the routine PLOT to obtain the desired plots, by

typing "PLOT =8000", and following the menu options. .

144

1%:

4

.,r..-4-# .

APPENDIX E

KR.C PROGRAM LISTING - IBM-PC VERSION

This Appendix contains a listing of the Extended Kalman

Filter program implemented in the IBM-PC microcomputerg as

well as the listings of the Trajectory Simulation routine

and the header file "kalman.h",, which are used by the main

program KR.C *The other header files declared at the.

begining of the main program are standard files contained in

the C compilers.

145

'...77

/4'K P4I-

/I* HEADER FILE - KALMAN.H ,l

/**'4***************4''''*'''4*4444*****4''4** *******4''*********'-

*define nmax 5 /* * of col. in pkk, q ... *I*define maxsq 25 /* size of equiv. vector PKK,Q...*/*define is 4 /* * hydroph. in any hydrophone 4',

/* array *I#define hcmax 12 /* # of col. in matrix HYDRO of */

/* hydroph. c-ord. */#define hmax 72 /* equiv. vector size of HYDRO 4'-

#define dmax 3#define itime 150 1* max.* of time slots *I#define npt 150*define icmax 5 1* max.* of optimal iterations */

/* allowed *I#define criter 0.1 /* criteria to compare ZDIFAV *'!#define na 0 /* may assume ONLY values 0 or 6 *'-

/* 0 =>single;6=> multiple arrays *I#define xpos 10000.0 /* initial torpedo x-coordinate 4',

#define xvel -50.0 /* initial torpedo x-velocity *I#define ypos 0.0 /* initial torpedo y-position 4'-

#define yvel 5.0 /* initial torpedo y-velocity */*define zpos 0.0 /* initial torpedo z-position *1#define swO 33000.0 1* swi define points where */#define swl 27000.0 I* switching from one to *I*define sw2 21000.0 /* another *-"*define sw3 15000.0 /* array */*define sw4 9000.0 /* is done. */#define sigdiv 1.0 L *#define scc 0.387463094 /* - in radians... *',#define sacc 36.20#define ve1 4860.0 /* speed of sound */

*define twopi 6.28315308#define rrvar 1.e-5#define rvar 1.e-10#define pkkdiag 1000.0#define limvar 1.5 /* defines threshold to use Q*/

/* constant 4'#define gmO 0.86#define gml 1.31#define cwO 1.0 /* initial was 1036.84 *I#define cwl 1.0 I* initial was 1036.24 *I*define cw2 1.0#define xminl -5000.0*define xminh 0.0#define xmaxl 5000.0#define xmaxh 40000.0

146

...........-....... ....... _............

I~~~~~~~~V T w~rrwz~~z . - -cr - .r *r.... . -' --- v - - .C .. - Z-

*doinn yminl -5000.0*dsfins yminh -5000.0*duF ine ymaxl 5000.0*def ine ymaxh 5000.0

/41 DEFINE ANSI DRIVER *

/41 *define STDIN 0 //41 #define STDOUT 1 /1* #define STDERR 2 **duf ins CLRS.CRN 4 puts ( \33C2J", stdorr)*define CURM(rowcol) 4printf(stdorr, "\33t%d;%dH, row,Cal)#def ine CURJJP~num) fprintf (stdvrr, "\33r%dA", num)#dsfine CURDN(num) fprintf (stdmrr,"\33E%dB", num)*define CURRT(num) fprintf (stdmrr,"\33C%dC', num)#deufine CURLT(num) fprintf (stderr,"\33E~dD"q num)#define CURPKIP fputs("\n", stdorr)*dtf ine CURS.AVE fputs( S\33E3BI stderr)*1def i no CURREST fputsea\33Eu~q stdorr)#def ine CLRjLINE fputsQ'\33EK",j stdorr)*def ine BELL 4 putc('\7*, stdorr)*define CLREOS(r, 0) (byte i_; CURM(r,c); \

f or (i_.!r; i_.<-25;+4i_) CLRLINE, CURDPN (1);CUR MV~r,c); I

147

1. REAL TIME KALMAN FILTER PROGRAM -KR.C *

#include 'stdio.h"#include "math.h"i#include "kalman.h"doublexkkmlrnmaxJgpkkmlCmaxsq~,pkklmaxsq3,xkklnmax3,dataldmax3;

1* STARTING THE MAIN PROGRAM... *maino(

1* VARIABLE DECLARATIONS *

FILE *fpxyz, *fopeno;FILE *fpe, *4openo;FILE *fpp, *fopeno;FILE *fptrj, *fopeno;FILE *fpzic, *fopeno;FILE *4param, *fopeno;FILE *fpin, *fopeno;

int ihourssimingisecs,ithscol ,row,maxopt;int iG,i ,j,kk,iin,icont,icurve,option;int param,trjoptgnrange;int ii ,jj,narr-ay,nzdiff,in,pli ,runn,c;int jstart,jand,worg;float rabs,zgate;double height,width,ymaxlymin,xmaxlxmin;double xscalu,yscale,bx,by;double hours,mins,secs,ths,time,oldtime,deltat;double sqrto,oldx,valumexO,yO,zO,vxO,vyO;double denom,a14, qdenom,gate, zdifav;double den,velm,temp;double a2,gl,g2,g3,pl,xdif+E3;double zhat ,zc , dk, .igcc, aigacc;double v,tr-uedr33,wd,g;double x ,y,z ,auxhgxd,yd,zd;double al,el,e12,e2,bb,dd,bd;double pki,pk3,pk5,pgate,rgate,p,pll;double phipkk~maxsqJgpktemp~maxsq3,qlmaxsq3;

148

double pi~maxsqlqpdum~maxsqlgqtempCmaxsq3;double gnum~nmax),gi~nmax~,xi~nmax~,hrow~nmax3;double zic~js~,ziCjJ3,zdif4lsJqrCjuJ,phis~isJqztrjs3;double xbCj%3,ybCj93,zbCjs3,rr~js3;*double hydroChmax3I,covwCZ3,gammaC2l ,optC53;

1* INITIALIZATIONS

f pxyznf open ('xkk.dat' , lw");fpe-fopen("xdiff.dat",'"w");*f ppnf open ("pkk.dat",I'lw"I);

1* INITIALIZE CONSTANTS, HYDROPHONE MATRIX, PHI, R,DATA FOR TRAJECTORY, AND LOAD X(0/-i), P(0/-1)

rnzdiff-i4; icontO0; icurve=l; valuecriter; wd-5.730;runn-1; narray-na; param-0; worginl; g=0O.156; nrange=0;Jstart=40; jend-80; ±8=0; maxopticmax;

hydroE03-36000.0; hydrorl3= 6000.0; hydroE2in 0.0;hydroE33-36030.0; hydro[43= 6000.0; hydroC53= 0.0;hydro[63-36000.0; hydroC73= 6030.0; hydroCElin 0.0;hydroE93=36000.0; hydroEI03=6000.0; hydroE1l]= 30.0;

hydro[123-30000.0; hydroCl33-6000.0; hydro[l43- 0.0;hydrotl53-30030.0; hydroCi136000.0; hydroC173= 0.0;hydroE183-30000.0; hydro[193=6O3O.0; hydro[2OJ- 0.0;hydroC213-30000.0; hydroC223-6000.0; hydroC233= 30.0;

hydroE243-24000.0; hydroC253=8000.0; hydroC263= 0.0;hydro[273=24030.0; hydrol283=6000.0; hydro[293= 0.0;hydro[303=24000.0; hydroE33I6O3O.0; hydroC323= 0.0;hydroC333-24000.0; hydroC343=6000.0; hydroE353= 30.0;

hydroE363=18000.0; hydroC373=6000.0; hydroE3Blin 0.0; .-

hydroC393-18030.0; hydro[4OJ=6000.0; hydrot413) 0.0;hydroE423-18000.0; hydroE43J=6O3O.0; hydro[443- 0.0;hydro[453-18000.0; hydro[463=6000.0; hydroC473= 30.0;

hydro[4e3-12000.0; hydroE493=6000.0; hydroE5O0J 0.0;hydroE513=12030.0; hydro[523-6000.0; hydroE533) 0.0;hydro[543=12000.0; hydroC553=6O3O.0; hydroE563= 0.0;hydroC573=12000.0; hydrot583=6000.0; hydrot593- 30.0;

hydroE60OJ6000.0; hydrot613=6000.0; hydroE623= 0.0;hydroC633)6030.0; hydro[643=6000.0; hydro[b53- 0.0;hydrotb66=6000.0; hydrot67)in6030.0; hydrot683) 0.0;

149

hydroE69Jin8000. 0; hycdroE703-6000. 0; hydroE713- 30.0;

rE03-rvar; rEljinrvar; rE2jinrvar; rE33-rvar;

gammaE03=gm0l gamma~l3-gi;covwE03-cw0; covwC I3Jicwl; covwC23-cw2;

XO-xpos; vxo-xvffl; yOmypoS; vyomyvel;*~z 0z POE;

for Ci0O;i<nmax;i++)for (j0O;j<nmax;j++) pkkmlrz*nmax~j3-0.0;

for (10O;i<nmax;i++) pkkmlti*nmax+i3-pkkdiag;

phis[0O-l.31; phis[13-0.0; phisE23-1.31; phisE33-0.0;

xb[O-(-15.0); ybE03-(-15.0); zbEO03(-15.0);xbC13- 15.0; ybCl3m(-15.0); zbCl3J(-15.0);xbE23-C-15. 0); yb[23- 15.0; zb[23-(-15. 0);xbt33J(-15. 0); ybE33J(-15.0); zbE3Jin 15.0;

/* INITIALIZATION FOR WFIND *vel M-1. * /vel;a14-phisEOI;a2-al4*al4;gl-a2*0. 5;g2-gl*gl;g3-al4*gl;si qacc-sacc*sacc;

"I £igcc-scc*scc;for (iinO;i<nmax;i++)

for (j0;lj~nmax;j++) qli*nmax+j3=0.0;qEniaxsq-13=sigdiv*a2*sigdiv;

/* INITIALIZATION FOR HROW *hrowCl3=0.0; hrowt33=0.0;

/* INITIALIZATION FOR PDUM *for (iiin;ii<nmax;ii++)

jj=ii*nmax;pdum~jj+13-0. 0;pduunCaj+320. 0;

pdumC63=1.0; pdum[1BJ=1.0;

/* INITIALIZING RR *Efor (ii=O;ii<4;ii++) rrtii3-sqrt(rrii J);

150

DAT INPUT.

restarts.

pr*t DATA INUTI

munuu("\ OPTON OPIOl)EN;printf ("\n");printf("\n 1 ALMN TRAJLTERY");printf ("WI");

printf("\n OPTINS AMN");printf ("\n");printf("\n 1) RUNIF TRAECTR")printf ("\n");printf("\n 2) RUNT) KALMN")printf ("\n);

pri ntf (\n ) MDFAAEES)printf ("\n aaeerwscage;. oumy)printf(Iln v 4) uIT"); TRYbfrecnine"*

printf ("\n"); vle(14 a1)

if (oparam!)

fprintf ("\n" %"nrryrn)fprintf(fpi,\nA a~er wahne %e oumy)

pritf(haE0to rn TRECTOR bu or coninu.")

printf(n n vale (1-4) a E39kk1E3)

asonf ("option-);g rn

elif (option-- gooqut

elsenfopoenu;r~npw)

fprintf~!r le-n"% Xdnrry un)

modi f y:print.f("\n MODIFY PARAMETERS OPTIONS a"l);

* printf("1\n"1);print-f("\n 1) ZDIFAY stop optimization");printf C"criteria");

*printf("\n 2) SINGLE /MULTIPLE ARRAY");printf("\n 3) INITIAL X, Y, and Z positions");printf("\n 4) INITIAL Vx and Vy velocities");printf("\n 5) NOISE COVARIANCE - diag_slum of");printf C" R");printf("\n 6) MAX. # ITERATIONS OF OPTIMAL);printf C" MANEUVER");printf("\n 7) REDEFINE MANEUVER - w /g /time);printf(I" interval");printf("\n 8) USE LOW / HIGH RANGE")printf ("\n"1);printf("\n 9) MAIN MENU - OPTIONS");printf ("\n"1);printfC"\nEnter value (1-9) t 1)scanf ("%d" ,&param);

swi tch (par am)

case Isprintf("\n %s '/e",g"Current CRITERIA isa",

value);printf("\nchange ? EY/N3 :"1);scanf ("%s", &c); Iif (c-' H1 c=='N') break;printf ("\nEnter new value (real)a")scanf ("%lf"1,&value);break;

case 2:printf("\nSINGLE array is the default");if (narray==O)

printf("1\n%s"1,"Current case is : SINGLE");else printf("\n%s","Current case is: MULTIPLE");printf("\nchange ? CY/NJ ")scan-F ("%s",&c);if (c-n' 1H c=='N) break;printf ("\nEnter S for SINGLE or"1);printf(" M for MULTIPLE array a "1);scan-f ("1%s" ,&c);if (c-s' H1 c-=S') narray=O;else if (c=='m' H1 c== 'M') narrayl1;else narray=O;break;

152

case 31printf('\nCurrent values are : ")IprintfC"\n~s Ue %s %a %s %e","lxpos.- ",xkkmICOJ,

1ypou - "qxkkanlC23qI" zpos ",xkkmlE43)1printf('\nchange ? CY/N3 :1");scanf ('Xs",&c);if Cc--nn 11 c--mN') break;printf("\nEnter now values i )printf(II\n~sII,Ixpos Noe -

BcanF ("%If Ux0);

printf c'\n%sII, Iypos - 1);1scanf (I%lf"I,&yO);

printfCI\n~s"I,Izpos - 1);1

scanf ("%lf ",I&zO);

xkkmlCO03nxOg xkkmlC23=yO; xkkmlE4inzO;

break;

case 41print4fC"\nCurrent values arm 1 a)printf(II\n~s %a %s %ell,"xvel =",xkkmltlJ,

yvel - ",xkkml3J;printfC"\nchangs IN CY/N3 ill);

scanf("%sI',&c);if Ccinm'n' 11 c-nN') break;printf("\nEnter new values : 1);printf("I\n%s"," lxvel - 1);scanf ("%If ',&vxO);

printf (I\n~s"I, Iyvel -. "1)

scanf ("%If",&vyQ);

xkkml[13-vxO; xkkml[E33-vyO;

break;

case 5:printf("\nCurrent values are ")printf("\n~s %e %s %e %.5 %e %s %al","rEO]

rEO3," r[13 - ",rE13," r123 ",rE23,rE33 - I, r[33);

printf("\nchange IN CY/N3 :a"); '

scanf ("Xe" ,&c);if (cin='n' 11 c--'N') break;printf("\nEnter new values a ");printf CI\n~s"I,"rEOJ = ");

scanf (IIlf"I,&rEO3);

15~3

41iA

printfC'\n~s"j"rE13 - "1);scanF ("%lf ",&rCl3) p

printf C"\n~s","1rr23 -")scanf ("Xlf" I&rC23)1

printfC"\n~s11,"rC33 - 1);scanf ("%lf",&rC:33);

for (ii-Olii<4;ii++) rrE113-sqrt(rtii3)1break;

case 6:printf("\nCurrent value is z :)printf("\n%s %d","# of iterations - ",maxopt);printf("\nchange ? CY/N3 sn");scanf Q'%s11,&c);if (c--ri' 11 c-'N') break;printf("\nEnter now value - "1)

printf("\n~s","1maxopt # -1)scanf ("Zd" ,&maxopt);break;

case 7:trmenu: printf("\n DEFINE MANEUVER \n");

printf("\n 1) Define maneuver based");printf("oan turn rate");printf("\n 2) Define maneuver based");printf C" on q-number");printf("\n 3) Redefine start and end"); -print-FC" time slots"); .-

printfC"\n 4) Modify menu");printf("\n\nOptionzscarifC "%d" ,&triopt);if (trjopt4) break;if (trjopt-l)

printf("\nCurrent value is i

printf("%s %e"l,"turn rate :w =1w)

worgl1;printf("\n Enter new turn rate (dg/s):");scanf ("%lf"1,&wd);

else if (trjopt==2)

printf("\nCurrent value is ")

printf("%s %e"l,"g-number :g 11= )worqinO;printf("\n Enter new g-number ")

scarif C"%lf"1,&g);

154

7 r

. . . . . . . . . . . .

also if Ctriopt--3)

printfC"\nCurrent values are 1 "1);print#fC"\n~s %d %s %d","tstart -printf ("Jstart,"tend - ",iend);rprintf("\n Enter start time slot ")scanf (lld"l,&Jstart);printfC"\n Enter and time slot : "1);scanf C"%d",&jwnd) p

gate trmenul

case sprintf("\nLOW range is the default");printfC" (use single array)");if (nrangeO)

printf("1\n~s"l,lCurrwnt case is i LOW");else printfCl\n%sllq"Cu-rent case is a HIGH");printfC"\nchangs ? CY/N3 ill);scanf C"Xsl",&c)jif (c-'n 11 c--mN') break;printf("\nEnter L f or LOW or");printf("l H for HIGH range 1 "1);scanf ('Xe" ,&c);if (c-1'l 111 c--'L) nrange-O;else if (c--'h' c--*H*) nrange-1;else nrange-O;break;

case 9.gate menu;break;

default ubreak;

gato modify;

run: /* START EXECUTION *printf ("\n"l);printf("\nEnter run # : ");scanf ("%d",&runn);

f param-f open ("1param. dat"11, "1w");

1* fprintf (fpxyz ,l"X2d" ,runn);fprintf (fpe, "%2d" ,runn);fprintf(-Fpp, "%2d" ,runn);fprintf (fparam, "%2d" ,runn);

if(narraymmO)c-Fprintf C-Fpxyz,Xuh".a Single");fprint-F fpe, "Zn','I Single");-Fprint-F(fppj "~sI","I Single");fprint-f(fparam,"%s","t Single");

also

fprintf (fpxyz,"Xu"I,'I Mult");fprintf Cfpe, SI%sII.II Mult");fprintf Cfpp,"Xu",".I Mult");fprlntf (fparan, I"", Mult");

fprintf(fparam,"\n %s %e",g"CRITERIA :",value);fprintf(fparam,"\n~s %e %s %e %s %e","xkkm1COJ3

xkkmlCO31" xkkmlC13 - ",xkkmltlJ,Ifxkkmlr23 - ",xkkm1E23);

fprintf'(fparam,"\n~s Ue %sn e","xkicmlE33 - ",xkkm13,xkkml[43 - "I,xkkm1C43);a

fprintf(fparam,"\n%s %e %s %a %s %e %s %e","rCO3 - el,rC:3,11 r[E13 = "~r-C13,"1 r122 = 1rC23,"1 r133 - "1,rM3); -4

fprintf(-Fparam,"\n~s %e %s %e %s %e","pkkmlEOJ= "

pkkmlro39" pkkmlrl3 - ",pkknil~lJ,'pkkmlC2i - ",pkkml[23);

f cIoue (f param);

f pininfopen ("trj. inp"," IW");

fprintf(fpin,"%d %d",narray,runn);fprintf(fpin,"\n%e %e %e",xkkml[OJ,xkkml[2I,

xkkmlr43) ;/*positions*/fprintf(fpin,'\n%e %e"I,xkkm111],xkkm1C33); /*vel*/fclose(fpin);if (option4) goto quit;

fptrj-fopen ("trj. dat"I,I"r");fpzic-FopenCIlzic.dat"l,"r");

wait: printf("\nPress g and ENTER to start ... 1)scanf ("/.n" ,&c);if (c',g, && c!'') goto wait;

if (nrangeO)

xmininxminl; xmaxinxmaxl;ymininyminl; ymax-ymaxl; 1

158*

else

xmax-xmaxh; xmin-xminh;ymax-ymaxhl ymininyminh;

hei ghtinymax-ymi n;width-xmax-xmin;xscals-600. 0/width;yscal .i-i 60.0/height;by-196. 0-ymin*yscale40. 5;bx-600. 0-xmax*xscale+0. 5;

biosini (6);frame (36,0, 196,600);bioset (23,1);printf C"%. lf%s%. If" ,xmin, ",",ymin);biosmet (6,61);printf(" A.lf%%.1-F",xmax," ," ,ymax);

biosset(22,65);printf("Yu %d","# ",runn);biosset (23,65);if (narraym0O) printf ("SINGLE');elso printf ("MULTIPLE");

printf ("\nwait ")I

KALMAN FILTER

/* START THE TIME SLOT LOOP AND SET ARRAY HANDOFF *

/* POINT *for (kk-0; kk<itimu; kk++)

dk-kk~l;

do

dostime (&ihours,&imins,&isecs,&iths);hours=ihours; mins-imins; secsinisocs; ths-iths;time=3600. *haurs+60. *mjns4.secu+0. 01*ths;del tat-ti mu-aldti me;

while (deltat < 1.25 && kk>O);

oldtimu-time;if (deltat>1.31) printf ("1*");else printf("I.");

157

/1printfC'"\nX. %al.'It - ",oldtime)1*/1* printf ("%s %d"," kk - "kk) *

/41 printfC"X%2Os %d \la*.**44. TIME -",kk)v-1

/41 GET HYDROPHONE ARRAY COORDINATES /

if (narray !- 0)

if (xkkmlCO3 > swO) 18-0;aso if CxkkmIC03 > swl) 19-12;ese if (xkkmlCO3 > sw2) iSin24;aso if (xkkmlCO3 > sw3) 18-36;miso if (xkkml[OJ > sw4) jin4e,ese iSin6O;for (iin0;i<4;i++)

j3:*ie;xb~i 3=hydrocj3;yb~i3-hydrolj+1 3;zb~i3Jihydro~j+23;

/41 SET THE TRUE TIMES AND THE TRUE POSITIONS /

f scanf (f ptr jjZlf %I f %Ilf"&dataco:j,&dataC 13 ,&dataC23);

fscanf(fpzic,"%lf %If %lf %If",&zicro3,&zic~l3,&zic[23,&zicC33);

truedEoj3-dataEO3; truedrlj3ndata[1:3;truedE23-idataC231

1* FIRST SET HROW-CALCULATE GAIN, ESTIMATE /

/41 COVARIANCE OF ERROR BASED ON ONE TIME *

/41 MEASUREMENT-TC,TX,TY,TZ *adapt: for (i0;li<js;i++)

xinxklkmICOJ-xb~i 3;y xkkml C23-ybCi3:;z-xkkmlC43-zbli 3;

dsnomisqrt (x*x-,-"*y+z*z);aux hinvsim/dencoa,hrowt[linx*auxh;hrowC23-y1auxh;hrowC43-z*auxh;

for (iiinO;ii<nmax;ii++)

i in-i i*nmax;

159

gnumC11 3-pkkmICiin3*hrowC03+pkkmI~iin+23*hrowC23+pkkmlCiin+43*hrowC431

gdwnom-hrowC 03*qnumnC03+hrowC23*gnumC23+hrowC43*gnumC43+rCi 3;

/*TISI THE FIRST GANCOLUMN *

printvac (hrow,5);

piintf("\nDENOMINATOR is 1 11);print-F(% C'Z.wnm)printf("\nYECTOR 61 1 \n");pri ntvuc (g 1,5);*

/* CALCULATE THE COVARIANCE OF ERROR PI /for (ii-O;ii<nmax;ii+-2)

pdum~ii3-(-giCOJ*hrowCii3);pdumtii+53-C-giEl3*hrow~ii 3);pdujn~ii+103m(-giC23*hrowCii3);pdumrii+153-(-gi E33*hrow~il 3);pdum~ii+203-(-gi C43*hrow~ii 3);

pdumC03+-I.O;pdumC 123+-i .0;pdumE24J+nl.0;piEOJ ipdumtoj*pkkmlCQJ+pdumE23*pkkml~lo3+

pdumE43*pkkml r203;pi[13 -pdumC03*pkkml~l3+pdumC23*pkkmlE'113+

pdumC43*pkcmlE21 3;piC2J -pdumEO3*pkkml[23+pdumC23*pkkml[123+

pdumC43*pkkml E223;pir33 =pdumC03*pkkmlr33+pdumC23*pkklCI33+

pdumE43*pkkml E233;pi[4J -pdumC03*pkkml[43+pdum[23*pkkmIC143+

pdum[43*pkkml E243;p C 53=pi1 13;pi C&3 =pdumCS3*pkkm1 C 1 +pkkml r63+

pdumE73*pkkml Cli3+pdumC93*pkkmlE21 3;piC73 -pdumC53*pkckmIC23+pkkmIC73+

pdumC73*pkkm C 123+pdumC93*pkkml C223;piE9J -pdumE53*pkkmlL33+pkkml[83+

pdumE73*pkkmlEl33+pdumE9J*pkkmlC233;pi[93 ipdumC53*pkkml[43+pkkml[93+

pi lOpipdumC73*pkkm C 143+pdumC93*pkkml C243;

159

pitll3-pi[73;

pit l22inpdumt 103*pkckml C23+pdumt 123*pkkml C12].pdumC 143*pkkmIC£223;

pitl33-pdumClOJ41pkkmlC33+pdumCl23*pkkmIC3+pdumC 143*pkkml[£233;

pitl43-pdumCI03*pkkmIC43+pdum£123*pkkml£143.

0 pdumE 143*pkkml C2431pit 153piC3;pit 163pi £9;pitl73-piC1331

picie3-pdumCl5J4.pkkmlC33+pdumEI7J41pkkmIl133.

piC1(3piC]<0)?-pt] it]pkC33piCJ ) pil)up;lJpic5Cpi24 0)-pduE23*kkl4 : pi22*kml;

pll(pk >pk3) 7 pkl pkk23;

1* pg~zat-sqrt p) em;4/

ziff Cx -i= 0.0) at

pl(if (3 < 10) -pi03iC3

rabs(p tump < 0) ? -t11 mp : ptamp;

p hil (pa11 > 0k5 .0);l p

pgatm-sqrt1vu1 m; ;*ga=3 *gau

i -F (x !- 1.0

if ( < 10) X1.0

% .* .* . .. t... -.. * - .t -. - * -... . - .d o . . .

I.printfC"\nMATRIX PDUM u \n')l loprintmat (pdum.5,5)lprintfC"\nMATRIX PI : n)printmat (p1,5,5);printf("\nZDIFF is 1 1);print#("\%* \n",zdiffEi3);printf("\nGATE is 1 "1);print+("\%& \n11,gatw); *

/* EDIT INVALID TIME MEASUREMENTS *zgatw-(zdif-F113 < 0) ? -zdiffCi3 g zdif-FCi3;if Czgate >- gate)

for (ii-0;ii<nmax;ii+-) gi~ii3=0.0; ;'-

zdiff Ci3-i999*0;/* TAG FOR INVALID TIME MEASUREMENT *

1* CALCULATE ESTIMATE BASED ON ONE MEASUREMENT*//* PREDICTION */ .- ::1for (iiinO;ii<nmax;ii++)

xiCiijinxkkmItii3+gi~ii3*zdiffli3;

1* printf'C'\nVECTOR XI u\n");printvec(xi,5); *

for (iiinO;ii<nmax;ii++)

xkkmnlli13-in~ ii)lpklcmllii3-pi Cii3;

for (Jj-nmax;jj<maxsq;jj-+) pkkmltjj3=pi~ij3;if (1 - 3) goto 156;

/ * end JS loop *

1* NOTE s CALLED ORIGINAL X(O/-1),XKKM1. UPDATED */* AFTER 1 MEASUREMENT CALLED IT XI, THEN MADE */* XKKMI AND WENT THRU ITERATION AGAIN. AFTER *1* YOU HAVE UPDATED XKKM1 FOR EACH MEASUREMENT */* XKK=XI AND PKK-PI. *

156; for (iiin0;ii<nmax;ii-4)

xkkli i3-xi Ciii;pkk~ii3-pi Cii3;

for Cii-nmax;ii<maxsq;ii++)

pkk~jj3-pi Ci);

161

1*printfQ'\nUPDATING VECTORS INTERVAL ... n)printfC"\nVECTOR XKKM1 s \n");printvuc (xkkml ,5) 3printf("\nMATRIX PKKM1 i naprintmat (pckal ,5,5);

printfV'1\nVECTOR XKK s \n");printyac(xkk,5);printfC"\nMATRIX PKK t \n");printmat(pkk,5,5); *

/* RECALCULATE TIME MEASUREMENTS, and FORM *1* ABSOLUTE VALUE OF RESIDUALS. *for (i0;i<4,i+)

/* EDIT INVALID IiMES FOR ADAPTIVE MANEUVER */* ROUTINE *

if (zdiffEi3 < 999.0)

x-xkkml[03-xb~i 3;yinxkcml r23-ybCi I;zixkkmlt43-zbli 3;

/* denom=.qrt(x*x+y*y+z*z); *dunom-x*x+y*y+z*z;x-idenomli f (x !- 0.0)

i f (x < 1.0) x-1l.0;do

oldxinx;x-( x + denom/x )*0.5;tumpol dx-x;rabsin(tump < 0) ? -temp :temp;

while Crabs > 1e1)

denomnx;zhat-vel m*denom;temp-zicti J-zhat;zdiffrijin(temp < 0) ? -temp :temp;

eise

zdiffli3=0.0;nzdiff-1l;

162

/* THIS FUNCTION COMPUTES THE ADAPTIVE Q MATRIX *if (icurvu -- 1)

bb-xkkmlE33*xkkml E3;dd-xckkmlE13*xkkmlCl3;al-bb+ddibdinxkkml r 1 *xkkml[3;atldd*iac:/al+bb*si qcc;*l2inbd*sigacc/al-bd*si qcc;e2=bb*si qacc/al+dd*si qcc;qCO3-g2*el;qC 1 Jg:3*ul;qr23=g2*e12;q133-g3*e12;qE63-a2*ol;

qEB3ma2*el2;qC123-ng2*e2;q£ 13)ing3*e2;qCl83-a2*e2;

bb-ngammaCO:3*gammaEO3;dd-gammar 1 *gamma[13 ;qCO3=bb*covwCO3;qr I -gammaEOI*covwC03*gammaE 1);qC2JO0.01 q133-0.0;qE6Jindd*covwEO3;qC73-O.0; qCS3inQ.0;qr 123-bb*covwC 1];qE l33JammaCOJ*covwE 1 *gammaE 1);qE lSBidd*covw 1 J;

q£53=qCU; q£1-0:Jq[23; qE 113JqC73;q[153=qC3i; qE163-qES); q[173)qE133;

1* printf("\nMATRIX 0 :\n");printmat(q,5,5); *

/* IF ALL TIME MEASUREMENTS EXCEED SATE, BYPASS */* ADAPTIVE MANEUVER ROUTINE. *

if (nzdiff !- 0.0)

zdifav-(zdiffE03+zdiff[13+zdiff[23+zdi-FF[33)/nzdif+;1* printf("\nZDIFAV is : 11);

printf("\%u \n",zdifav); *

163

/* DO NOT PERFORM ADAPTIVE MANEUVER IF FILTER 4

/* HAS NOT ACHIEVED THE STEADY STATE. 4

i f (kk > 3)

1* dostime(&ihours,&imins,&isucs,&iths);hours-ihours; mins-imins; sacsisimcs;thu-i thu ptimu-3600.*hours+60. *minu4.uucs+O. 01*ths;printfC"%s %all," timel = ",time); 4

/* IF ZDIFAV MEETS CRITERIA TRANSFER OUT 4

/4 OF ADAPTIVE MANEUVER ROUTINE. 4

if (zdifav > value && icont < maxopt)

/* INCREASE GAIN 4

f or (j -0; j< max sq; j++)pkkmlcj3inpkktj3+q~j3;

/4 printf("\nINCREASE GAIN AND PERFORM");printf("1 ADAPTIVE ... \ 1printf("\nMATRIX PKKM1 : \n");printmat(pklcml,5,5); 4

/* PERFORM ADAPTIVE MANEUVER BY 4

/4 REITERATING SAME TIME SLOT. * -:

icont+-In;gotc adapt;

i cont=0;nzdiffin4;xdif E3-xkk[03-tr-uedCO3;xdiff El JxkkC23-truedt iJ;xdiff E23=xkkE43-truedC23;

/4 temp=xdiffC13*xdiff~l3/pkkmlCl23;if (temp < limvar) icurve=0;else icurve=1; 4

fprintf(fpe,"\n %d %e %e %e"l,kk,xdiffE03,xdiff[13,xdif23);

/4 WRITE 4

/4printf("\nVECTOR ERROR :XDIFF YDIFF ZDIFF\n");printvec (xdiff ,3); 4

/4 PREDICTIONS FOR PKKM1 4

for (i-O;i<nmax;i+-)

164

phipkk~i I-pkkti J+pkck~i+53*phisCO31phipkkri+53-pkkri+53;phipkk~i+103-pkkCi+1OJ+pkk~i+153*phisC23;phipkk~i+l53-pkkti+153;phipkkc~i+2O3-pkkCi+2OJ;

for (i-O;1<nmaxli++)

in-i*nmax;pktmmp~in3-phipkk~in3+phipkk~in+J4IphisEO3;pktamp~in1-1Jphipkk~in+1:3;pltmpCin+23-phipkk~in+23+phipkk~in+33*phi sr23;pktampEin+33-phipkkEin+33;pktampcin+43-phipkktin+43;

for CjiO;j<maxsq;j++) pkkmlCj3npktempCj3+q~j3;

1*printf('\nPREDICTIONS FOR PKKM1 ... \n)printf("\nPIATRIX PKKM1 s \n");printmat (pkkml ,5,5); *

/* CALCULATE NEW XKKM1 *for (iiiO;ii<nmax;ii++)

cxkkml113-xkktii3;1* pl-sqrt(fabs(pkkliJJ)); *

xkkmlC03-xkkmIC03+phisCO3*xkk~l3;xkkml E23xkkm1E23+phisEC2*xkkC33;

1*printf("\nNEW XKKM1 ... (for next time");printf(I" interval)\n");print-F("\nVECTOR XKkM1 \nl)printvec(xkkml,5); *

fprintf(fpp,"\n %d %* %e %e",kk,pkkCO39pkkEl23,pkkE243);

fprintf(-Fpxyz,"\n %d %e %e %e"l,kk,xkkEO3,xkkE23,xkk[43);

/* PLOTTING *colmxkk[OJ*xucale+bx;row-xkkr23*ysca1 e+by;bioswd (row,col);

biosset (3,0);printf ("%s %.If',"x 'xkk[03);

165

biomuet (3,13);printFC("%a %.lf 11 "y i",xkkC23);

printf ("%s %.1f ", z -,xkkC43);biosut (3,42);printf Clu %.1f',"vx -',xkkCl3);biossut (3,54);printf("%s %.lf","lvy "xkkC33);b iosset (3,965) ;print-F("%s %d"g"time in"gkk); *

3 /* and TIME loop *

4close(fpxyz);f closeu(f pe);fclouu(fpp);fclose(fpzic);fclose(fptrj);cikeyrdo;biosini (3);

quit:printf("\nquit ? EY/N3 ill);scanf ("%s",&c);i - (c--'n' IIc-='N') goto restart;

/ * and MAIN PROGRAM *

1* PRINTVEC will print the vector elements given1* the name and the dimension. *

1* PRINTMAT will print the matrix elements in the *1* usual matrix form, given the dimensions. *

printvwc (vuctorldim)int dim;double vectorC3;

mnt il j;for CJ=O;j<dim;j++) printf ("%e %1s",vector~jJ," "1);printf ("\n");

printnmat (matrix ,row,col)mnt row,col;double matrixC3;

int i,j;for (i=O;i<row;i+4)

for (jiO;j<col;j++) printf ("%e

printf ("\n");

167

1* FRAME *1* Draw a frame on the screen. */* *** NOTE ***1* The upper left corner of the screen is 0.0 and the */* bottom right corner is 199,639. *

int frame(ulrow,ulcol.,lrrowvlrcol)int ulrowgulcol,lrrow,lrcol;

int count,irowligiauxgicol,tcount;

/* do top line first *for (icol - ulcol; icol <= rcol; icol++)bioswd(ulrowlicol);

/* do both sides at once *for (irow - ulrow+1; irow < Irrow; irow++)

bisdio(lo)bioswd (irowlcol);

/* do bottom line *for (icol = ulcol; icol < lrcol; ic-ol++)b ioswd (Irrow, i col)

/* do vertical center line with scale *i=0.5*(ulcol+lrcol )+0.5;for (irow -ulrow+1; irow < lrrow; irow++) bioswd(irow,i);for (irow - ulrow+16; irow < lrrow; irow+-16)

f or (tcount-i-4; tcount <= i+4; tcount++)bioswd (irow,tcount);

/* do horizontal center line with scale *i =0.5* (ul row+l rrow) +0.5;for (icol = ulcol+1; icol < lr-col; icol++) bioswd(i,icol);for (icol - ulcol+60; icol < lrcol; icol+in80)

f or (tcounti-3; tcount <= i+3; tcount+')bioswd (tcount,icol);

/*-------------------------------------------------------------*

raturn(0); /* done *

168

1* TRAJECTORY SIMULATION ROUTINE -films TR.C *

*include ustdioah'#include Ssmathhs*include 'kalman.h"trajeccnarray,hydrogxbgybgzbgxkkmlljstart,jendlwdgg,worg)

double hydroEJ ,xbCJ ,ybEI ,zbC3,xkkmlEJ ,wd,g;i nt narray, wor-gjstartljend;Cint pligilstart~and;double pplqr1,sqrt0,zcs,zic143qzi143;double xdqydqzdqvqdenqv11lm,gg;static double xn[63-C-3.01, -2.0, -1.5, 1.0, -0.6,

0.0);static double ynE63-C0.0, 0.02289 0.0689 0.13579

0.27439 0.5);static double snCSJ-(43.8596, 11.36369 7.25689,

2.891352, 2.65887);

double dataC53,wr;int runnjiS;FILE *fptrj, *fopeno;FILE *fpzicq *fopeno;int jjjj;

1* THIS ROUTINE COMPUTES TRUE TRAJECTORY OF TORPEDOdataEojintrue x position, data[13= true y position -

dataC23-true z position, w-turn ratedata3mx-velocity, dataC43=yvelocityjstart-time slot where maneuver startsjend-time slot where maneuver ends *

dataC03-xkkmlE0J; dataE13-xkkmIC23; data[23-xkkml[43;dataE3)inxkkml~l3; dataC43=xkkmlE33; velm-1.0/vel;rl=0.49;wrtwopi*wd/360. 0; gg-32. 2*g; start=jstart; end-jend;

printf("\n %s \n',"start execution of trj.c");fptrj=-fopen ("trj.dat","w');f pz i cf open ("z ic. dat", "w"); -

for (jj-O;jj<npt;jj++)

1* GET HYDROPHONE ARRAY COORDINATES *if (narray '=0)

1697

if (dataC03 > SWO) 19-0;a1so If (dataC03 > swi) i1912;aiso If (dataC03 > sw2) 1in24;also if (dataC03 > sw3) 19-36;miso 1# (dataCO3 > sw4) i9-49;also 15in601for (i-0;i<41ie+)

xbC13-nhydrocj3; 'ybC13J-hydroEJ+131zbl13-hydrolj+23;

/* COMPUTE THE TRUE TIMES AND THE TRUE POSITIONSWRITE TRUE TIME and TRUE POSITIONS *

printf ("Xe"l, ". );f pintf(-fptrj , 1\n %@ %s, %ally

dataE0J ,datatlJ ,dataC23);

for (i0;li<4pi++)

xd-data[03-xbC 1];yd-ata~l3-yb~i 3;zd-d~taC23-zb~i 2;don-uqrt (xd*xd+yd*yd+zd*zd);zi Ei 3 val m*den;

1*printf("\nTRUE TIMES z \n");printvac (zi ,4);printf("\nTRUE POSITIONS a\n;printvec (data,3); *

/* GET NOISE,SCALE AND ADD TO TRUE */* MEASUREMENT TIME *pp 1 =317. 0*rl1plippl;rl-ppl-pli;v-r ;j -0;if (v > 0.5) v=1.O-r1;

r2s if (v < YnCJ+13) goto r9;

goto r2;re: vi(v-ynCj3)*sncj3+xn~j];

i f (r I >- 0. 5) v-(-v);zcs-(v/3. 00) *0.00001;

1* printf("\nRANDOM NUMBER :a");printf('Xu \n",zcs) ;*/

170

zicCi3inzcs+zi~i3;

fprintf~fpzic"\l Ue %a %a %e'zicEO3,zicC13qzicC23,ziC3)

1* NEW DATA *if (start >- npt) linw(iiOjnptjdata)jaiso

if (end > npt) andinnpt$line~jjfO,start~data);circleoii ,start~endldatagwrlgggwOrg);line~jj,end,npt~data)j

f clcue (fP ri);fclose(fpzic);

circle(j ,startgendgdatagwwlggqworg)double dataC3,wwg;int J,startgandgworg;

double sinogcoso,atanopdouble r-,vt,aux ,a,b,teta,tetaO,mlalfawaggagvw;gingg; WMww;if(J >- start && i < end)

if (dataE33-in.O && dataC43inO.O) goto out;vt-sqrt(dataE33*dataE33+dataC43*dataL43) 4

if (worgl-) g=w*vt;alse wing/vt;r-vt/w;if (dataE33)mO.O)

b-datar LI;if (dataE43>O.O)

a-dataEO3-r;tetaO-O. 0;

else

a-dataC03+r;et&OO. 5*twopi;

else if (dataC43-m0.0)

171

'p.--- , %*4 *4** 'd.

F V w -7 17-r--T-!P4 -M WT hI-I -R

a-ndataC031if CdataC33>0.0)

bindataC13 2r;totaOin-0. 25*twopi;

bindataC 1 -r;tnt&0-0. 254ltwopi;

m-dataE43/dataC331auxinr/sqrt Cl * +m4m);if (dataC33 > 0.0) aux--aux;bindatat 1 -aux;a-dataE03+m*(dataCl13-b); :ot~taO-atan2 (-dataE3J ,dataE43);

tuta-1 .31*w~tetao;data[03mr*cos(tta)4a;dataE13Jw*sin (twta)+b;data[3i-vt*sin (tata);dataC43-vt*cos (teta);if(indi

wa-w*360. 0/twopi;ga-9/32. 2;printfC"\n ... from TRAJEC u ");printf ('\n");printfQ'\n~s %e %s"911 turn rate " ,wag

printf('\n~s %.2f\n","O g-number a ',qa);

out:

line~i ,start,end,data)double dataC3;int istart,end;

if((j >- start) && Ui < end))

dat&E03+-1.31*dataC33;data~l3+-1.31*data[43;

172

. RI W. WT - .

APPENDIX F

LISTIN8 OF CIEKR1.C - CIE VERSION

I* *I1"!

/* HEADER FILE - KALMAN.H *//* CIE MICROCOMPUTER VERSION */

*define nmax 5 /* # of col. in pkk, q ... a"#define maxsq 25 I* size of equiv. vector PKKQ...a/#define is 4 I* # hydroph. in any hydrophone a-

I* array a.#define hcmax 12 /* # of col. in matrix HYDRO of */

/* hydroph. coord. */#define hmax 72 I* equiv. vector size of HYDRO *I#def ine dmax 3#define itime 150 /* max.# oF time slots *I#define npt 150#define icmax 5 1* max.# of optimal iterations a,

/* allowed *-#define criter 0.1 /* criteria to compare ZDIFAV a.#define na 0 /* may assume ONLY values 0 or 6 *I

/* 0 =>single;6-> multiple arrays *.#define xpos 10000.0 1* initial torpedo x-coordinate a-#define xvel -50.0 /* initial torpedo x-velocity *I#define ypos 0.0 1* initial torpedo y-position */#define yvel 5.0 /* initial torpedo y-velocity */#define zpos 0.0 I* initial torpedo z-position */#define swO 33000.0 /* swi define points where */ ",,.#define swl 27000.0 I* switching from one to a,#define sw2 21000.0 /* another */#define sw3 15000.0 array aI#def in sw4 9000.0 /* is done. a,#define sigdiv 1.0#define scc 0.387463094 /* - in radians... a/ ..-

#define sacc 36.2#define vel 4860.0 I* speed of sound *I#define twopi 6.28315308#define rrvar i.e-5#define rvar 1.e-10#define pkkdiag 1000.0#define limvar 1.5 /* defines threshold to use Q*/

/* constant a,#define gmO 0.86#define gml 1.31#define cwO 1.0 /* initial was 1036.94 *I#define cwl 1.0 I* initial was 1036.84 *I#define cw2 1.0

173

r.

*I* *l -- 4. . 4. .. ... P t 4.

1* KALMAN FILTER PROGRAM FOR THE CIE MICRO *I. (FILE CIEKR1.C) *

*include "stdio.h"#include 'math.h'*include 'tcalanan.h'doublexkkml Enmax I pkkml Emaxsql ,pkklmaxsq] ,xkkEnmax3I,dataldmax3;

/* STARTING THE MAIN PROGRAM... *main()

FILE *fpxyz, *fopeno;FILE *f pm, *fopeno;FILE *4pp, 4fopeno;FILE *fptrj, *fopeno);FILE *4pzic, *fopeno;FILE *fparam, *fopeno; 4

FILE *4pin, *4openo; Vint ihoursgimins~isecs~ithsgcol ,row;int iG,i ,igkk,iin,icont,icurveloption,param;int trioptlnrange;int ii,ij,narray,nzdiff,ingpligrunngc;int jstart,iundgworg;float rabs,zgate;double heightgwidthgymax,ymin,xmax,xmin;double xscale,yscale,bx,by;double hours,mins,secs,ths,timegoldtime,.deltat;double sqrto,oldx,value;double denom,a14, qdunom,gate,zdifav;double den,vulm,temp;double a2,gl,g2,g3,p1,xdif4E33;double zhat,zcsgdk;double v,truedC339wd,g;double x ,y,z ,auxh,xd,ydlzd,sigcc~sigacc;double al~el,el2,e2,bb,dd,bd;double pkl,pk3,pk5,pgate,rgate,p,pll;double phipkklmaxsq3,pktemp~maxsq3,qlmaxsq3;double piEmaxsq3,pdumtmaxsq3,qtempEmaxsq3;double gnum~nmaxlvgi~nmax3,xi~nmax3,hrowtnmax3;double zictjs3,zitjs3,zdifftjs3,r~js] ,phisE is] ,zt~is3;double xbcis3,ybrjs3,zbcjs3,rrtis3;double hydro~hmax3,covw[33,gammat23,optE53;

fpxyz-FopenV'xkk.dat",'w');fpenf open ("xdif f.dat", "w");

fpp-fopen ('pkk.dat"w");

174

1* INITIALIZE CONSTANTS, HYDROPHONE MATRIX, PHI, R9,PDATA FOR TRAJECTORY, AND LOAD X(0/-1)v P(0/-1)

nzdif 4-4; icont-01 icurvu-1; valuuincriter; wd=5.730;runn-1; narray-na; paramO0; worginl; g-0.156;nrangs-0;istartin4O; jandin8O;

hydroEO3-36000.0; hydroC13- 6000.0; hydroE2in 0.0;hydroE3Jin36O30.0; hydroC43n 8000.0; hydroE53- 0.0;hydroE6Jin36000.O; hydroC73- 6030.0; hydroE8J- 0.0;hydroC93-36000.0; hydroC1lOJ6000.0; hydroC113= 30.0;

hydroC123-30000.0; hydroCl3Jin6000.0; hydroCl43- 0.0;hydroC153=30030.0; hydroC163-6000.O; hydroC173- 0.0;hyciroCI83=30000.0; hydroCl9Jin6O3O.O; hydroC203= 0.0;hydror213-30000.0; hydrot223=6000.0; hydroE23in 30.0;

hydroE243=24000.0; hydroC253-6000.0; hydroE263= 0.0;hydroE273-24030.0; hydroC283-6000.0; hydro[29in 0.0;hydroC3o)-24000.0; hydroC313=6030.0; hydroC32in 0.0;hydroE333-24000.0; hydroC343-6000.0; hydroE353- 30.0;

hydroE363-18000.0; hydroC373-6OOO.0; hydroC383- 0.0;hydroC393-19030.0; hydroC403-6000.0; hydroE413= 0.0;hydroC423-18000.0; hydror43Jin6O3O.0; hydro[44J= 0.0;hydro[45JinleOOO.0; hydroC463=6000.0; hydroE473- 30.0;

hydroC4e3inlZOOO.0; hydroE493=6000.0; hydroC501= 0.0;hydroC513-12030.0; hydroE523=6OOO.0; hydroE533= 0.0;hydroC543=12000.0; hydroC5J6030.0; hydroES8)= 0.0;hydroE573=12000.O; hydroC[a8in6000.0; hydroC593- 30.0;

hydroC603-6000.0; hydroC613-6000.0; hydroC623J 0.0;hydro[633=6030.0; hydroE643-6000.0; hydroC653- 0.0;hydroC663=6000.0; hydroC673=6O30.0; hydroC683= 0.0;hydro[693-6000.0; hydroC7O0in6000.0; hydroC713- 30.0;

r-COJ=rvar; rC13=rvar; rC23=rvar; r[33=rvar;

gamma[03-gm0; gamma[13=gml;covwC03-cwO; covwClj3=cwl; covwC23=cw2;

xkkm1E03-1000.0; xkkmlC13-(-50.0); xkkml[23JO.0;xkkunl[33J.0; xkkmIC43=0.0;

for (i=0;i<nmax;i++)for (j0O;j<nmax;j++) pkkmlli*nmax+j3=0.0;

for (i0;li<nmax;i++) pkkmlli*nmax+i3-pkkdiag; *

phis[0l=1.31; phis[13-o.0; phis[23=1.31; phis[33=0.0;%%

175

_' 7 -, -. - .

xbCO03(-15. 0); ybC03- (-15. 0); zbCO3J(-15. 0);xbC13- 15.0; ybC13m(-15.0); zbC13-C-15.0);xbC2-(-15.0); ybC2Jin 15.0; zbC23-(-15.0); .-

xbC33-(-15.0); ybC33-(-15.0); zbC33J 15.0;

/* GET DATA INPUT 4

/4 DATA INPUT *

menu: /4 OPTIONS MENU 4

printf ("WI );printf("\n KALMAN FILTER "1);

printf ("\n");

printf("\n OPTIONS :)printf ("\n");printf("\n 1) RUN TRAJECTORY");printf ("\n");printf("\n 2) RUN KALMAN");printf ("\n");print-F("\n 3) MODIFY PARAMETERS");printf (11\n");print-fC"1\n ' 4) QUIT");printf ("Wn");i f (param!'0)

printf ("\n");printf("\nA par-ameter was changed ...

printf(" You may have to run");printf(" TRAJECTORY before continue.");

printf("\nEnter value (1-4) : ");scanf ("%d" ,&option);if (optionl1)L

f pi n-fopen ("trj. inp","w");fprintf(fpin,"%d %d",narray,runn);fprintf(fpin,"\n%e %e %e"',

xkkmlEO3,xkkml1,xkkmIC23);fprintf(fpin,"\n%e %e",xkkmlE33,xkkmlE43);f close(f pin);

trajec(narray,hydro,xb,yb,zb,xkkml ,jstart,jend,wd,g,worg):paramO0;goto menu;

176

else if (option--2) goto run;also if (option-i3) goto modify;also if (option-in4) goto quit;aiso goto menu;

modifyiprintfC"\n MODIFY PARAMETERS OPTIONS -.");

printf ("\");print-F("\n 1) ZDIFAY stop optimization");printf ("criteria"l);printf("\n 2) SINGLE / MULTIPLE ARRAY");printf("\n 3) INITIAL X, Y, and Z positions");printf("\n 4) INITIAL Vx and Vy velocities");printf("\n 5) NOISE COVARIANCE -diag...lem of");printf ("R");print+("*\n 6) ERROR COVARIANCE -diag~jelem of");printf(" PKK");pri ntf (" \n"1);print-F("\n 7) MAIN MENU - OPTIONS");printf (11\n");print4("\nEnter value (1-7) : ");scanf ("Wd",&param);

swi tch (param)

csprintf("\n %s %e"g"Current CRITERIA is :",value);

printf("\nchange ? EY/N3 n)

scanf (1"%s" ,&c);if (a-in' c=-'N) break;printf("\nEnter new value (real) ")

scanf ("Xlf11,&val ur);break;

case 2:printf("\nSINGLE array is the default");if (narray==O)

printf("\n%s"','1Current case is : SINGLE");else

printf("\n~s","Current case is : MULTIPLE");printf("\nchange ? EY/N3 ")

scan-f ("%s",&c);if (c--in H c-=N*) break;printf ("\nEnter S for SINGLE or");printf(" M for MULTIPLE array - "1);

scanf ("7.s",&c);if (cm-s' H c-='S') narrayO0;else if (c=-'m' H1 c-=M') narray=1;el se narray-O;break;

177

* -:--.17.-.

case 3:printf("\nCurrent values are 11)printf("\n~s %a %s %e %s %e"l,"xpos- ".,xkkmIroj,

Iypos - I",xkkm1C2i,"I zpos = asxkkmlE43);printf('\nchange ? EY/N3 :11);scanf (IS%sI8&c)jif Cc-n It' c-N') break;printfC"\nEnter now values x :)printf ("\n~sII,Ixpos - 1)scanf CI%1fI,&xkkm1EO3);

printf("\n~sI,I"ypos=scan-F ("%If ",&xkkmlC23);

printf('I\n~s"I,1zpos - )scarifC("Xlf",&xkkm1E4I);

break;

case 4:printf("\nCurrent values are : ");printf('\n~s %e %s %e","xvel =",xkkml1t3,

Hyvel =o "I,xkkmlC33);printf('\nchange ? EY/N3 n)scanf (11s".,&c);if (c--in H1 c='N') break;printf("\nEnter new values : "); Fprintf("1\n~sI1,"xvel - 1)scanf ("Xlf".,&xkkm1C1J);

printf('1\n~s"I,1yvel =O

scanf ("%lf",&xkkm1t33);

break;

case 5:pri ntf C'\nCurrent values are 'I");printf("\n%s %e %s %e %s %e %s %ell,"IrtOJ=

r[03,11 rElJ - ",r[13," r123 1 ",r[23,rE33 = 11,rE33);

printf("\nchange ? tV/NJ ")scanf C"%sI1.,&c);if (c=n' H' c=-'N') break-,printf("\nEnter new values : ");printf("\n~s11,"Ir1O3 21=0scanf ("%If ",&rrO3);

print-f ("\n%s"1,"rCI)scanf ("% f ", &r El1);

printf("\n%sI1,"rt23 =1)

178

scan-F ("%f,&rC2)I

pr nt-f(" \ns" , "r133 -1)scanf ("%If",&r E33)1

break;1

came 63print-f("\nCurrent values arm z 11);printf("\n~s % %s %a %s e ","pkkmlEOJ

pkkmlCO3," pkkmlE63 - ,pkkmIC63,pkkmlrl23 - ",pkkm1El23);

printf("\n~s %a %s %e","pkkm1C19 = ",pkkm1E183,pkkmlC243 - ",pkkmlE243);

printf("\nchange ? CV/N3 s");scanf (%a' &c);if (c--n' H1 c--*N) break;printf("\nEnter now values : ")Ip rintf("\n~s","pkkmltOJ -1)

scanf ("%Xl",&pkkmlCO3)p;

printf("\n%s",'pkkmlt&J -1)

escanf ("/lf",&pkkmlC63);

printf ("\n%%" ,"pkkmnlEC123 -1)

scanf ("7.f",&pkkml[l23);

printf ("\n~s","pkkml[lBJ =1

scanf C"%lfllg&pkkml[lBJ);

printf("\n~s","pkkm11243scanf ("%lf",&pkkmlt243);

break;

case 7:goto menu;break;

default:break;

goto modify;

run: /* START EXECUTION *printf ("\n");printf("\nEnter run # :");scanf ("%d" ,&runn);

fparam-fopen ("param. dat"l, 'w");

179

f* printf (f pxyz ,"X2d",runn);fprintf Cfpe, "%2d" ,runn);fprintf (fpp,l"X2d" ,runn);fprint-F(fparam, "X2d" ,runn);

if (narray-inO)

fprintf Cfpxyz,"%s","8 Single");f printf (f pe, "%s", " Single') ;fprintf (fpp,"Xs." Single");fprintf (fparam,1l"Xe"," Single");

else

f printf (f pxyz , "%s"," Mult");f printf (f pe, "%so',"1 Mult");Ifprintf (fpp,1lXu","l Mult");fprintf (fparam,l"xll","l Mult");

fprintf(fparam,"\n %s %o","CRITERIA : ",value);fprintf(fparam,"\n~s U.c %s %e %s Xe","xkkrnlCO3 = '

xkkrnlEOJ," xkkml[13 - ",xkkml[13,,xkkmlE2J - ",xkkmIC23);

fprintf(fparam,"\n~e %e %s %e"9"xkkmlE33 ",xkkml3,ofxkkmInC43 - ",xkkmIC43);

fprintf(fparam,"\n~s %e %s %e %s U %s %e"l,"r[03 ",r[03,"1 r113 =lrC3rC23 = ',r[23,'1 rC33 - ",rC33);

fprintf(fparam,"\n~s Ue %s %e %s %e","pkkrnltO) "

pkkmlCO3," pkkmrnlt = ",pkkmlrl3,ofpkkmlE23 = ",pkkmlE23);

fclose(fparam);

f pin-fopen ("tri. inpo,"fwls);

ml. fprintf (fpin, "%d Xd" ,narraygrunn);fprintf(-fpin,"\n~e %e %e"l,

xkkrlco),xkkmlE23,xkkmIE43);/*positions*/fprint-f(fpin,"\n~e %e"l,

xkkrlEIJ,xkkmlE33);/*velocities*/fclose(fpin);if (option-4) goto quit;

f ptrji=4 open"tr j. dat",r"1) ;f pz i cf open ("1z ic. dat"1,"'r"1) ;

/* INITIALIZATION FOR OFIND *ve.1 -i0/vel;a14=phis[03;a2-a14*aI4;

ISO

gl-&2*0.5;

g3inal4*gl;si qacc-sacc*sacc;si gcc-scc4.cc;for (iiOi<nmax;i++)

for (iinOli<rmax;j++) qCi~nmax4.j3-O.O;qCmaxsq-1 J-aigdiv*a2*uigdiv;

/* INITIALIZATION FOR HROW 4hrowC13inO.O; hrowC3-0n.O;

/4 INITIALIZATION FOR PDUM 4for (iiiO;ii<nmax;ii++)

jj-ii~nmax;pdumCjJ+13=0n. 0;pdumCji+3n0. 0;

pdumC63-1.0; pdumClGJ=1.0;

/* INITIALIZING RR 4for (ii=0;ii<4;ii++) rrEii.J=sqrt(rCii3);

printf("\Pr-ess any key and ENTER to start ... 1)scan-F("%s" I&c);

/* INSERT TIME CHECKING HERE *

/* START THE TIME SLOT LOOP AND SET ARRAY HANDOFF 4/* POINT 4

for (kk=O;kk<itime;kk++)

/4 printf("\n~s %e","t =",oldtime);

printfQ"%s %d",' kk ",kk); 4

dk-kk+l;

/4 printf("%20s %d \n,*44*4*4TIME :",kk); 4

printf("\nwait )

printf (".H);

/4 GET HYDROPHONE ARRAY COORDINATES 4

if (narray !-0)

if (xkkml[03 > swO) iS=0;else if (xkkmlEOJ > swl) i8-12;lse. if (xkkmlrO3 > sw2) iS=24;sise if (xkkmICOJ > sw3) i8-36;else if (xkkmlE03 > sw4) iSin42;

lei

xbE13-ihydro~j3;ybE13J-hydro~j+131;zbEi32ihydro~j+23;

/* INSERT TIME CHCKING + WAITING LOOP HERE 4

guttpz /4 ETTH/* ET HETRUE TIMES AND THE TRUE POSITIONS

WRITE TRUE TIME and TRUE POSITIONS 4

f scanf (f ptrj, "%I f %I f %1f"&dataCOJ,&data13j,&dataE23);

fscan-F(fpzic,"%lf %If %If %If",&zicEO3,&zic~l2,&zic[23,&ezicE33);

truedEr3-dataEO3; trued[13=dataEll;truedC23-data[23;

/4 FIRST SET HROW-CALCULATE GAIN,ESTIMATE 4

/4 COVARIANCE OF ERROR BASED ON ONE TIME 4

/* MEASUREMENT-TCTX,TYTZ .*

adapt: for (iiO;i<js;i++)

xxkkml EOJ-xbCi 2;y-xkkmIE23-yb~i 2;zinxkkml E43-zbti 2;

dunom-sqrt (x*x+y~y+z*z);auxh=vel m/denam;hrow[0O-x~auxh;hrow[23=y~auxh;hrowC43-z~auxh;

for (ii-O;ii<nmax;ii++)

A ii niijnmax;

gnumrii 2=pkkmiriin3*hrowrOJ+

pkkml~iin+23*hrowL23+pkkmItiin+43*hrowt43;

gdenom=hrowCO3*gnumCO3+hrowC23*gnumE23+hrowC43*gnum[43+rlil;

/4 THIS IS THE FIRST GAIN COLUMN *for (ii-O;ii<nmax;ii++) gitiiJ=gnum~ii3/gdenom;

1* printf("\nVECTOR HROW :\n");

182

printvac (hrow.5);printf("\nYECTOR 6NUM s \n");printvac Cgnum,5);printf('\nDENOMINATOR is i "i);printf CX. \n',lgdunom);printf("\nVECTOR 31 t \n");printvec (gi ,5); *

/* CALCULATE THE COVARIANCE OF ERROR PI *for Cii-O;ii<nmax;ii+-2)

pdum~i13-(-giEO3*hrow~ii3);pduanCii+53-(-gi Cl3*hrowCii 3);pdumEii+103=(-gi £23*hrowc ii3);pdum~ii+153-C-giC33*hrowEii3);pdum~ii+203in(-gi[43*hrowCii3);

pdumCO3+-1 .0;.pdumE 123+-i1.0;pdumC243+-In.0;

piCOJ -pdumC03*pkkmIC03+~pdum[23*pkkmIC103+.pdumE43*pkkml C203;

pi113 =pdumco3*pkkmI£13+pdumC23*pkkml£113+pdumC43*pkkml £213;

piE23 -pdumC03*pkkmIC23+pdumC234lpkkmI£123+pdumE43*pkkml £223;

pi £33 -pdumE03*pkkmI E33+pdumC23*pkkmlC 133+pdumC43*pkkml £233;

pi[43 -pdumC03*pkkml[43+pdumC23*pkkml[143+pdum[43*pkkml £243;

* pi[53=piEl3;pi £&3 -pdumr53*pkkml £13+pkkml £63+

pdum£73*pkkmll1.A+pdumt93*pkkmlE2l3;pi[73 -pdum£53*pkkmlC23+pkkmlC73+

pdumC73*pkkml£123+pdumE93*pkkmlE223;pi£63 -pdumC53*pkkmI£33+pkkml£93+

pdumE73*pkkml £ 33+pdum£93*pkkml £233;piE93 =pdum£53*pkkmlC43+pkkmlC93+

pdumC73*pkkml £143+pdumC93*pkkml C243;pi[103mpi £23;pi[113inpi 73;pirCl23=pdum£ 103*pkkml £23+pdum£ 123*pkkml[E123+

pdum£ 143*pkkml £223;pi £ 33=pdumIn 03*pkkml £31+pdum£ 123*pkkmlEC 33+

pdumE 143 *pkkml[£233;pi[£143=pdum£ 103*pkkml C43+pdum£ 123*pkkml E143+

pdum£ 143*pkkml £243;pi£153=pi£3T3;pi £163-pi £82;pi[£173-pi £133;

183

pi 18:3-pdumrl5:3*pkkmlr3:3+pdumcl73*pkkmlrl33.pkkml C 19+pdumC193*pkcml C233J;

pi 19:3-pdumCl5:3*pkkmlC4:3+pdumCl73*pkkmlrl43+pkkmlCl93+pdumCl93*pkckmlC243;

pi C2OI-pi C43;piC21l-piC93;pi C223-pi C143;piC233-piC193;pi C243-pdumC203*pkkml C4:l+pdumC223*pkkmlC 14:34

pdumC243*pkkml E243;

/* CALCULATE FIRST MEASUREMENT PREDICTION *zhat-vulm*denom;zdif-f113-zicCi3J-zhat;

/* COMPUTE THE SATE FOR ERRONEOUS TIME * .

/* MEASUREMENTS *pklin(piEO3 < 0) ? -piE03 a piEOJ;pk3(ipi[123 < 0) ? -piCl23 s piCl23;pk5-(piE243 < 0) ? -piC243 : piE243;pll1(pkl > pk3) ? pkl : pk3;p =(p11 > pk5) ? p11 : pk5;

1* pgatuinuqrt (p)*velm;41/

xm=p; 7i-f (x !-=0.0)

i f (x < 1.0) x-1.0;do

oldx=x;Xin( x + p/x )*0.5;temp-al dx-x;rabs-(temp < 0) ? -temp :temp;

while (rabs > 0.0);

pgatunx *vel m;gaten3. 0* (pgate+rrli3);

1*print-F("\nMATRIX PDUM : \n");printmat (pdum,5,5);print-F("\nMATRIX PI : \n");printmat (pi ,5,5);print4('1\nZDIFF is t 11);

printf ("\nGATE is ";

printf("V/.e \n",gate); *

184

/* EDIT INVALID TIME MEASUREMENTS *

zgatnuCzdiffti3 < 0) ? -zdif4Ci3 a zdif4Ci3;

if (zgate >- gate)

for Cii-O;ii<nmax;ii++) gi~ii3Jm0.01zdiffti 2-999.0;

1* TAG FOR INVALID TIME MEASUREMENT *

/* CALCULATE ESTIMATE BASED ON ONE *

/* MEASUREMENT PREDICTION *

xiri13-xkkmInCii3+gi Cii32*zdiffti3;

1*printf("\nVECTOR XI i naprintvuc Cxi ,5); *

for (iiQO;ii<nmax;ii++)

xkkenl~ii3-xi Cii 2;pkkmltii3inpitii3;

for (ii-nmax;ii<maxsq;jj++) pkkmlCjalinpitjj];if Ci -- 3) goto 156;

/ * end 35 loop *

/* NOTE a CALLED ORIGINAL X(0/-l),XKKM1. */* UPDATED AFTER 1 MEASUREMENT CALLED IT XI,*//* THEN MADE XKKM1 AND WENT THRU ITERATION *I. AGAIN. AFTER YOU HAVE UPDATED XKKM1 FOR */* EACH MEASUREMENT XKK=XI AND PKK-PI. *

156: for (iiinO;ii<nmax;ii++)

xkk~iijinxitii3;pkktii3-pitii I;

for (jiinmax;Jj<maxsq;jj++)

pkkrjj3-pi Cjj3;

1*printf("\nUPDATING VECTORS INTERVAL ... \n)printf("\nVECTOR XKKM1 : n)printvuc (xtckml ,5);printf("\nMATRIX PKKMI : n)printmat (pkkml ,5,5);

printf("\nVECTOR XKK : \n");printvec(xkk,5);printf('"\nMATRIX PKK : \n");

185

printmat(pkkvS,5)1 L*1

/* RECALCULATE TIME MEASUREMENTS, and FORM */* ABSOLUTE VALUE OF RESIDUALS. *for (i0O;i<4;i+4)

/* EDIT INVALID TIMES FOR ADAPTIVE MANEUVER */* ROUTINE *

i f (zdiffCi3 < 999.0)

x-xkkmlE03-xb~i 3;y-xkkmlC23-ybti 3;zixkkml[43-zb~l J;

/* donom-uqrt(x*x+y~y+zaz); *

denomnx *x+y*y~z *z;x-denom;if (x !- 0.0)

i f (x < 1.0) Xinl.0;do

oldx-x;x-( x + dunom/x )*0.5;temp-oldx-x; **rabs-(temp < 0) ? -tamp :temp;

while (rabs >- 1.e-12);

denom-x;zhat-vel m*denom;temp-ziali3-zhat;zdiff~i3-(temp < 0) ? -temp :tamp;

zdif-FCi3J0.0;nzdi ff-=l;

1* THIS FUNCTION COMPUTES THE ADAPTIVE Q MATRIX *if (icurve -1)

bb-xkkml C3J*xkkml C33;dd-xkkm C 13*xkkml 1 3;almbb+dd;

bdinxkkml CI *xklcmlC3;ulindd*ziqacc/al+bb*ui gcc;ol2inbd~si gac/al-bd*si gcc;&2-bb*si gacc/al+dd*si gcc;qEO3-g2*el;qE 1 2ig3*ol;qE23-g2*oi2;qC3Jing3*e12;qE62ina2*e1;q £72-qC3;q £92 ia2*. 12;qE 122in2*e2;q[132ing3**2;qC lBina2**2;

bb-ngammaE03*gammaEO3;dd-gamnmaE13*gamma[ 13;q£O03nbb*covwCO3;qE 13-gammaEOJ*covwEo2*gammaC 12;qE23-0.0; qC3320.0;q[63=dd*covwEO3;q[73-0.0; qC83-0.0;qCl23-bb*covwC 12;qE 133=gaaumaCO24lcovw£ 1 *gammaC 12;qEl83-=dd*covw£ 12;

q[53=q[lJ; qElO0inqC23; qEll2-qE73;q[lSJ-qC33; qC163eq[83; qC173=qC133;

1* printf("\nMATRIX 0 1 ni* printmat(q,5,5); *

1* IF ALL TIME MEASUREMENTS EXCEED GATE, BYPASS */* ADAPTIVE MANEUVER ROUTINE. *

if (nzdiff !- 0.0)

zdifav=(zdiffC03+zdiffClJ3.zdiffE23+zdiffC33)/nzdiff;1* ~printf("\nZDIFAV is: )

printf("V/.u \n",zdi-Fav); *

/* DO NOT PERFORM ADAPTIVE MANEUVER IF */* FILTER HAS NOT ACHIEVED THE STEADY STATE. *if (kk > 3)

1* IF ZDIFAV MEETS CRITERIA TRANSFER OUT /*/* OF ADAPTIVE MANEUVER ROUTINE. *if (zdifav > value && icont < 5)

16E7

* /* INCREASE GAIN 4for Cj-O;j<maxuq;i++)

pkkml Cj3-pkkEJ3+qCjJ;/4printf('\nINCREASE GAIN AND PERFORM");

prirntf(" ADAPTIVE ... \n");printf("\nMATRIX PKKM1 z\n");printmat(pkkenl,515); *

/4 PERFORM ADAPTIVE MANEUVER BY 4/4 REITERATING SAME TIMESLOT.

icont+=1;goto adapt;

skip:i continO;nzdiff-4;xdiff CO3xkkCO3-truedCO3;

* xdiffE 13-xkkE23-truedEI13;xdiffC23-xkk143-truedC23;

.J. /4 tempinxdiff Cl Jxdiff Cl /pkkml r123;if (temp < limvar) icurveO0;else icurvumi;

f printf (fps, "\n %d %a %e %all,kk,xdiffEO3,xdiff~l3,xdiffE23);

/* WRITE 41*printf("\nVECTOR ERROR :XDIFF YDIFF ZDIFF\n");

printvec Cxdiff ,3); 4

/4 PREDICTIONS FOR PKKM1 4

for (i0O;i<nmax;i++)

phipkkli3=pkk~i )+pkkli+5:I*phisCOJ;phipkk~i+53-pkk~i+53;phipkkli+1Oi-pkk~i+1OJ+pkkli+153*phist23;phipkk~i+153=pkkli+153;phipkkri+20:i=pkk~i.203;

f or (i0;i<nmax;i+e)

in-i *nmax; .pktumprin3inphipkktin:J+phipkktin+13*phisE03;

188

pktamprin+13-phipkkrin.131~pktempEin+23-phipklcCin+23+phipkk~in+33*phisC23; '

pktampCin+33-phipkkCin+33;pktwmp~in+43-phipkk~in+43;

for (i-Olj<maxsq;j4+) pkkmlCj3=pktampCjJ+qCj3;

/4printf("\nPREDICTIONS FOR PKKM1 ... \n)printfC"\nMATRIX PKKM1 z \n");printmat (pkkml ,5,5); 4

/4 CALCULATE NEW XKKM1 4

for (iiiO;ii<nmax;ii++)

xkkmlrii3-xkkrii3,;/4 p1-sqrt(fabs(pkkEii])); 4

xkkmI10O-xkkm1 EO3+phisCO3*xkk[l13;xkkanl C2)ixkkml C23+phisC23*xkk3;

/4printf("\nNEW XKKM1 ... (for use in next time'l)lprintf (" interval) \n");printf("\nYECTOR XKKM1 \n");printvec (xklcml ,5); 4

fprintf(fppl"\n %d %a %e %e"l,kkjpkkCO] ,pkkt 122 ,pkkE243);

fprintf(fpxyz,"\n %d %e %e %e"l,kk,xkkEO03,xkkE23,xkk[43);

S/4 end TIME loop 4

fclose(fpxyz);fclose(fpe);fclose(fpp);fclose~fpzic);fclose(fptrj);printf("press a key and ENTER to continue ... 1);scanf ("Xs",&c);

quit:-printf("\nquit ? EY/N3 :"1);scanf (l%s"l,&c);if (c--n' 1H c-'N') goto menu;

)/4 end MAIN PROGRAM *

189

* printvac~vactor~dim)irnt dim;double vactorE3;

int i, jfor Cj0O;j<dim;j-0) printf('Xu %ls",v~ctorCj3," 11);printf CInI;

printmat (matrix ,row,col)int row~col;double matrixrj;

int i~j;for (imO;i<row;i++)

-Cfor (j0O;j<col;j++) print-f("%e

printf ("\n");

190

trajuc(narray,hydrolxb,ybvzb,xkkmlljstartljundvwdicg,worg)double hydroC3,xbE3,ybC3,zbC3,xkkmlrl ,wd,g;int narray,worg, istart, jend;

int pligi,start,and;double pp1,r1,sqrt0,zcs,zic143,ziE43;double xd ,ydlzdgvldengvelmqg;static double xnE63-(-3.01, -2.0, -1.5, 1.0, -0.6,

0.0);static double ynE63-(0.0, 0.0228, 0.0668, 0.13579

0.2743, 0.5);static double snE53J(43.8596, 11.3636, 7.25689,

2.891352, 2.65887);

double datarsJ,wr;int runn,iS;FILE *fptrj, *fopeno;FILE *fpzic, *fopeno;int j,jj;

1* THIS ROUTINE COMPUTES TRUE TRAJECTORY OF TORPEDOdataE03=trus x position, dataE13- true y positiondataE23=true z position, winturn ratedata3-x velocity, dataE43=yvelocityjstart-time slot where maneuver startsjend-time slot where maneuver ends *

dataCO3-xkkmlEO3; datarl3=xkkml[23; datar23-xkkml[43;dataE33=xkkml13; data[43=xkkmlr33; velml1.0/vel;r 1=0.49;wr=twopi*wd/360.0; ggq32.2*g; startjstart; end=jend;

printf("\n %s \n","start execution of trj.c");fptrjfopen ("trj.dat, I'w-1);f pzic=f open ("zic.dat", "w");

for (jj=O;jj<150;jj++)

1* GET HYDROPHONE ARRAY COORDINATES *if (narray !=0)

if (datatO) > swO) i8=0;else if (datatO] > swi) i8=12;else if (dataCO3 > sw2) i8324;else if (data[o] > sw3) i1836;else if (datatO] > sw4) i8=48;else i8=60;f or (i =0; i< 4; i++)

xb~i3=hydrotjJ;

191

AD-RI67 871 KALMAN FILTER REAL TIME TRACKING USING MICROPROCESSORS 3403AND A NEW LANGUAGE FOR MILITARY SOFTNARE(U) NAVAL

POSTGRADUATE SCHOOL MONTEREY CA J A FERREIRA 27 MAR 6

7 UNCLSSIFIED FIG 9/2 NEh Mhhhhhhh

mhhoomhhmhlm

131.2

1j16

1.25

MiInr, CHR

ybC13=ihydroCJ+13;zbC13-hydroCJ+23;

/* COMPUTE THE TRUE TIMES AND THE TRUE POSITIONSWRITE TRUE TIME and TRUE POSITIONS *

pri ntf ('Xs" 1);fprintf~fptrjga"\n %& %a %all,

dataC02 ,dataC 12 dataC23);

for (iinu;1<4;i++)

xd-dataC03-xb~i32;ydindataC 13-ybCi 2; ~*zd-dataC23-zbli 2;don-sqrt Cxd*xd+yd*yd~zd*zd);ziC13 -velm*den;

1*printf("\nTRUE TIMES : n)pri ntvc (z i 94) ;printf("\nTRUE POSITIONS : \n");printvec (data,3); *

/* GET NOISE,SCALE AND ADD TO TRUE */* MEASUREMENT TIME *pplin317. 0*rl;pliinppl;rl-ppl-pli g

i f (v > 0.5) v-1.0-rl;r2: if (v < yntj+13) goto r8;

j4m1;goto r2;

res vi(v-yn~j3)*snCj3+xn~j3;if Cr1 >- 0.5) v-C-v);

zcsn C/:3. 00) *0. 00001;

1* printfC"\nRANDOM NUMBER :'1);printf ("%e \n",zcs);*/zicri 3-zcs+zi Ei 3;

fprint-f(-Fpzic,"\n %e %e %e %e",zicEO,zic113,zict23,zicE-13));

/* NEW DATA *line~jj,0,150,data) ;

192

fclose(fptrj);fclou.(fpzIc);

1 ine(j ,.tartgand~data)double dataC31int j~start,andicif(Cj >- start) && Ci < end))

dataCO3+-1 .31*dataC33;datatl3+ml.31*dataE43;

193

APPENDIX 8

LISTING OF PLOT.C - PLOTTING ROUTINE

1. PLOTTING ROUTINE -FILE PLOT.C (1BM-PC) *

*include "stdio.h"#include 'math.h"#include "limits.h"i#include "kalman.h"maino(

float ixCnpt3,varrnpt3,sum,.umsq;int i,col,rowlinc,c,tt,choicegnarraygrunniop~cc;float xlnpt3, ylnpt3, zlnpt3,ttnpt3,xx,yy,zz;float xmax ,xmingymax ,ymin~powergyaux;float height,width,xscale,yscale;float ymaxt ,ymint ,xmint ,xmaxt;double xkkmlt53;

FILE *fpt, *fopen();FILE *4pxyz, *fopen();FILE *fpe, *fopeno;

* FILE *4pp, *fopeno;FILE *fpin, *fopeno;

f pi nfopen ("trj. inp's, 'r"1);fscanf(fpin,"%d %d11,&narray,&runn);fscanf(fpin,"%lf %If %lf %14 %1+f1,

&xkkmICO),&xkkmlE239&xkkmlE43,&xkkml[123,&xkkmIC33);

fclose(fpin);

CLRSCRN;printf("\n INITIAL CONDITIONS FOR RUN # )

printf ('7d1 ,runn);puts("\n \n");if (narray-O)

printfQ'\n CASE is SINGLE ARRAY.");else printf('\n CASE is MULTIPLE ARRAY.");printf("\n %.s %e %s %e %s %e \n"," xpos *

xkkmlCOJ," ypos = ",xkkm1E23," zpos "

xkkml[43);printf("\n %s %e %s %e \n"," xvel =V

xkkml[13," yvel =",xkkmIE33); I

choi ceinl;

'94

printfC"\n\n\n\n\n\n\n\n\n\n\nPruss any key to");printf(C" continue ... "1); 4.

c-keyrd();

for( ;)

CLRSCRN;printf(C" PLOT OPTIONS v "1);

printf C"Wn");printf("\n 1) XEST vs TIME-.SLOT '1);

printfC"\n 2) YEST vs TIMESPLOT S);

printfC"\n 3) ZEgST vs TIMELT")printf("\n 4) YEST vs XES __LO);)printf("\n S) X.ERR vs TIMES.LOT 1);printfC"\n 6) YEgRR vs TIMES$LOT "1);

printf("!\n 7) ZERR vs TIMES.LOT "1)

printfC"\n 8) XYAR vs TIME_.SLOT 11);printf("\n 9) YVYAR vs TIME-SLOT "1);

printfC"\n 10) ZYAR ve TIMESPLDT ");

printf("\n 11) PKKX vs TIMESLOT "1);

printf("\n 12) PKKY vs TIME_SLOT "1);

print.F("\n 13) PKK_Z vs TIMEA.LOT '1);

print-F ("\n");printf("\n 14) TRUE TRAJECTORY "1);printfC"\n 15) CHANGE PLOT [dot <-> linear");pri ntf C" (defaul t-l inear) 3 1);printfC"\n 16) EXIT")printf ("Wn");print-F("\n enter value (1-16) ")

mop: CURSAVE;scanf("%2d",&op);if (op<1 11op>16)

BELL;CURREST;goto mop;

printf("\n~s%d~s"1," option is "1,op,Is Y/N3 z "1);

scanf ("%s"1,&cc);if (cc'-*y' && cc!-*Y')

BELL;CURREST;goto mop;

printf("\n wait")

switch (op)

case 1:

195

f pxyz-f open C "xkk. dat", "r");If scarif (fpxyz,"1\n %d %f %f %f",&tt,&xx,&yy ,&zz);xC03-xx; tECj3nttjymaxinxx; ymininxx; xmax-npt; xminin0.0;for Ci-l;i<npt;i++)

fscanf(fpxyz,"\n %d %f %f %f",&tt,&xxg&yy,&zz);

xrijinxx; tC13-ntt;

if Cxx>ymax) ymaxinxx;also if (xx<ymin) ymin-xx;

printf ("%s",".");p

plot(ymaxgxmaxgxminlyminlxltlchoice);biosset (3,30);printf(C" %s ".,"X__EST vs TXME_...LOT"I);biosset (4,8);pri ntf ("%s",I" (FT) 1);fclose(fpxyz);break;

case 2s

f pxyz-f open ("xkk. dat" , r");fscanf(fpxyzl"\n %d %f %f %f",&tt,&xx,&yy,&zz);xEO03nyy; tE03-tt;ymax-yy; ymin-yy; xmax=npt; xmin=0.0;for (i1l;i<npt;i++)

f scanf (f pxyz, "\n %d %f %-F %/f"&tt,&xx,&yy,&zz);

x~i3=yy; tri3J=tt;

if (yy>ymax) ymax-yy;else if(yy<ymin) ymin=yy;

printf ("%s",. II);

plot Cymax ,xmax ,xmin ,ymin ,x ,t,choice);biossut (3,30);printf("I %s ",1Y-EST vs TIME_SLOT");b iossut (4, 8) ;print-f ("%s5","1 (FT) "

fclose(fpxyz);break;

case 3s

196

f pxyz-f open C "xk'k. dat" ' "Ffscan-F~fpxyzl"\n %d %f %f %fI",&tt,&xx,&yy,&zz);xCO3-zzl tCO03ntt;yuax-zz; ymin-zz; xmax-npt; xmin-O.0;for Ciin;i<npt;i++)

facanf(fpxyz,'\n %d %f %f %f",&tt,&xxg&yyg&zz);

xCijinzz; tC13-ntt;

if Czz>ymax) ymax-zz;else if Czz<ymin) ymininzz;

print-F ("Xs"" "1);

plot(yniaxgxmaxsxmingymingxqtgchoice);biosset(3930);printf(" %s ",'ZEST vs TIMES.LDT");biosset (49);printf ("Uc"," (FT) 1);f close (f pxyz);break;

case 4s

f px yz-f open ("x kk. dat" Ir"1);1fscanf(fpxyz,"\n %d %f %f %f",&tt,&xx,&yy,&zz);tC03-nxx; xCO3-yy;

C ymaxinyy; ymin-yy; xmax-xx; xmin-xx;for (iml;i<npt;i-+)

Cfscanf (fpxyz,"1\n %d %~f %/f %hf"1,

&tt,&xx,&yy,&zz);t~i3-xx; xci3inyy;

if (yy>ymax) ymax-yy;else if(yy<ymin) ymin-yy;

if (xx>xmax) xmax-xx;else if (xx<xmin) xminxx;

printf(11%s", 1. "1)

plot(ymax,xmax,xmin,ymin,xgt,choicm);biosset (3,30);printf(" %s 11,11YEST vs XEST");bioset (4,9);pri nt-F ("Xs" ,"11(FT)"1);biosset (22,63);printfC("X"," (FT) ");fclose(fpxyz);

197

break;

case 5.

fpe-fopen('xdif-f.dat1"0'Ir");fscanf(fps,"\n %d %f %f %f"g&tt,&xx,&yy,&zz);xEO3-xx; tCO0intt;ymax-xx; yniin-xx; xmax-npt; xmin-0.0;for Ci1l;i<npt;i+4)

Cf scanf (f pe, 11\n %d %f %f %f",1

&tt,&xxl&yyg&zz);x Ci 3-xx; t~i3-tt; .--

if Cxx>ymax) ymaxinxx;

else if (xx<yinin) ymin-xx;

pr intf %se"" 1 )

plot(ymaxgxmaxgxmin,yminlx,t,choice);biosset (3,30);print-f(" %s "9"XERR vs TIME_S$LOT");biosset (4,8);printf ("%sl'," (FT) 1);f close (f pe);break;

case 63

f pe-fopen 'xdi ff dat","r")fscanf(fpe,"\n %d %f %f %f",&tt,&xx,&yy,&zz);x[03myy; tEO3=tt;ymax=yy; ymin-yy; xmax-npt; xmin=0.0;for (i1l;i<npt;i+-)

f scatf (f pe, 11\n %d %f %f %f",&tt,&xx,&yy,&zz);

x~i3=yy;, tli3=tt;

if (yy>ymax) ymax-yy;else if(yy<ymin) ymininyy;

printf ("Us",".)

plot(ymax,xmax,xmin,ymin,x,t,choice);biosset(3,30);printf(C" %s ","Y-ERR vs TIMESLOT");biosset (4,8);pri ntf (11%s"," (FT) "

fclose(fpe);

198

* .-. - . D ft

break;I

case 7t

f penf open (11xd iff dat, "r 1)* fscanf~fpo,'\n %d %f %f %f',&ttg&xx,&yy,&zz);

xE03-zz; tEoj3ntt;ymaxinzzl ymzin-zzj xaaxinnpt; xmlrin0.O;

I for Ci1;li<fptli*+)

fscanf(fpeq"\n %d %f %f %41',&tt,&xx,&yy,&zz);

xCi3-zz; tEijintt;

if (zz>ymax) ymaxinzz; C-als i-f zz<ymin) ymin-zz;

biosset (3,30);Iprintf(Il %s ","ZEXRR vs TIME_9SL0TI);

biosset (4,8);pri ntf ('%s ,' (FT) I')f ciose Cf pe)break;

case 81

f p-f open('x di ff .dat ,r 1);

fscanf(fpe,"\n %d %f %f Vf",&tt,&xx,&yy,&zz);xC03-xx; tC03intt;varC03=0.0; suai-0.0; *umaqin0.0;ymax-xx; ymin-xx; xmax-npt; xminin0.0;

p. ~for (iinl;i<npt;i+.) f,

N ~~&tt,&xxy&z)x Ci 3-x x tti3-~tt;

sum+inxci3; sugnsq+ix~i3*xEi3;varli3-((tli]+1.0)*sumsq-sum*sum)/((tliJ+1.O)*tEi3);

xx=varti3

if (xx>ymax) ymax-xx;else if(xx<ymin) ymin-xx;

)r n - "s ," 1

plot(ymax,xmax,xmingymin,var,t,choice);

biosuet (3,30);

199

d. --- --

printf C* %s "1,"XJJAR vs TIME.SBLOT");biosset (4,7);printf (IXsa " (FTsq)1") ;fcl ouCf pe);break;

case 9: .*"

fpeufopun ("xdif~f.dat", "r");fscanf(fpe,"\n %d %f %f %f",&tt,&xx,&yy,&zz);xC03-yy; t[03-tt;varro3-0.0; sum-0.0; sumsq-0.0;ymax-yy; yminyy; xmaxinnpt; xmin=0.O;for (i1l;i<npt;i++)

f scanf (fps ,"1\n %d %f %f %f",&tt,&xx,&yy,&zz);

x~i3-yy; t~i3-tt;

sum+-x~i3; sumsq+inx~i3*xciJ;varCi3=((t~i3+1.0)*sumsq-sum*sum)/((ti+1.0)*tiJ);

yyinvarCi J;

if (yy>ymax) ymaxyy;esu if(yy<ymin) ymin-yy;

printf ("Xe-",' 1);

plot (ymax,xmax,xmin,ymin,var,t,choic.);*b i oset (3,30);printf(" 7.s ","Y .VAR vs TIMESLOT");biosset (4,7);print-f ("e"," (FTsq)";--f close (f pe);break;

case 10:

f psf open("'xdif f .dat","r")fscanf(fpe,"\n %d %f %f %f",&tt,&xx,&yy,&zz);x[03=zz; tCO3=tt;-varCO3=0.0; sum=0.0; sumsqO0.O;ymaxzz; ymin-zz; xmaxnpt; xmin=0.O;for (i1l;i<npt;i++) h

Cfscanf(-Fpe,'\n %d %f %f %f",

&tt,&xx,&yy,&zz);xti3=zz; tli.J=tt;

sum+mxCiJ; sumsq+=xti3*xti3;varti3-((tCi3+1.O)*sumsq-sum*sum)/((tEi]+1.O)*tli3);

200

zzinvarE13I;

if (zz>ymax) ymaxinzz; . Iaiso if(zz<ymin) ymin-zz;

printf ("2."l,". 11)

plot(ymaxlxmax,xmingymin,var,t,choicm);biosuet (3,30);printf("1 %s ","ZYVAR vs TIMESLOT");bioasset (4, 7);printf (1%" 1", (FTsq)")f ci as* (f pm);break;

case Ili-Cf ppnf open (1p k k. dat" "r"1)fscanf(fpp,"\n %d %f %f %f",&tt,&xxg&yy,&zz);xCO3-xx; ttojintt;ymax-xx; ymin-xx; xmax-npt; xminino.0;for (i-l;i<npt;i++)NO

1C '

fscanf (fpp,"\n %d %f %f %411,&tt,&xx,&yy,&zz);

if (xx>ymax) ymax-xx;

else if(xx<ymin) ymin-xx;

printf ("U5",. 1);

plot Cymax ,xmax ,xmin,yminqx ,t~choicu);biosset (3,30);printf("1 %s ","PKKX vs TIMESLOT");bi asset (4, 7);printf ("%s"," (FTsq)1");f close (f pp);break;

case 12:

f pp=f open("p kk. dat 1"rl);fscanf(fpp,"\n %d %f %f %f",&tt,&xx,&yy,&zz);

ymaxyy; yminyy; xmaxnpt; xmin=0.O;for (i=l;i<npt;i++)

f scanf (f pp,"11\n %d %f %f %f",&tt,&xx,&yy,&zz);

201

if (yy>ymax) ymax-yy;also if(yy<ymin) ymininyy;

printf ("Ws",1".11)

plot(ymaxgxmaxgxmingymin,x,t,choice);biosset(3,30);printf(U %am 111PKK_V vs TIMES.LOTV);biosset(4,7); Pprintf ("Xs" 6 (FTsq) ");f cl ose (f pp)break;

case 13t

f ppnf open"p k k. dat" "r"1)fscanf(fpp,"\n %d %f %f %f",&tt,&xx,&yy,&zz);xEO3-zz; t[03mtt;ymax-zz; ymininzz; xmax-npt; xmin-O.O;for (i=l;i<npt;i++)

f scan-F(f pp , 1\n %d %f %f Vfl,&tt,&xx,&yy,&zz);

x~i3=zz; t~i3-tt;

if (zz>ymax) ymaxzz;else if(zz<ymin) ymin=zz;

pr-intf ("%s"',".

plot(ymax,xmax,xmin,ymin,x,t,choice);biosset (3,30);printf(" %s ","PKK_Z vs TIMESLOT");biosset (4,7);printf ("%so', (FTsq) '

fclose(fpp);break;

case 14:

f pt=f open ("tri. dat" fo *

fscanf(fpt,"\n %f %.f %f",&xx,&yy,&zz);xC03=xx; y[0O=yy;ymaxyy; ymin-yy; xmaxxx; xminxx;for (i=l;i <npt;i++)

fscanf(fpt,"\n %f %.f %f",&xx,&yy,&zz);xliJ=xx; yli.J=yy;

202

if(yy>ymax) ymax-yy3also if (yy<ymin) ymin-yy;

if (xx>xmax) xmax-xx;else if (xx<xmin) xmin-xx;

printf ("%s"1,1.1");

plot(ymaxlxmax,xmingyminlyvx,choicu);biosuet (3,30);printf (" %s ","TRUEVy vs TRUEX"1);biosuet (4,B);printf ("1%sl, " (FT) ");

biosmet (22,63);-printf ("1%s"," (FT) "1);

fclose(fpt);break;

case 15:.

printf("\n CHANGE PLOT (enter option ))printf ("\n");printf ("\n 1) LINEAR INTERPOLATION BETWEEN");printf C" POINTS");printf("\n 2) SIMPLE DOT");printf ("\n");printf("\n ENTER choice 11)scanf ("%d"1,&choice);goto skip;break;

case 16: goto ending;default:

goto skip;break;

I /* end case *

CURMV(4,67);printf("'%s %d","-# ",runn);CURMV(5,67);if (narray==0) print-F("SINGLE");else printf ("MULTIPLE");,

c-keyrd 0;biosini (3);

skip:

I 1* end for--forever-loop *

203

* uendings prlntfC'\n");printfC"\nEnd platting ... Press any key");c-keyrdoC;CLR.P.CRN;

.V.

204

/41 Function to draw two axis.4/

/41 **1* NOTE 444 1/41 The upper left corner of the screen is 0,0 and the 4//41 bottom right corner is 1999639.4/

int axis~ulrowgulcol,lrrow,lrcollmantxgmanty)int ulrowgulcolllrrowvlrcol;double mantxC39mantyC3;

int countlirowgiliauxgicolgtcount,cincgrinc,hinc;Radoubledeltax ,deltay,ymax ,xmin ,x ,ygrowmax ,rowmin ,colmax ,colmin;double deltargdeltaclaux;

ymax mantyCoJ;xminmmantx ci :;deltay0. 1*(mantyEO2-wnantyclJ);deltax-O. 1*(nmantxEo)-mantxriJ3);rowmax-Ilrrow; rowmin-ul row; del tar=0. 141(rowmax-rowmin);colmax-lrcol; colmin-ulcol; deltac0. 1*(colmax-colmin);rinc=deltar+0.5; cinc-deltac+0.5;

/41----------------------------------------------------------------------41

/41 do vertical--------line with scale4/i-ulcol;for (irow - ulrow+1; irow <= lrrow; irow++)* bioswd(irow.,i.);for (irow-- ulrow; irow <- lrrow; irow+=rinc)

for (tcountmi-4; tcount <= i; tcount++)bioswd (irowltcount);

/41 do horizontal--------line with scale /i-lrrow;for (icol = ulcol+1; icol <= lrcol; icol++) bioswd(i,icol);for (icol - ulcol; icol <= lrcol; icol+=cinc)

for (tcount=i; tcount <= i+4-, tcount++)bioswd(tcount,icol);

/ * - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - -- - - - - - - -

/41 write vertical scale *y=ymax;for (irow = ulrow; irow <- Irrow; irow+=rinc)

iirow/.O+0.5;

205

bi osuut Q 0) 1print-F(7.lf',,y);y--deltayl

/* write horizontal scale 4x-xmi n;f or Cicol - ulcol;- icol <= ircol; icol+-cinc)

iinicol*0. 125-1.5;biouset(24,i);print-f("7.lf",x);x+-d41tax;

/--------------------------------------------------------------------/

/* done *

return (0);

plot(yymaxgxxmaxgxxmin,yymin,xltlchoice)float yyanax,xxmax ,xCJ ,trE,xxmingyyiin;int choice;

float xscale,yscale,heightgwidth,bxlby;float ymax,xmaxlymingxmin;double xauxlyaux,mantxC23,mantyC23glog1O;double aux~powo),auxl;int ilinc,rowgcollixexpgiyexp,C,oldrow,newrow,ii,iimax;int collvcolOgrowlyrowogirow,oldcol,icol,newcol;float deltac,deltar,delta,ang;

/* DETERMINE THE SCALING 4ymax=yymax; xmaxinxxmax; ymin=yymin; xminxxmin;if (ymax =iymi n)

ymax-ymax+5. 0;ymi nymi n-5.0;

i-F (xmax=-xmin)

xmax-xmax 45.0;xmi nxmi n-5.0;

hei ght-ymax -ymi n;width-xmax-xmin;xscals-i5da0. 0/width;yscalein-160. 0/height;by-184. 0-ymin*yscale+0. 5;bxbO0. 0-xmax*xscale+0. 5;

206

vWU W 7-r. -7. -7

1* DETERMINE EXPONENT and MANTISSA *xauxil. 0;,aux-abs(xmax); auxlinabs(xmin);if Caux < auxi) aux-auxi;

* if (aux!inO.0)

* xaux-logl0Caux);£ xoxpinxauxiif (ixwxp<0) ixaxp--I1;aux-ixwxplxaux-pow(lO.0,aux);

mantx C03nxmax/xaux;mantx Cl3Jixmin/xaux;

yauxl .0;aux-abs~ymax); auxl-ab(ynin);i f (aulx < auxi) aux-auxl; c-if (aux!-0.O)

yaux -1og 10 aux)

i yexp-yaux;if (iyoxp<0) iyexp--Il;aux-iymxp;yaux-pow(l0.0,aux);

matC3ya/axmantyC03-ymax /yaux;

printf("\n~s1","Press any key to see plot.");cikeyrdo;biosini (8);axis(24,40,189600,mantx ,manty);

yaux-yaux;b i osst (3,8);printf ("%. lm',yaux);

xaux-xaux;biossut (22,6);printf (11%. le",xaux);

207

*Lv

/* PLOT USING LINEAR INTERPOLATION BETWEEN POINTS *

if (choicml)

cal 0itE03*xscale+bx;rawOnx CO3*yscale+by;b i owd (raw0,Icol0);

-for (iinl;i<npt;iI+)

cal lt~i J*xscale+bx;rowlnx Ci 1*yucale+by;deltac-col -colo;,del tar-i-owl-rowo;

i f (abs(delta-) >=abs(deltar))

angdultar/del tac;iimax-abs(col 1-colO);ical-l;col -cal 0;if (coll<colo)

icalm-1;

for (ii-o;ii<iimax;ii++)

col+=ical;delta-cal-cal 0;row=ang*delta+rowo+0. 5;

1* if (choic=1)

if (abs(deltar) > 5 && abs(deltac) >5)goto discont; ~'*-

bioswd (row,col);

/ * end-if abs(deltac)>=abs(deltar) *

else if (abs(deltac)<abs(deltar))

ang=del tac/del tar;iimax-abs(rowl-row0);irowl1;

208

row-rowO;if (rowl<row0)

i rowi--; II

for Cii-o;ii<iimax;ii+4)

row+-i row;del tainrow-rowO;col-iang*del ta+col 0+0.5;

/* if (choice--i) '*

if (abs(deltar) > 5 && abs(deltac) >5)goto discont;

bioaswd (row 9col)

1 * end_plseif 4

colOincol 1; rowOinrowi;I 1* end-if for i-09200 *

I /* and-if choice-i 4

Thisporton-i tobe ued i yo wan Jus th

points**4*without**44linear *4*interpolation.***4*

if (chiceP.2

fo/ Cipi~psi+

col -tC 13 x sca o5b.

row-x~3*ysclep.-

/4 Thi ortio iso b sd fyuwatjstte

goto trm*5o

* ~~~i. oinswtot ieritrplto.4

fokrdio;int+)

temiototrio

discont9

4,0

APPENDIX H

LISTING OF THE BATCH FILES -IBM-PC

1* Batch film 'CE.BAT' (IBM-PC). Contains thue*se ~quence of calls to compile thefile KR.C **

1* USAGE: "c. kr' *

ici %11c2 %1link cs+%l+biosio+tr+dostimeXl,nul,lcms+lcserase %l.objpause

210

/* Batch film "KAL.BAT" (IBM-PC). Contains the */sequence of calls to execute the files KR.C */

/* (Kalman Filter program) and PLOT.C (the *//* plotting routine). *//* USAGE. "kal" *//* ./ -'

graphics-loopkr -4000rem DO YOU WANT TO PLOT RESULTS ?pauseplot =8000rem DO YOU WANT TO TRY ANOTHER CASE ?rem CTL-BREAK will stop execution. Press any other key to -

continue.pauseCLSgoto loop

--

° 211 ".

-. . . . . . . C s'. -. ' -.- -. . . . . . . . .

I,

-7 VT

APPENDIX I

SIMTERM- PROGRAM DOCUMENTATION

This Appendix contains the listing of the file "readme"

found in the PC-SIG LIBRARY (Public Domain Library), disk

number 362.

2.1

212t

• °F

SIMTERM Instructions Pages 1'4.4

I. Introduction

Feel free to make copies of this program. There isno restriction on the distribution as long as the copyrightnotices are left on the source and that there is no charge -for the distribution. This program is made available tousers of IBM-PCs (and compatibles since I really like the WKCOMPAQ/PLUS). No guarantees are made or implied. If you haveany questions or suggestions, please give me a call and Iwill try to answer them. A recent acquisition of aninterrupt driven communication driver from the University ofBritish Columbia Computer Center has enabled me to removethe requirement that you have the IBM Async CommunicationsSupport package which included a driver I used to use. Thisversion of SIMTERM is therefore self-contained and requiresno other software to run. Have fun using it and **HAPPYCOMPUTING**.

This floppy contains all the software for theterminal simulator for the IBM PC. The main function ofthis program is to provide a reasonable terminal simulationon the IBM PC of an HP-like terminal to a UNIX(TM) system.The file 'simterm.exe' is the load module and when invoked,assumes a default of 1200 baud, 1 stop bit, no parity, andflow control (XON/XOFF). The only programs that have to beon the floppy are SIMTERM.EXE, HELP.TXT, and MENUS. If youhave a VENTEL or a Hayes modem, the file BOARDS can becopied over as a starter for a list of BBSs. The BOARDS fileis set up for a VENTEL autodialer. The dialing strings willhave to be changed to conform for the Hayes standard.

If you have DOS 2.0, put SIMTERM.EXE, HELP.TXT,BOARDS and MENUS in a subdirectory called \simterm. Thatway, if you change directories during a SIMTERM session, byspecifying a path for any type of file transfer, SIMTERMwill be able to find its way back to these two importantfiles. If you don't have DOS 2.0, then just IGNORE theoccasional "directory not found" messages, they're harmless.

2. Command Line Parameters

Optional parameters on the command line willoverride the defaults as follows:

Copyright 0 1982,1984 Jim & Eric Holtman

213".'

SIMTERM Instructions Pages 2

."-a. put the simulator into the ADM3a emulation mode

for those systems which don't support TERMCAP.

'-b' Causes SIMTERM to assume you are using amonochrome display, even though you may not reallyhave one.

'-B' will cause the transmit and receive buffers to becleared when a BREAK (End key) is sent. This ismainly for high baud rate lines where there isstill junk in the buffers that you don't want toSee.

"-cn' specifies which COMx device you want to use. The

default is 1 (use COM1).

"-d' Enter direct connect mode. SIMTERM won't wait forcarrier detect. Use this option if you'reconnected directly to the -emote, with no modem.You can also use this option when SIMTERM sems tohang at startup, with the "Estamlishcommunications link" message. This may be casewith Hayes modems if you don't have them optionedto raise the carrier detect.

-D' When this flag is used, SIMTERM will ignore DEL(127 Ascii) characters, and the characterimmediately following the DEL. We use it becausewe have a noisy line.

"'-e" specifies that the EPSON printer does not have theGRAFTRAX option. Therefore when using 'ibmpr' toroute stuff to the printer, the code will simulateunderlining, subscripts, and superscripts. Defaultis an EPSON with GRAFTRAX.

9' Causes SIMTERM to assume you are using a graphicsdisplay, even though you may not really have one.

'-h' SIMTERM will operate in the 'half-duplex' mode;i.e., it will echo the characters the user typessince the host system does not.

.-in' specifies the IRQ (interrupt request level) to be

Copyright @ 1982,1984 Jim & Eric Holtman

214

F=ooo5.

- - .7

SIMTERM Instructions Paget 3

used. Default is 4, but some boards (like theQuadram) use IRQ3 for COM2, so in this caseSIMTERM would have to be invoked with the ,,,parameters '-c2 -i3' to set it up correctly.

Tells SIMTERM that you have a Hayes modem insteadof the default VenTel modem. This will enableSIMTERM to use the directory dialing features(Alt-V)

"-M" Redefines the cursor control keys compatible witha set of UNIX-like utilities on a PRIME computerat UofVa.

-nxxxx if you have a Ventel or Hayes modem, this optionwill automatically dial the number when thesimulator is started.

.-0" turns off the error messages caused bycommunications line; e.g., framing error andinvalid parity.

-px- specifies the parity where 'n -none, o'-odd,se'-even, "m'-mark, and "s'-space.

"-PFfil" Use file "file" as a script source file (seeSCRIPTS below)

-PDscr Execute script "scr" upon start-up (see SCRIPTSbelow)

"-r this causes SIMTERM to wait for the horizontalretrace when writing to the screen. If you noticesparkles' on your graphics monitor, invoke

SIMTERM with this option and it should clear up.

"-snnnn' specifies "nnnn' as the baud rate (e.g., -s300).

'-Sn' specifies the number of stop bits (1 or 2)

"-vxxxx specifies the file (xxxx) to be used when theAlt-V options is invoked to dial BBS. The fullpath name of the file should be specified.


215. -

.- . . .. . . . . . . . . . . .

SIMTERM Instructions Page: 4

-wn" set the word length to 'n'. Default is 8 (7 data +1 parity).

"-xn' this option will turn off the XON/XOFF flowcontrol. The program will send an XOFF when thereceive buffer is almost full and then an XON q5#when it has emptied out. It will also recognize anXOFF from the host to stop transmitting and thenan XON to start again.

"-kfilename' will read in the file 'filename' toinitialize the function keys. The file is of theform

n-string of chars

where O<=n<=9

exampleO=who^"V1=who 1 wc^V

If the string is terminated with a "^V' (chr(174)-- obtained by holding down the 'Alt' key and typing 174 onthe key pad), a 'carriage return' will be appended.

For example:

If you want to run at 9600 baud, even parity,without waiting for carrier detect

simterm -s9600 -pe -d

If you want to use COM2 (and it requires IRQ3) at300 baud and run script "xyzzy" from file "scr2"

simterm -s300 -c2 -i3 -PFscr2 -PDxyzzy

If you are using a Hayes modem and want a numberdialed when SIMTERM is started, use the following commandline:

simterm -m -d -n12145551212


216

.' . . S ~ S~ . S.* N *~. ~ *.. 5$ -


3. Program Functions (Unix specifics)

Once loaded, the program will use all availablememory above the module for save space for paging throughprevious output. I have 256K on my system and this providesabout 20 pages (1500 lines) of saved text. The TERMCAP entryfor the terminal is in the file 'termcap.ibm'. This shouldeither be added to your termcap library or the 'shell'variable TERMCAP should be set to it before trying to useany package that requires cursor addressing. The TERMCAPentry makes use of the fact that 'vi* (and 'curses') willuse a special sequence to enter and leave the control mode.The simulator uses these sequences to redefine the cursorcontrol keys so that they can be used in that mode; e.g., in"vi' the "up arrow" will send in a "k", while the "PgDn"will send in a " D" to scroll the display down, etc. Forthose of you who are running on UNIX systems that use"terminfo", there is a file "terminfo.ibm" which has theappropriate definitions.

Output to the printer can be initiated underprogram control with a filter similar to 'hp' which is usedfor HP terminals. The source file for "ibmpr' (which is the,filter that use on UNIX) is on the floppy. You shouldcompile this UNIX and then use for controlling output to theprinter. A typical use of this filter is:

nroff -T37 file 1 ibmpr -p

The *-p' option says to route the output to theEPSON printer. The '-i' option will cause underlined textto come out in italics. The '-c' option is used to invokethe compressed mode (132 characters/line).

If those escape sequences are output, they will bedisplayed appropriately on the CRT and output to the printercorrectly.

4. User Defined Function Keys

There are 10 user definable functions keys thatwill send a character sequence of up to 30 characters. To


217

-7 -7,


display the current contents of the keys, hold the 'Alt' keydown and type "='. To change the contents, hold the 'Alt'key and type "- and follow directions. To invoke thefunctions keys, hold down the 'Alt' key and type thecorresponding digit (e.g., 'Alt 3'). A user can define afile with the contents of the keys and cause it to be readin with the *-k' option on the command line.

5. Upload/Download (Unix Specific)

This is used to transfer 'text' files (ones thatonly contain printable ASCII and tabs) between the PC andthe host. To remember which way the data flow, just rememberthat the IBM PC looks 'up' to UNIX, while UNIX looks 'down'on the PC. To move files between the PC and UNIX, hit 'Cntl-PgUp' to transfer PC->UNIX. Answer the prompts. Hit 'Ctrl-PgDn' to move from UNIX- >PC. As the transfer is takingplace, a message will indicate the # of bytes transferred.When going UNIX->PC, there will be more bytes on the PC diskthan on UNIX because of added 'end-of-line' and 'end-of-text' bytes. This can only be used to transfer TEXT files.

6. Upload/Download (Straight ASCII & XMODEM)

The program can be used to connect to BBSs andany other systems that support async devices. To download afile to the PC, go through the normal dialog on the BBS andthen when it is waiting for you to hit return to start thetransfer, press Alt-C to open the 'copy' file that willreceive the data. You will be put back to the originalscreen where you can hit return. At the end of the datatransfer, hit Alt-C to close the copy file.

To upload a file from the PC to a BBS, again gothrough the dialog to setup the transfer and when the BBSis ready, press Alt-D to specify the data file. This willthen 'dump' the file to the comm line. At the end of thetransfer, you may have to type in a control sequence toindicate that you are done. This option will request a'delay' between characters so that you do not overrun thereceiving system. Use '0' for no delay; a value of '1' isprobably sufficient for most systems that can not handle thenormal baud rate. Experiment.


218


SIMTERM supports the XMODEM protocol for datatransfer from a BBS. To invoke this option, hit Alt-R toreceive with XMODEM at the point that the sender is ready.You will be prompted for the file to receive into and thenthe PC will initiate the transfer of data. Transmitting withXMODEM is invoked with Alt-T and prompts will follow.

7. Script files

Script files are used to allow SIMTERM to domindless interactive login and other such sequences. AScriptfile can be created by your favorite word processorand SIMTERM will compile it for you, and save the compiledversion so you don't have to re-compile at each run. Scriptfiles read much like English, so they are both easy tounderstand and create.

Script Commands Available

Each of the following commands can appear in ascript file. Note that each separate command must beseparated by a space, tab or newline. The compiler ignoresextra white-space, so you can have indenting, multiplecommands per line, or whatever you Want. Syntax notes: "str"in the following denotes a string constant delimited byquotes. A string can have multiple words. To get a quotecharacter into a constant, write it as \". To get abackspace, write it as \\. Although one-word strings neednot have quotes, they should, so that they are not confusedwith keywords (commands). Non-quoted strings generatewarnings from the compiler.

name "str" --- defines an entry point for thescript that you can use to identify this part, i.e, if a"name whuxlb" appears in the file, then you can startrunning the script by using the name "whuxlb". (See "runningscripts" below) If you choose "whuxlb" from the pop-up menulater, the script will start executing at the statementimmediately following the "name" statement. "1

send "str" --- sends the string to the remotecomputer, with a carriage return tacked onto the end. Someescapes can be placed into the string, as follows: an "\1"causes a one second pause when it is encountered. "\m" sends


219

NO%--Li 'r.

111vyr nwnr1.

'7%

SIMTERM Instructions Pagel 8

a carriage return. "\j" sends a line-feed. "\c" suppressesthe carriage return that is normally tacked onto the end ofa string. Remember, though, you need to type TWO backspacesto get one, so these sequences should appear as \\1,\\m, etcin your file.

dial "str" --- dials the string on a VenTeldialer. Obviously, if you don't have a ventel, don't usethis command (Fake it with "send"). For example, to dial"3395" on a Hayes, put a line like "send ATDT3395" in yourfile.

say "str" --- outputs "str" onto the screen, sothat you can put tracer statements in the script.

goto "str" --- branches to str. labels to bebranched to must be followed by a colon. I.e. early in thescript you have a line "label." all by itself. Later, youcan call "goto label" (no colon) to branch back up to theearlier statement

gosub "str" --- like goto, but when a "return"statement is encountered later, the program branches BACK tothe statement that follows the gosub. Gosubs may be nested.

return --- returns from a "gosub" statement

input "str" --- Prompts the user with "str",reads one line, and sends it to the remote computer.

settime "str" --- sets the timeout value to "str"seconds. Default is fifteen.

quit --- execution is returned to SIMTERM.There is an understood "quit" at the end of file - -

The Logical Operators (IF/CASE) 77-77

Besides the COMMANDS, the script compilerrecognizes two logical operators, the if/else statement andthe case.

IF SYNTAX

Copyright Q 1982,1984 Jim & Eric Holtman

220


if "str" stmntl lso stmnt OR if "str" {stmntstmnt

OR Iif "str" stmntl (no ELSE) } elss stmnt

OR 3 else {stmnt

When encountered, the program will scan the outputfrom the remote computer until either it finds "str" or ittimes out. If it finds "str", stmntl is executed, if ittimes out, stmnt2 is executed. Execution then drops down tothe next statement, unless the stmntl or stmnt2 contained agoto or gosub or quit.

stmntl or stmnt2 may be either single commandsfrom above, or multiple commands delimited by C and ) (notewhite space around the brackets. If the else following theif is missing, then execution falls to the next statement.Note that ifs may be nested. For example

if "tr" dial "4244" else if "str2" dial "4324"

Be aware, however, that input is only scannedonce, so if "str2l' appears before "str" in the preceedingexample, both tests will fail!!

CASE SYNTAX

The case statement is like mu'tiple simultaneous IFs.It allows you to set up a few strings to be searched forSIMULTANEOUSLY, with an action specified to be executed ifand when one of the strings is found. Syntax is

casestri: actionstr2: action

otherwise actioncaseend


221

. . . . ....'. ... .. . .. .... " ,. .''." ." ; ,....' ''''., '''' ... ..,, ",. ." "" . .' . .''- ... . " '-'

SIMTERM Instructions Page: 10N

Action can be either a single statement (say,send, dial, or even an if, or another case.) or multiplestatements surrounded by C and ) (like the if clause.). Forexample, if you have

caseerici dial "eric's #"

holtman. dial "another "caseend

and the input is "djsfhj gfjg eric holtman" then "eric's #"-will be dialed and the case will quit. Note that "holtman"is not matched since a case statement executes as soon asany of the strings is found.

Script Examples

Call number 3395, wait for "login" prompt. If Iget it, gosub to a login procedure. If I don't then saysomething, then quit. The login subroutine will send my id,then prompt for my password, send it, then return.

name exampledial 3395if "login" gosub "subr" else C

say "3395 didn't answer"quit

quitsubrg

send "id3434"input "Your password, sire?return

Note that after the "return" the next statement tobe executed will be "quit".

Compiling Scripts

To use the example above, copy it out to a filecalled, for example, "prog". Then invoke SIMTERM with a -

PFprog flag, like so: "simterm -PFprog". This tells SIMTERMthat you wish to use scripts from the file "prog". If no -PFflag appears, SIMTERM will read the file "scripts" which iswhere you may want to keep compiled scripts. After SIMTERM


222

,.. .,-'

SIMTERM Instructions Pag .11

gets running, type an Alt-L. This informs SIMTERM that you 77wish to use a script. It will examine the file "prog" anddetermine that it needs to be compiled. Tell SIMTERM whatfile you want to put the compiled stuff in, then sit backand watch for error messages. After it compiles, hit returnand proceed to the next step. Remember the file name youused for the compiled scripts, and use it next time you runSIMTERM, so that you don't have to recompile everythingagai n.

Using a script

After you type Alt-L, or after the compile isfinished, a menu of all "name" strings will pop up. Ifyou're still using the example, only one name ("example")will be there. Using the up and down arrows, make yourchoice, then hit the space bar. SIMTERM will then executeyour script starting at that named point.

To execute a script from startup, use the -PDflag. For example, to run script "example" from the file"prog.scr", type

simterm -PFprog.scr -PDexample

8. Functions Available from the keyboard .'-

All these functions can be accessed from thekeyboard by typing the appropriate character. If you forgetthe commands, Alt-H will bring up a menu for you. Justfollow the menu selection process until you find the desiredfunction. Note that the menu will tell you which key to useto activate that function in the future. I.e., the menu hasAlt-L next to the script choice to remind you that you canuse Alt-L instead of the menus to activate the scriptfunctions.

F1 Will 'toggle' the display so that usuallynon-displayed control characters are displayed in reversevideo using the upper case letter that they are equivalentto.

F2 Will 'print' the line that the cursor ispositioned at.


223

. . . . . . . . . .

SIMTERM Instructions Pagel 12

F3 Send a BREAK

FS Freezes the display above the cursor.Handy to save some data on the upper portion so you can seeit as you do othercommands.

F9 Turns on the row/column display at thebottom of the screen so that you know where the cursor iscurrently positioned.

F1O Toggles XDN/XDFF. A convenient way ofstopping/restarting the output to the CRT when connected toa system that honors the XON/XOFF protocol for flow control.

Alt-F1 Will terminate SIMTERM, but leave the DTRline high so OR that you can restart SIMTERM and pickupwhere you left Alt-255 off.

Alt-F2 Terminate SIMTERM, and drop DTR so thatthe modem will hang up.

Alt-F3 Toggles the DTR (data terminal ready)lead on the communications line. This is used to drop thecarrier at the modem so that you can redial.

Alt-F4 Toggles the graphics-mode. This willdisplay characters with their parity bits so that thegraphics' characters of the PC (>127) are should. This isused on some of the BBSs.

Alt-F5 Clears line 25 if it becomes cluttered.

Alt-F6 Display various mode settings on line 25.

Alt-F7 Resets the communications software. Insome instances an interrupt might be missed and leave thesystem in a state where it is expecting data that will notbe sent. If you suspect this, try an Alt-F7 before rebootingthe software to see if it breaks you out of the situation.

Alt-C Puts SIMTERM in a 'capture' mode so thatall input received over the communication line is logged toa disk file. Handy for picking up ASCII text from a

Copyright Q 1982,1984 Jim & Eric Holtman p

224

...........................................................

SIMTERM Instructions Page 13 p,

system that does not support XMODEM. It will prompt you fora file name.

Alt-D Will 'dump' a ASCII file on the PC to thecomm line. This is a way of 'uploading' to a system thatdoes not support XMODEM. You can also specify a delay toprevent overruns.

Alt-E Toggles the 'vi' mode. Handy when youexit SIMTERM and then come back into it with UNIX still in•vi'. The mode should be TRUE to have the cursor keys workfor 'vi'.

Alt-F Enter the 'local' line editing mode.. Thisallows you to make corrections to a line and then reenterit. Use the cursor control keys to move to the line, 'Ins'and 'Del' to 4ixup the line and then hit return to reenterthe line and leave the line edit mode.

Alt-H Display a 'help' menu listing the definedfunction keys.

Alt-L Invoke the 'script' processor

Alt-R Receive a file using the XMODEM protocol.

Alt-T Send a file using the XMODEM protocol.

Alt-V Will display a menu that is used with theauto-dial' capability of the Ventel. Once in this menu,

select the desired feature.

Alt-= Display the user defined function keys

Alt-- Change a user defined function key

Home Soto top of file in "vi" (unix specific)

Ins Send the current line to remote (similarto the ENTER key on an HP-2621) OR if in "vi" toggle insertmode.

Del Clear display from cursor to bottom OR if "% -

in "vi" delete a character


225

C ' - .'. . -". *

...%,SIMTERM Instructions Pagesa14 %.,

Arrows Move through screen memory in the Wu

specified direction

PgUp Back up one page through memory

PgDn So down one page in memory

Cntr+ Toggle output to printerPrtSc

Ctrl+ Download (Unix --- > IBM)PgDn

Ctrl+ Upload (IBM --- > Unix)PgUp

9. Remote Control of SIMTERM

Certain sequences of characters, when receivedover the communications line, will cause SIMTERM to executecertain functions, like clearing the screen, movingcharacters, turning the printer on and off. In the followingdescriptions ESC is Escape (ASCII 27), and Ctrl-X denotesthe character generated by holding down Ctrl and typing X.I.e. Ctrl-A is ASCII 1. Note: in the descriptions "ESC H F"for example means ESCAPE, capital H, capital F, with NOSPACES.

ESC A "move the cursor up one lineESC B -- move the cursor down one lineESC C -- move the cursor right one spaceESC D -- move the cursor left one spaceESC E -- take SIMTERM out of INSERT modeESC F -- remotely program a function key. After

the F, send a number from 1 to 9 to indicate which key toprogram, or a 0 to program key 10. Everything following thenumeral, up to, but not including a terminating Ctrl-Z isthen stored in that function key. (See User Function Keys,above)

ESC H -- moves cursor to top left of screen(HOME)

ESC K -- clears the rest of the line, fromcursor position over


226

-" °N

°' -I3

_...--,,,. ,-,.-1-,

SIMTERM Instructions Pages15I...

ESC 3 -- clears display from cursor to and ofline, and all lines below the cursor

ESC L -- inserts a line above the cursor byrolling bottom lines down. Bottom line oftext is lost.

ESC M -- current line is deleted, lower linesscrolled up to close the gap

ESC P 1 -- turn on printer (Like Ctrl-PrtSc).Everything that now appears onscreenwill be copied to the printer

ESC P 2 -- like P 1, but printer is put inCOMPRESSED PRINT MODE.

ESC P E -- like P 1, but printer is put inEMPHASIZED PRINT MODE.

ESC P i -- doesn't turn printer on, just setsITALICS mode. So, to print in ITALICS,first turn on printer (ESC P 1), thenhave the remote computer send this

ESC P P -- turn on printer, and set intoPROPORTIONAL print mode.

ESC P 0 -- turn off printer. Screen is no longercopied to printer

ESC 0 -- Enter INSERT mode. All subsequentcharacters will be inserted before thecursor on this line

ESC R -- Deletes the character at currentcursor position.

ESC T A -- Cause SIMTERM to begin ADM3Asimulation

ESC T a Cause SIMTERM to end ADM3A simulationESC T R -- Cause SIMTERM to set up to play ROGUE,

so that arrow keys work.ESC T r -- Cause SIMTERM to end rogue playing set

upESC V S -- Cause SIMTERM to set up for vi, so

that arrows, Ins, Del, PgUp and PgDn doreasonable things in this Unix texteditor

ESC V s -- End Vi modeESC Exc -- causes character 'c' to be printed x

number of times, i.e. to print 27 B's,send ESC I ESC B, since ESC has an ASCIIvalue of 27.


227

'..., .. : j,-./.'.,,. .,.. ., . .... ................. ...... .-. . .. .-........ -. . .. .> . .>....'. . <..ii

I |I

SIMTERM Instructions Page 16

ESC &dB -- set screen attribute to a value thatequals BOLD on the printer (Actualscreen color varies from monochrome tographics monitor.)

ESC &dD -- set attribute to correspond toUNDERLINE

ESC &dH -- set attribute to be SUPERSCRIPTESC &dL -- set attribute to be SUBSCRIPTESC &dQ -- set attribute back to normalESC-y x move cursor to position y,x: where y .,

and x are the ASCII characters whichequal the desired position plus 32. I.e.to move to position 3,3 send ESC - # #,since the ASCII value of # (35) minus 32is 3

ESC -- request ID. SIMTERM will send back thestring "IBM PC" terminated by a newline.

10. XMODEM Under UNIX

When using XMODEM with a UNIX system, the programsxrecv.c' and 'xtrans.c' should be uploaded and compiled on

the host. These programs are setup for the System V releaseof UNIX and may need rework if used on other systems (e.g.,Berkeley). To send a file from the PC->UNIX, typ 'xrecvfilename' on UNIX and when the prompt comes up to starttransmission, hit Alt-T and follow the instructions.

To send a file from UNIX->PC, type 'xtransfilename' on UNIX and when-the prompt comes up to set up thereceive, hit Alt-R and follow the instructions.

If you are sending ASCII Text files, invoke the"xrecv'/'xtrans' with the "-t' argument so that conversionis done between the DOS and UNIX conventions. The XMODEMprotocol using the '-t' option is the safest way to transfertext files between systems since checks are made forsuccessful transmission and any errors introduced by noisylines will be filtered out.

Note: if the '-d' option is given to eitherprogram, the following will occur. xrecv leaves arunning commentary on the connection status in "xr.err",and xtrans leaves a corresponding file called "xt.err". They


228

- ,F' -

SIMTERM Instructions Pagt".17

are used for. debugging purposes. RO

II. Using SIMTERM with Other Modems

SIMTERM has been tailored to work with the VENTELor Hayes modem. If you are using it with another modem thenmake sure that the modem is optioned to present the 'modemready' signal. I you want to be able to 'dial' the phonenumber from the keyboard, you will also have to option it topresent the 'carrier detect' signal. SIMTERM will not getpast its initialization code unless it see both the carrierdetect and the modem ready signals.

If you are using an acoustic coupler or a 212dial up', then when the message 'establish communications'

appears, dial up the computer and enable the 'data' (e.g.,hit the DATA button on the 212 or put the receiver in thecoupler).

If you are using any modem (or are directlyconnected), and SIMTERM seems to hang right after the"Establish communications link" message, it probably meansthat your modem (or local connection) is not outputting thecorrect RS-232 signals for SIMTERM. If the equipment's notyours, or you don't understand how to fix the problem, oryou're just too lazy to bother, try invoking SIMTERM withthe -d flag. I.e. say "simterm -d" instead of just"simterm".. This will keep SIMTERM from looking at the stateof your modem, and your problem will disappear. If it stilldoesn't work, you can always give us a call.

12. Compiling the Simulator

If you look at the link control file 'simterm.lnk'you will see the programs that have to be compiled orassembled. You must use the MACRO assembler (masm). Thefile 'struct.mac' has some macros for structured assemblycode which makes it easier to write. The file 'mactest.asm'has some examples of the use of the structured macros. The•relations' between variable or constants are the same asthe conditional jumps with the 'j' removed. 'long' can beappended to the macro to cause a long jump to be generated.

com.asm' is the interface to the Async Comm line.The file 'comm.inc' has the entry points defined.


229

. .. . . . . .. f

,1411 4Y 67 7 77.. .1.77 1.-..74177 . . . ? 7 N N -. . ..V


The file 'comm.inc ° has the entry points defined. -"xxgraph.asm' has functions for cursor control and such fromPASCAL programs. The file 'graph.inc' has the entry pointsdefined.

'arglist.obj' and 'arglist.inc' provide 'argv' andargc' for the parameters to the PASCAL program. See'mainsim.pas' for examples of their use. Most of the programwill be self-explanatory (hopefully). Any questions, give mea call.

If you want to run SIMTERM on a PC with 128K, youwill have to have access to the compiler and macroassembler. Instead of re-compiling and linking the programsin simterm.lnk, use "smterm.lnk". This will build a SIMTERMwithout VenTel features and without Login Scripts, but italso knocks about 50K off the '.exe' file.

13. Funny Error Conditions

Sometimes SIMTERM will appear to lockup and notaccept any input from the keyboard (you will hear the bellindicating that the input buffer is full). This is usuallycaused by an incomplete ESCAPE sequence that has been sentto SIMTERM and it is waiting for the rest of the characters,which will probably never come from the computer. To 'breakout' of this condition, depress at the same time the 'Ctrl','Shift', and 'Alt' keys on the left side of the keyboard.This will cause SIMTERM to terminate its 'wait state' andcontinue. This sometimes happens after dialing into a BBS orother system since you might get some noise of the line.

Also the transmitter may have missed an interrupt,and what you are typing is not sent. If you suspect this,hit Alt-F7.

If SIMTERM gets into a funny condition that youcan not get it out of, then terminate it with 'Alt-Fl' andthen reinvoke it. This procedure will leave the PC attachedto the system and you can pick up where you left off.

There have been problems on some systems that usePROKEY, NEWKEY, or otr.er software that interprets thekeyboard. If SIMTERM is locking up on you and you have a


230

b..


program like this loaded, reboot the system without it andtry it. Whatever those programs are doing, they are not veryfriendly to SIMTERM.

Jim Holtman Eric Holtman %35 Dogwood Trail Watson Dorm, Room 142Randolph, N. J. 07869 Station #2(201) -361-3395 Charlottesvi lle,VA 22904-0029Net Address. ... !harpo!whuxlb!jph ... !harpo!whuxle!eric

If you add any interesting new features to theprogram, please keep me informed.

1 have several other public domain programs that Iwill be willing to trade for ones that I don't have. Dropme a note (or a floppy disk) with what you have and we cansetup a trade. Changes and improvements are continuallybeing added to the program and if you are interested in whatthe changes are, just drop me a note (or a floppy) for thelatest information.

Copyright *1982,1984 Jim & Eric Holtman

231

.S ... -%..7

- - p .- -. . . . .~- - - - - - -* S~~b 4 "'~. *. .* ., , *,.-* .

~~. ~ ~ I~~rr~ .- 37 T-IF Fy T777r j,- -s.YIW

APPENDIX J

ASSEMBLY LANGUAGE ROUTINES -IBM-PC

This Appendix contains the listings of the file

"biosio.asm" and "dostimeasm", used in the IBM-PC Kalman

Filter program to generated graphic output and to evaluate

execution time respectively. The "biosio.asm" is also used

by the plotting routine, PLOT.C *The -Files "biosio.asm" and

"dostime.asm' are part of the Public Domain Library and can

be found in the PC-SIB library disk number 216.

At!~

*4*biosic.asm *1

IBM-PC microsoft "C" under PC-DOS v2.00

; MICROSOFT "C" callable 8088 assembly routines that;interface directly with the basic I/O system (BIOS).

; NOTE -- The IBM Technical Reference Manual contains a;listing of the BIOS and more complete descriptions of each; interrupt.

; Written by L. Cuthbertson, April 1984

PGROUP GROUP PROSPROS SEBMEIT BYTE PUBLIC 'PROS'

PUBLIC BIOSINI,BIOSSET,BIOSPOSPUBLIC BIOSUPBIOSDWNBIOSRCABIOSWCABIOSWCPUBLIC BIOSWDBIOSTTYBIOSCUR *.

PUBLIC COMINI qCOMOUTCOMINCOMSTAT

PUBLIC INP,OUTP

ASSUME CStPGROUP

; ****4***********4********4*************41****4************** -

*** VIDEO I/0 ***,

NOTE - the video I/0 interrupt description starts on page;A-43 of the Tech Ref Manual.; ** * * 4 ** * * * 4 ** *.,,*N 4 * 1 ** * * * * ** N * * *

; Initialize screen I/0 using the BIOS set mode call

synopsis biosini(stype);

int stype; screen type

0 - 40x25 BW (power on default); 1 = 40x25 Color

2 = 8Ox25 BW3 = 80x25 Colorgraphics mode4 = 320x200 Color5 = 320x200 BW6 = 640x200 BW

%.4

233% %

internal use only7 - 90x25 BW card

BIOSINI PROC NEARPUSH BPPlOY DPqSPPlOY AL,CBP+43 ; SCREEN TYPE IN ALPlOY AH90 ; SET MODE FUNCTIONINT 10H ;BIOS VIDEO 1/0 INTERRUPTPOP BPRET

BIOSINI ENDP

;Set the current cursor position.

synopsis biosset~irow~icol);

*41no value returned *1

int irow; 0 to 24mnt icol; 0 to 79

BIOSSET PROC NEARPUSH BPPlOY DP,SPMOY DH,EBP+43 ; ROWPlOY DLqCBP.63 ; COLUMNPlOY DH,0 ; CURRENT PAGE NUMBERMOY AH,2 ; CURSOR POSITION SET FUNTION NUMBERINT 10H ;VIDEO I/O INTERRUPTPOP BPRET

BIOSSET ENDP

;Return the current cursor postion.

;synopsis iret - biosposo;

mnt iret; high order bits contain rowlow order bits contain column

BIOSPOS PROC NEARPUSH BPMOY BP,SPMOY BH,0 ; CURRENT PAGE NUMBERPlOY AH,3 ; CURSOR POSITION FUNCTION NUMBERINT 10H ;VIDEO I/O INTERRUPTMOV AH,DH ; MOVE INT RETURN INTO FUNCTION RETURNMOV ALDL ; DITTO

BIOSPOS ENDP

234

.I7

Read the contents of a given screen call.

synopsis irstmbiosrcao);

int iret; high order bits contain attributes; low order bits contain character

; NOTE - Attributes are defined on page 13-9 of the DOS v2.0; manual

BIOSRCA PROC NEARPUSH BPMOV BP,SPMOV BH,0 ; ACTIVE DISPLAY PAGEMOV AH,8 j READ CHARACTER + ATTRIBUTES FUNCTIONINT 10H ; VIDEO I/O INTERRUPTPOP BPRET

BIOSRCA ENDP

; Write a character to the screen - with attributes.

; synopsis bioswca(char,count,att);

*** no value returned **

int char; character to outputint count; number of times to output characterint att; character attribute

; NOTE - Attributes are defined on page 13-9 of the DOS v2.0;manual

BIOSWCA PROC NEARPUSH BPMOV BP,SPMOV AL,EBP+4] ; CHARACTERMOV CX,EBP+6] ; NUMBER OF CHARACTERS TO WRITEMOV BL,[BP+8] ; CHARACTER ATTRIBUTEMOV BH0 ; ACTIVE DISPLAY PAGEMOV AH,9 ; WRITE CHARACTER/w ATTRIBUTES FUNCTIONINT 10H ; VIDEO I/O INTERRUPTPOP BPRET

BIOSWCA ENDP

; Write a character to the screen - no attributes. ..

; synopsis bioswc(char,count);

235

| tit no value returned e

;int char; character to output; ~int count; number of times to output character

BIOSNC PROC NEARPUSH BPMOV BPSP

MOV ALq[BP+4] CHARACTERMOV CXqEBP+63 NUMBER OF CHARACTERS TO WRITEMOV BL,O; ;CHARACTER ATTRIBUTE -NULLMOV BH90 ACTIVE DISPLAY PAGE "MOV AH910 !WRITE CHARACTER ONLY FUNCTION NUMBER

INT 10H ;-VIDEO 1/0 INTERRUPTPOP BP "RET .4.

BIOSWC ENDP

Write a dot in graphics mode....

synopsis biaswd (irowt icol) ;"

*** no value returned **

int irow; c a t pint |col; niu

BIOSWD PROC NEAR

PUSH BPMOV BPSPMOV DXEBP+4] ; ROWMOV CX,EBP+6 ; COLUMNMOV AL,1 ; CREEN COLORMOV AH,12 ; WRITE A DOT FUNCTION NUMBERINT 1OH ; VIDEO I/O INTERRUPT

POP BPRET

BIOSWD ENDP

Return the current video state of the screen.

;,rt.adt ngrpi. o.

synopsis iret =dbioscuro);

int iret; low bits are the made currently set

;(see biosini for description)high bits are the number o columns on

screen

BIOSCUR PROC NEARPUSH BPMOV BPqSP

236

NOV DXCBP+4 ; RO

MOV AH, 15 ; CURRENT VIDEO STATE FUNCTION .a

INT 1OH ; BIOS VIDEO IO INTERRUPTPoP BPRET

BIOSCUR ENDP

3 **1* communications port **

; NOTE - the communications port I/0 is described starting;on page A-20 of the Tech Ref Manual.

; Initialize the communications port.

synopsis iret - comini(port.params);

int iret; return status (see comstat)int port; communications port to initialize (0,1)int params; bit pattern for initialization'-

7 6 5 4 3 2 10-BAUD RATE- -PARITY- -STOP BIT- -WORD LENGTH-

; 000 - 110 XO - NONE 0 - 1 10 - 7 BITS001 - 150 01 - ODD 1 -2 11- 8 BITS

; 010 - 300 11 - EVEN011 - 600

; 100 - 1200101 - 2400110 - 4800111 - 9600

COMINI PROC NEARPUSH BPMOV BP,SPMOV DX,EBP+4] ; COMM PORT TO INITIALIZEMOV AL,CBP+63 ; PARAMETERSMOV AH,0 ; INITIALIZATION FUNCTIONINT 14H ; COMM PORT I/O INTERRUPTPOP BPRET

COMINI ENDP

; Write a character to the communications port.

; synopsis iret = comout(port,c);

237

°A °*

int iret; error return C >127 if error occured)- see AH under modem control

int port; communications port to write (O or 1)int c; character to write.

COMOUT PROC NEARPUSH BP Is

MOV BPSPMOV DX,CBP+4] ; COMM PORT TO WRITEMOV AL,[BP+6] ; CHARACTER TO WRITEMOV AH91 ; WRITE CHARACTER FUNCTIONINT 14H ; COMM PORT I/O INTERRUPTPOP BPRET

COMOUT ENDP

Read a character from the communications port.; Waits for character if one is not ready.; See \comm\ibmtty.c for example of polling comm port

for character without wait.

; synopsis c = comin(port);

int c; character read from comm port,> 127 if error or no character ready.- see AH under modem control

int port; communications port to read (0 or 1)

COMIN PROC NEARPUSH BPMOV BP,SPMOV DX,CBP+4] ; COMM PORT TO READMOV AH,2 ; READ CHARACTER FUNCTIONINT 14H ; COMM PORT I/O INTERRUPTPOP BPRET

COMIN ENDP

Check the line and modem status

; synopsis iret - comstat(port);

int iret; ; line and modem statusAH - high order bits contain line control status

; bit 7 time outbit 6 -trans shift register emptybit 5 - trans holding register emptybit 4 -break detect

* ; bit 3 - framing errorbit 2 -parity error

*. 23e

0*?"' V e. '.. " € • , " < '< ' "N" " ".".. "."'' ".",""." "/ ",""."'"." "'"'.',""'. "'

KA-if -r~roramrynp-.7-17 wnrru-17"J WV nw-1 r

I bit 1 -. overrun errorbit 0 - data ready

AL -low order bits contain modem statusbit 7-received line signal detectbit 6 - ring indicatorbit 5 -data set ready 'v

bit 4 - clear to sendbit 3 - delta receive line signal detectbit 2 - trailing edge ring detectorbit 1 -delta data set readybit 0=- delta clear to send

;Note - from page A-21 of Tech Ref Manual

mnt port; ;communications port to check

C~OMSTAT PROC NEAR.PUSH BP

MOY DP,EP+4 COMM PORT TO CHECK

MOV AH,3 ; STATUS FUNCTIONINT 14H ;COMM PORT 1/O INTERRUPTPOP BPRET

COMSTAT ENDP

**io.asm *

;IBM-PC 9089 assembly for interface with microsoft "C";under PC-DOS

;"C" callable subroutines that provide absolute pointer; addressing.;Pointers within a IBM-PC microsoft "C" program are;relative to the start of the programs data segment. These;subroutines provide a mechanism to address absolute memory; locations.

;Supplied by microsoft - commented by L. Cuthbertson, April1984

;Read an absolute memory location.

;synopsis c =readabs(loc);

239

int c; contents of memory locationint loc; absolute memory location in hex

INP PROC NEARPUSH BP &?,o".MOV BP,SPMOV DX,CBP+4] ;SET LOCATION ADDRESSIN ALqDX ;READ LOCATIONXOR AH,AH ;CLEAR HIGH BYTEPOP BPRET

INP ENDP

; Write an absolute memory location.

synopsis writeabs(loc,c);

int loc; absolute memory location in hexint c; integer to write to memory location

OUTP PROC NEARPUSH BPMOV BPSPMOV DX,rBP+4] ;GET LOCATION ADDRESSMOV AXEBP+6] ;SET ADDRESS OF INTEGER TO WRITEOUT DX,AL ;WRITE TO MEMORY LOCATIONPOP BPRET

OUTP ENDP ' S.

PROS ENDSEND

*** dostime.asm **

; IBM-PC microsoft "C" under PC-DOS

; Microsoft "C" callable 8088 assembly routine that uses a;DOS function call to obtain and return the hours, minutes,;seconds, and hundreths of a second as integers.

; synopsis dostime(&hour,&min,&sec,&ths);

***4 no function value returned **

int hour; 0-23 military timeint min; 0-59int sec; 0-59int ths; 0-99

240 "-

~ '.~ 4 ~ gd*.P. J~V * .o*ooo S' o

C P S. p~~ "4p .. ,',** . . .

I Written by L. Cuthbortson, May 1984I

PGROUP GROUP PROSPROS SEGMENT BYTE PUBLIC 'PROS'

PUBLIC DOSTIMEASSUME CS. PGROUP

DOSTIME PROC NEARPUSH BPMOV BPSPMOV AH,2CH ; DOS SET TIME FUNCTION NUMBERINT 21H DOS FUNCTION CALL INTERRUPTMOV AL,CH ; MOVE HOURS INTO AXXOR AHAH ; CLEAR HIGH BYTEMOV DI,[BP+4] ; ADDRESS OF HOURS VARIABLEMOV ED13,AX ; MOVE HOURS INTO HOURS VARIABLEMOV BLCL ; MOVE MINUTES INTO BXXOR BHBH ; CLEAR HIGH BYTEMOV DIEBP+6] ; ADDRESS OF MINUTES VARIABLEMOV ED13,BX ; MOVE MINUTES INTO MINUTES VARIABLEMoV CL,DH ; MOVE SECONDS INTO CXXOR CHCH ; CLEAR HIGH BYTEMOV DI,cBP+8] ; ADDRESS OF SECONDS VARIABLEMOV EDI],CX ; MOVE SECONDS INTO SECONDS VARIABLEXOR DH,DH ; CLEAR HIGH BYTEMOV DIIEBP+1O] ; ADDRESS OF THS VARIABLEMOV CDIJDX ; MOVE 1/IOOTHS INTO THS VARIABLEPOP BPRET

DOSTIME ENDP

PROS ENDSEND

241

'-

r

241 '

'-o.°

'52.

LIST OF REFERENCES

1. O'Brien, Paul A., An Application of Kalman Filtering toUnderwater Tracking, Master's Thesis, Naval PostgraduateSchool, Monterey, California, September 1980. *

2. Kirk, Donald E., Optimal Estimation: An Introduction tothe Theory and Applications, EE3310 Class Notes,Unpublished.

3. Bozic, S. M., Digital and Kalman Filtering, EdwardArnold (Publishers) Ltd. 1981.

4. Hunter, Bruce H., Understanding C, Sybex Computer

Books, 1984.

5. Jamsa, Kris, The C Library, Osborne McGraw-Hill, 1965. -

6. Kernighan, Brian W. and Ritchie, Dennis M., The CProgramming Language, Prentice-Hall, Inc., 1978.

7. Schustack, Steve, Variations in C, Microsoft Press,1985.

8. IBM, Technical Reference for the IBM Personal Computer,Personal Computer Hardware Reference Library, 1983.

9. Norton, Peter, The Peter Norton Programmer's Guide tothe IBM PC, Microsoft Press, 1995.

10. Peerless Engineering Service, Fortran ScientificSubroutine Library, John Wiley & Sons,Inc, 1984.

11. CIE Systems, Regulus Reference Manual, BussinessComputer Operating Systems Reference Series, BussinessComputer Software Library, 6 DR8-0012-001, 68 DOC-3041,Second Revision.

242

INITIAL DISTRIBUTION LIST

No. Copies1. Defense Technical Information Center 2

Cameron StationAlexandria, Virginia 22304-6145

2. Library, Code 0142 2Naval Postgraduate SchoolMonterey, California 93943-5002

3. Department Chairman, Code 62 2Department of Electrical and Computer

EngineeringNaval Postgraduate SchoolMonterey, California 93943-5000

4. Professor H. A. Titus, Code 62Ts 5Department of Electrical and Computer

EngineeringNaval Postgraduate SchoolMonterey, California 93943-5000 .

5. Associate Professor A. Gerba, Code 62Gz 1Department of Electrical and Computer

EngineeringNaval Postgraduate SchoolMonterey, California 93943-5000

6. Commanding Officer 2Naval Underseas Weapons Engineering StationKeyport, Washington 98345

7. Capt. Jose A. Fernandes 2CTA - IAE - ESB ". .12200 - SAO JOSE DOS CAMPOS - S.P.BRAS IL

2 3 .0.

243

. . .. . . . . . . . *> --

7 S. - -"-. '--- .~. ----. ,-..-.--...-0-

-07

... ......

Documents

USING NICROPROCESSORS V3 71 KLMN FILTER REL AND A MEW ... · eficiency, and portability are emphasized in the software design. 4. The program was tested, using simulated trajectories,