znaval postgraduate school · znaval postgraduate school monterey, california-- ,. % ~,.,...

zNAVAL POSTGRADUATE SCHOOLMonterey, California

-- ,. %

~,., -

FIEDTHESIS

FXDPOINT SMOOTHING ALGORITHM TO THETORPEDO TRACKING PROBLEM

by

Sadi Karaman 04

JUN 1988

*j~ %.'

Thiesis Advisor: H. A. TIUS

Approved for public release;distribution unlimited.

86 11 14 055

THESIS * S:

SECURITYv CLASSIFICATION1 OF THIS 'AGE

REPORT DOCUMENTATION PAIGEla REPORT SECURITY CLASSiFICATION 1b. RESTRICTIVE MARKINGS

2a SECURITY CLASSIFICATION AUTHORITY 3 DISTRIBUTION JAVAILABILITY OF REPORT

2b. DECLASSI FICA TION / DOWNGRADING SCHEDULE A:)-roved for Public' release;distri'bu-tion is unli;mite.-

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

%

6a. NAME OF PERFORMING ORGANIZATION 6b OFFICE SYMBOL 7a. NAME OF MONITORING ORGANIZATION(if apicable)

Naval Postgraduate School 62 Naval Postgraduate School6c. ADDRESS (City, State. and ZIP Code) 71b, ADDRESS (City, State, and ZIP Code)

Monterey, CA 939'43-5000 Monterey, CA 93943-5000-

Ba N4AME OF FUNDING iSPONSORING 8b. OFFICE SYMBOL 9 PROCUREMENT INSTRUMENT IDENTIFICATION N4UMBERORGANIZATION j(if appicable)

SC ADDRESS (City, State, and ZIP Code) 10 SOURCE OF FUNDING NUMBERSPROGRAM IPROJECT ITASK( IWORK .JNIT .-

ELEMENT NO NO NO ACCESSION NO%

1TITLE (~iclude Security Classification) IFIXED POINT SMOOTHING ALGORITHM TO THE TORPEDO TRACKING PROBLEM%

-PERSONAL AUTHOR(S) Sd aaa

3a TYPE OF REPORT 13b TIME COVERED 14 DATE OF REPORT (Year, Month. Day) 15 PAGE COiNT 'kJ

The_________ IFRMT _19_,_Jun

* 6SUPPLEMENTARY NOTATION

COSArI CODES I8 SUBJECT TERMS (Continue on reverse if necessary and identify by block number)?ELD GROUP SUB-GROUP Fixed Point Smoothing, Tornedo Tracking,

Sequential Extended Kalman Filter

'9 ABSTRACT (Continue on reverie if nlecessaryf and identify by block number)

A sequential extended Kalman filter and optimal smoothing a-.r.;m asdeveloped to provide real time estimates of torpedo position and de7ath- 't-he three dimensional underwater track -ing range at the Naval Tor _ _ r'Keyport, Washington. The measurements consisted of acoustic pulse -_rarsi-t

tie rmthe torpedo to the receiving array, which are nonlinear -:nc-I sof the :oositions and the diepth of the torpedo, were linearized an_ egains and filtered estimates o stes cal-culated. By running -hesmon :

subroutine, all pas: filtered estimates of states and error covarian:e :'-

smoothed. The program was tested, usingr simulated torpedo tra~ec-jr-es tharaversed both'single and multiple arrays, on an IBM,-PC. The resui'-E35.~-ha- ie performance was denendent on system noise ar-d thne dis-an:9--he hdrophone array from the torpedo and the smoothed est imates c --~nd error covariances were bet ter than or ecua 1 toc the fi!-:ered -- :-

-rS'13UTION/AVAILABILiTY OF ABSTRACT 21 ABSTRACT SECURITY CLASSIFICATIONJ C.ASSIF/ED/tJNLIMITIED 0 SAME AS RPT 0 DTIC UJSERS

- .AMAE OF RESPONSIBLE !NDIVIDUAL 2213 TELEPHONE (Include Area Code) 22c CFFL(E S VBG.

* rof . -arol' A. (,D) - '-DO FORM 1473. 84 MAR 83 APR edition m~ay be used until exmausted SECURITY CLASSi9;CAr'CN ; -. ~ .SC

All otImer editiom are obsolete1%

Approved for public release; distribution is unlimited.

Fixed Point Smoothing Algorithm to the Torpedo TrackingProblem. -

by

Sadi Karaman -

LTJG., Turkish NavyB.S., Turkish Naval Academy, 1979

Submitted in partial fulfillment of therequirements for the degree of

MASTER OF SCIENCE IN ELECTRICAL ENGINEERING

from the

NAVAL POSTGRADUATE SCHOOLJune 1986 -

-' / /~* "* " °

Author: _ _.-..-._Sadi, Karaman .

/~~~? •'-- ,.'.*,'

Approved by: _ " _-_ _ _ _ _ _ _ _

H.A. Titus, Thesis Advisor

Alex Gerba, Second Reader

H.B L -ca, of ..

H.B. Rigas, Chair parlmnt ofElectrical and Computer Engineering

a. *% *

AVA

J.j. Dye', Dean of Scienceand Engineering

" 4-.'"2

,...

%.

V ..

%ABSTRACT

%

A sequential extended Kalman filter and optimal

smoothing algorithm was developed to provide real time

estimates of torpedo position and depth on the three k

dimensional underwater tracking range at the Naval Torpedo

Station, Keyport, Washington. The measurements consisted of

acoustic pulse transit times from the torpedo to receiving

array, which are nonlinear tunctions of the positions and

the depth of the torpedo, were linearized and filter gains

and filtered estimates of states calculated. By running the .,

smoothing subroutine, all past filtered estimates of states

and error covariance were smoothed. The program was tested,

using simulated torpedo trajectories that traversed both i

single and multiple arrays, on an IBM-PC. The results showed

that filter performance was dependent on system noise and

the distance to the hydrophone array from the torpedo and

the smoothed estimates of states and error covariances were

better than or equal to the filtered estimates.

C,

,..A -,\i

i 4.•o- - . • °*' . . 4- 4- 4 4. o. 4- .- .. .. + o 4

Fi-J -. - -. -J IT '7. - .7- - --- .V. p. )

TABLE OF CONTENTS

I. INTRODUCTION .................... 14

II. DESCRIPTION OF RANGE TRACKING GEOMETRY ............. 15

I I I . THEORY ............................................ 7

A. EXTENDED KALMAN FILTER ....................... 17

B. OPTIMAL SMOOTHING ............................ 20

IV. PROBLEM DEFINITION ............................... 22

A. OBSERVATION AND PLANT STATE EQUATIONS........ 22

B. DEFINITION OF MULTIPLE ARRAY TRACKING ......... 27

C. SEQUENTIAL EXTENDED KALMAN FILTER ............ 30

D. OPTIMAL SMOOTHING ALGORITHM .................... 33

V. SIMULATION RESULTS ................................. 35

A. MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN ...... 35

B. MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN ......... 36

C. SINGLE ARRAY ADAPTIVE MANEUVERING RUN ......... 37

D. SINGLE ARRAY ADAPTIVE STRAIGHT RUN ............. 38

VI. CONCLUSIONS ....................................... 39

FIGURES ..................................................... 41

APPENDIX A: PROGRAM DESCRIPTION........................ 86

A. GENERAL ............................... 86

B. RUNNING THE PROGRAM ON THE IBM-PC ..... 86

APPENDIX B: SEQUENTIAL EXTENDED KALMAN FILTER AND

OPTIMAL SMOOTHING PROGRAM LISTING ........... 88

4.4

-4. .4°-

4-.,

- . - .7 .T Z R Ti 4T W-W'. w * %]

APPENDIX C: PLOTTING PROGRAM LISTING FOR HP PLOTTER... 104

APPENDIX D: PLOTTING PROGRAM LISTING FOR MONITOR ...... 109

APPENDIX E: BATCH FILES ............................... 113 ,77

LIST OF REFERENCES ....................................... 115

INITIAL DISTRIBUTION LIST ................................ 116 .i

S:.-

S..y

,%.'/5

Si5

.. %

.9'..,

S.' .. .

-a""''

,.,4. " ," " " ," , A' , ', * , , , , , . .•.- ,'-, -. "..-," " .. ' .. '-. ', ' .. ' -. ' -.. :

LIST OF FIGURES

2.1 Geometry of a Tracking Array ........................ 16

3.1 Advantage of Performing Optimal Smoothing.

Mean Square Estimation Error vs. Time ............. 21

3.2 Three types of smoothing: (a) fixed-interval, _

(b) fixed-point, (c) fixed-lag smoothing .......... 21

4.1 Geometry of Multiple Array Tracking: (a) Coordinate

System, (b) Hydrophone Location Matrix ............ 29

5.1 Multiple Array Adaptive Maneuvering Run. #1

Y vs. X , with noise .......................... 41k/k k/k


Yk/N vs. X k/N, ',ith noise .......................... 41


Xk/k vs. Time Slots, with noise................... 42


k NX vs. Time Slots, with noise ................... 42k/N

5.5 Multiple Array Adaptive Maneuvering Run. #1 -.

Y k/k vs. Time Slots, with noise ................... 43

5.6 Multiple Array Adaptive Maneuvering Run. #1 -

k/N vs. Tii,,e Slots, with noise .....................43


Z vs. Time Slots, wjith noise..................... 44k/k v.Tm


.I.%

k/k vs. Time Slots, with noise .................... 44

5.9 Multiple Array Adaptive Maneuvering Run. #1P k/k(1,1) vs. Time Slots, with noise ............... 45

6

i

5. 10 Multiple Array Adaptive Maneuvering Run. #1Pk/N (1,1) vs. Time Slots, with noise.............. 45

5.11 Multiple Array Adaptive Maneuveri ng Run. #1P klk(2,2) vs. Time Slots, with noise .............. 46


Pk/N (2,2) vs. Time Slots, with noise .............. 46

5.13 Multiple Array Adaptive Maneuvering Run. #1 -P k/k(3,3) vs. Time Slots, with noise ............... 47

5.14 Multiple Array Adaptive Maneuvering Run. #1 ..,-'Pk/N(3,3) vs. Time Slots, with noise ..............

47


P (44) vs. Time Slots, with noise ............... 4B

k/k

5.14 Multiple Array Adaptive Maneuvering Run. #1P kN(3,3) vs. Time Slots, with noise................478.-

%5.17 Multiple Array Adaptive Maneuvering Run. #1 J

P (4,5) vs. Time Slots, with noise................ 49

k/k


Pk/N(5,4) vs. Time Slots, with noise .............. 46


P k/k(5,5) vs. Time Slots, with noise................49..

5.18 Multiple Array Adaptive Maneuvering Run. #2 '..S.kPN(5,5) vs. Time k/Nlots, with noise ..............49 . .

5.19 Multiple Array Adaptive Maneuvering Run. #2. ?

Y vs. X with noise ........................ 50

~k/k vs k/k5.20 Multiple Array Adaptive Maneuvering Run. #2

• vs. Xk/N , with noise ......................... 50

k/N /5.21 Multiple Array Adaptive Maneuvering Run. #2

" k/k vs. Time Slots, with noise .................. 51


Xk/N vs. Time Slots, with noise ................... 51

5.23 Multiple Array Adaptive Maneuvering Run. #2 "-p

Yk/ vs. Time Slots, with noise...........-'.-...5

% ..

7 .'.-

":•--


Y vs. Time Slots, with noise ................... 52k/N


/ vs. Time Slots, with noise ................... 53k/k

5.26 Mul-iple Array Adaptive Maneuvering Run. #2

Z vs. Time Slots, with noise .................... 5.'. .k/N

5.27 Multiple Array Adaptive Maneuvering Run. #2-Pk/k(1,1) vs. Time Slots, with noise .............. 54


P / 11 vs. Time Slots, with noise................ 54

5.29 Multiple Array Adaptive Maneuvering Run. #2P k/k (2 ,2) vs. Time Slots, with noise............... 5

5.30 Multiple Array Adaptive Maneuvering Run. #2. (~2 ,2 ) vs. Time Slots, with noise.............. 55

5.31 Multiple Array Adaptive Maneuvering Run. #2Pk/N(3,3) vs. Time Slots, with noise ............... 56

5.32 Multiple Array Adaptive Maneuvering Run. #2Pk(3,3) ;vs. Time Slots, with noise .............. 56

5.3 Multiple Array Adaptive Maneuvering Run. #2Pk/k(4 ,4) vs. Time Slots, with noise ............... 57 A

5.34 Multiple Array Adaptive Maneuvering Run. #2P (4,4) vs. Time Slots, with noise ............... 57

5.35 Multiple Array Adaptive Maneuvering Run. #2Pk/N(5,S) vs. Time Slots, with noise .............. 58

5.36 Multiple Array Adaptive Maneuvering Run. #2'

Pk/k(5,5) vs. Time Slots, with noise .............. 58

5.37 Multiple Array Adaptive Straight Run.

Y k/N vs. X Sos with noise........................ 59

',.,,

5•5 Mlil ra Aatv.aevrngRn 2' .

Yk/kvs.Xk/k , .th n is.*.* . .... .... ......... 59 -. ".

- .:.- . .-. ,


YRkN k/N with noise .......................... 59


Xk/k vs. Time Slots, with noise O...................60


X k/N vs. Time Slots, with noise ..................... 60


Yk/k vs. Time Slots, with noise .................... 61


Y vs. Time Slots, with noise................... 61k/N


Z vs. Time Slots, with noise ................... 62k/k


Zk/N vs. Time Slots, with noise. ....................62

5.45 Multiple Array Adaptive Straight Run.P (1,1) vs. Time Slots, with noise .............. 63k/k


P (1,1) vs. Time Slots, with noise.............. 63k/N

5.47 Multiple Array Adaptive Straight Run.P k/k(2,2) vs. Time Slots, with noise .............. 64

5.48 Multiple Array Adaptive Straight Run.Pk/N (2,2) vs. Time Slots, with noise .............. 64

5.49 Multiple Array Adaptive Straight Run. . .

P k/k(3,3) vs. Time Slots, with noise .............. 5

5.50 Multiple Array Adaptive Straight Run.P k/N(3,3) vs. Time Slots, with noise.............. 65

9

. . . . . . . . . . . . .. . - . . . . . - - ° . . o ':

5.51 Multiple Array Adaptive Straight Run.P k/k(4,4) vs. Time Slots, with noise ............... 66

5.52 Multiple Array Adaptive Straight Run.P k/N(4,4) vs. Time Slots, with noise ............... 66


Pk/k (5,5) vs. Time Slots, with noise ............... 67

5.54 Multiple Array Adaptive Straight Run.Pk/N (5,5) vs. Time Slots, with noise ............... 67

5.55 Single Array Adaptive Maneuvering Run.

Yk/k vs. Xk/k , with noise ........................ 68


Y vs. X ,with noise ........................... 684.k/N k/N


X vs. Time Slots, with noise ..................... 69k/k .5.,


X vs. Time Slots, with noise ................... 69k/N


Y vs. Time Slots, with noise ..................... 70k/k


Y vs. Time Slots, with noise ..................... 70k/N 4


Zk/k vs. Time Slots, with noise ..................... 71


k/N vs. Time Slots, with noise .................... 71

5.63 Single Array Adaptive Maneuvering Run.P (l11) vs. Time Slots, with noise .............. 72

k/k

0'

10 -'1,

.4,?i~

5.64 Single Array Adaptive Maneuvering Run.P (1,1) vs. Time Slots, with noise ................. 72k/N

5.65 Single Array Adaptive Maneuvering Run. __

Pk/k (2,2) vs. Time Slots, with noise ............... 73

5.66 Single Array Adaptive Maneuvering Run.4 P (2,2) vs. Time Slots, with noise ............... 73

k/N

5.67 Single Array Adaptive Maneuvering Run.P (3,3) vs. Time Slots, with noise .............. 74

k/k5.68 Single Array.Adaptive Maneuvering Run. .-

P (3,3) vs. Time Slots, with noise ............... 74k/N

5.69 Single Array Adaptive Maneuvering Run.P (4,4) vs. Time Slots, with noise .............. 75k/k

5.70 Single Array Adaptive Maneuvering Run.P k/N (4,4) vs. Time Slots, with noise .............. 75

5.71 Single Array Adaptive Maneuvering Run.P (5,5) vs. Time Slots, with noise ............... 76k/k

5.72 Single Array Adaptive Maneuvering Run.P (5,5) vs. Time Slots, with noise...............76k/N

5.73 Single Array Adaptive Straight Run.

y k/k vs. Xk/k , with noise ......................... 77 .'


Yk/N vs kN with noise........................ 77


Xk/k vs. Time Slots, with noise................... 78


Xk/N vs. Time Slots, with noise ................... 78


Y vs. Time Slots, with noise ................... 79k/k

11 "'=

**-...-.'.--"

-74


k vs. Time Slots, with noise ................... 79k/N


Z vs. Time Slots, with noise ................... 80k/k


Z k/N vs. Time Slots, with noise .................... 80

5.81 Single Array Adaptive Straight Run.p (1,1) vs. Time Slots, with noise ............... 81k/k

5.82 Single Array Adaptive Straight Run.p k/(1,1) vs. Time Slots, with noise ............... 81k/N


Pk/k (2,2) vs. Time Slots, with noise.............. .2

5.84 Single Array Adaptive Straight Run.NP (2,2) vs. Time Slots, with noise.............. 82

5.85 Single Array Adaptive Straight Run.P (3,3) vs. Time Slots, with noise...............83k/k

5.86 Single Array Adaptive Straight Run.P (3,3) vs. Time Slots, with noise ............... 83k/N

5.87 Single Array Adaptive Straight Run.P (4,4) vs. Time Slots, with noise ............... 84k/k


p (4,4) vs. Time Slots, with noise .............. 84. ..k/N

5.69 Single Array Adaptive Straight Run.P (5,5) vs. Time Slots, with noise ............... 85k/k

5.90 Single Array Adaptive Straight Run.P k/N (5,5) vs. Time Slots, with noise .............. 35

.144... 12

A .% .

ACKNOWLEDGEMENT

I would like to express my gratitude to Professor Hal . 4

Titus for his professional guidance, assistance and

encouragement during the course of this research. I would

also like to thank to Professor Alex Gerba for his help and

suggestions.

Finally, I want to thank my wife, Maria, for her

patience and support, and my father Sait and my mother

Zeynep from whom I inherited the desire for education.

,r. °'-°

.q ~~ ~ ~~ ~~~ .... . .. . . . . . . . .. -

~NN.

I. INTRODUCTION

The Naval Torpedo Station at Keyport, Washington

currently operates two three-dimensional underwater tracking

ranges utilizing a sonar transmitter installed in the

torpedo to be tracked. The transmitter is synchronized with

a master clock. Timed acoustic pulses are received by

hydrophone arrays and then relayed via cable to a computer

at the observation site. The computer calculates the

positional coordinates of the torpedo and plots its

trajectory through the water.

The measured data, which consist of the elapsed time

from transmission of a* pulse until its receipt at the

hydrophone array, is corrupted with noise due to combined

r,+fects of environmental factors and measurement

instruments.

The intention is to implement and test a sequential

extended Kalman filter and smoothing routine which processes

the transit times of the acoustic pulses and generates the

filtered and smoothed estimates of the positions of tracked

torpedo at a particular time. The design takes into account

the elimination of the storage problem.

41

147

D-4

• p-r.-',i.., , '.-. .. , -.. ,' 'i : '"' L '

II. DESCRIPTION OF RANGE TRACKING GEOMETRY -.

The hydrophone array, consisting of four independent ...-

elements, defines an orthogonal coordinate system in whi.ch .,",

transit time measurements are made. As shown in Figure 2.1i, :

four hydrophones X, Y1 Z, and C are on four adjacent .

vertices separated .-by a distance d, along the edge of the ..

cubs. The origin of the array coordinates is at the center

of the cube with the orthogonal coordinates parallel to its-..

edge. Positional information is computed from the transit" -

times of a periodic synchronous acoustic signal traveiing -..

from the torpedo to the four hydrophones an the array. The "'

.torpedoes are equipped with sohar transmitters which are ..

transmitting an acoustic signal in every 1.31 seconds,

within a range accuracy 3 to 30 ft. When tracking by...

multiple arrays, the signal from the closest hydrophone -

array is defined as the basis for the time measurements and-.,

for the range calculations. A more detailed description of i

the range tracking capability is described in [Re+. 1, 2]."-""

'15

:'-.

*1 ________________-"________

-'~~W -~. J.-. .. .- -x4 v-4 -V - -. -J -~J -, .jJ -* ~ Y - ,:

x ,vINY

*-. *.I

Z)r

/ -..'-I

/ / _-I

-1~ / .

T

i/ I /? S*:

/ II , .--. / *4 ...*:

TT

R YDROPHCNE Z(-d/2,--d-/2,d/2'

d/

HYDROPHONE C -

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

* HYDROPHONE y .

*HYDROPEONE X(d/2,-d /2

-d/ 2)

.4'.:p...:,

Figure 2.1 Geometry of a Tracking Array

16

.p.

-p . .: U ' ._ . . .x : : ; . , -, , ' -t ; : 5 ; : , + . < - : ; .. -, : , . - ; : .; ; . : -: .. : : , ; , ; - < .: - . - , - ., , - , ; .: ., - -_ , . ; - -

III. THEORY

A. EXTENDED KALMAN FILTER*'-

The basic idea of the extended Kalman filter is to

relinearize about each estimate X(k/k), once it has been

computed. As soon as *a new state estimate is made, a new and

better reference state trajectory is incorporated into the

estimation process. [Ref. 3, 4, 53

For the three-dimensional location problem three

position states (X, Y, Z) and two velocity states (V X, Vy

specify target motion. The discrete linear and nonlinear

observation equations are given by

X(k+1) = .XWk + r . W(k) (3.1)

and 1...

Z(k) - h(X(c) , k) + VWk (37.2)

where: and r are constant matrices;

%I?~

h is a nonlinear function of the state X

W(k) is the plant excitation noise;

VWk is the measurement noise.

*In these equations the plant noise and measurement noise are

assumed uncorrelated (white) with zero mean. That is,___

TEW(k).W (j)3 Q*(k & kj

17

I.

S. S.. .

Pp T

EEV(k).V T i) ] R(k)kj

where: & = 1, k = j;

0, k j.

In order to apply the linear filter, Equation .2 is

expanded in a Taylor series about the best estimate of the

state at that time and only the first order terms are kept.

Equation 3.2 gives

Z(k) = H(k) . X(k) + V(k) (3.5)

where *~)=Xkk-1

H(k) = -- X- (3.4)!X(k) = X(k/k - 1) '

X(k/k - 1) is a predicted value of the state at time k,

given the measurements until time k-i.

A state error vector is defined by

X(k/k) = X(k/k) - X(k),

and a predicted state error vector is defined by

X(k/k - 1) = X(k/k - 1) - X(k). ..,'.

The covariance of state error matrix is defined by

P(k/k) = EEX(k/k).X (k/k)],

the predicted covariance of state error matrix is given by

ATP(k/k - 1) = EEX(k/k - 1).X (k/k 1- )].

1-3

The state excitation matrix is given by

T T0(k) - r.EEW(k).W (k)3.r

and the measurement noise covariance matrix is

R(k) -EEV(k).VT W3).

The Kalman filter equations are given by [Re+. 3, 4, 5):

P0klk ~kk (k) (3.5)

G(k)-P(Ic/k-l)H (k)[H(k)P(k/k-l)HT -1+~ 3 (36

P(k/k) - CI G (k) H(k)3 P(k/k-1) (k)+Ik. 7)

A,

X (k+1 /k) X(k/k) (3.8)D

Z(k/k-1) =h(X(k/k-l), k) (39

X(k/k) =X((k/k-1) + G(k) CZ(k) -Z(k/k-1)J (3.10)

The 0 matrix serves not only to allow for maneuvering

but also to account for any model inaccuracies, that is, any

discrepancies between the true action of the torpedo and its

characterization by Equation 3.1. The 0 matrix also serves

to prevent the gain matrix G(k) from approaching zero by

always insuring uncertanity in the predicted covariance of .,

error matrix P(k+l/k) [Re+. 1, 3, 4, 5).

19

Ad

'ell'

B. OPTIMAL SMOOTHING

Smoothing is a non-real time data processing scheme that

uses all measurements between 0 and N to estimate the state

of a system at certain time k, where 0 < k .< N. The smoothed '.-.-

estimate of X(k) based on all measurements between 0 and N

is denoted by X(k/N). The smoothed error covariance is

denoted by P(k/N) and P(k/N) < P(k/k) means that the

smoothed estimate of X(k) is at least as good as the

filtered estimate or equal to its filtered estimate for all

the time, except the terminal time. This is shown "' --

graphically in Figure 3.1. As portrayed in Figure 3.2, there

are three classes of particular interest because of their

applicability to realistic problems [Ref. 3, 4, 53. One is

the Rauch-Tung-Striebel form, which was chosen in our

particular problem [Ref. 6, 73.

The smoothed state estimate and the smoothed error

covariance matrix are given by

X(k/N) = X(k/k) + A(k)[X(k+I/N)- X(k+I/k)] (3.11)

X(k+l/k) = X(k/k) (3. 12)

4 TP(k/N)=P(k/k)+A(k)[P(k+l/N)-P(k+ /k)JA(k) (3.13)

whereT 1'

A(k) = P(k/k) T Pl(k+l/k) for k N.

20

.-. 4-:,.4o° ..

-. o"4

.4'I -

.4% .-

.4 f -

Nican square estimahion error W

Backward )filter/Forward filter Pj:

Figure 3.1 Advantage of Performing Optimal Smoothing *

Data available from whole interval

(bo Qt,,.

Estimats as one time of interes

If

Growing length of data

to ,

Data

interval-'

Figure 3.2 Three types of smoothing: (a) fixed-interval,

(b) fixed-point, (c) fixed-lag smoothing.

21. 0

%'%

a..-

r -7: ' -. '7"

IV. PROBLEM DEFINITION

A. OBSERVATION AND PLANT STATE EQUATIONS

In the torpedo tracking problem, the non-linear

observation equations are the four independent transit times

from the tracked torpedo to the hydrophones, T , T , r andVJ. c x y

T .Thus the non-linqar measurement matrix is defined by~-jI 2

TZ(k) C T (k) *T (k) T (k) T (k)] 3+ V(k) (4.1)- C x y Z

where

1 2 2 2 L12Wk=7-Xk)d2 +(Y(k)4-d/2) 4-(Z(k)+d/2) I

Lr ()--(X (k)-+d/2) (Yk4d2 +Zk)/) I

.1P.

1 2 2 2 1/2T (k)=_-;j(X(k)-d/2) +(Y(k)-d/2) +(Z(k)+d/2) I

1"W 2 (Y k -/ )2 +2 1/2

T (k)=---C(X(k)+d/2) +(Y(k)4-d/2) 4-(Z(k)-d/2) 3z vel

Since the transit times are readily available and

non-linear functions of position, these equations can be

linearized and Kalman filter theory applied using the

extended Kalman filter. This procedure produces a real time

A..

* W.,

filtering an the transit times T ,T ,T and T, without

Equation 3.4 can be used to give the linearized

4observation matrix. When the derivatives are taken and

evaluated at the predicted state values X(k/k-1) =X*(k) the

result is

X (k) + d/2 Y +k 4 d/2 V (k) + d/2'deldenl denl

X (k) - d/2 Y (Idh +- d/2 V -(k) 4- d/2'I e2den2 den2

X (k) + J/2 Y X (k) - d/2 V (k) +- d/2vel--------------------* den3 denS den3

X (k) +- d/2 'C (kj) +- d/2 Z (k) - d/2den4 den4 den4

where:

62 2 2 1 /26',deni(C(X*(k)-d/2) +(Y* (k)4-d/2) 4-(Z*(k)4-d/2) I

2 2 2 1/2den2=C(X (k)-d/2) +(Y*(k)4-d/2) 4-(Z (k)4-d/2) I

2 2 2 L/2den4=t(X (k)+d/2) 4-(Y (k)-d/2) 4-(Z*(k)-d/2) J 'e I

Fr %

k22

~ 2

-~0 0 00

The plant state equations are

X(k+l) X(k) +~ T V (k) +g

-~V (k+i) 'I (k)+g

Y(k+1) = ~ ) k (4.2)

V Y(k) * V (k)k+ g y -)

Z(k+1) Z(k) + g.* I 5

where X(k), Y(k) and Z(k) are the position coordinates of

the torpedo at time t(k), V X(k) and V (k) are the X and Yx y

components o+ the velocity.

24

.- 7W7 77- 7 7-

The excitation terms g1 through g5 are included to take4,5

V into account the random changes in speed (r~) headingVt

)Y and depth (y ), which are assumed to be independent,Z~

zero mean, rates of changes. Typical maneuvering parameters

for the torpedo are given in [Re+. 83.

22Sbt= 22 O/sec; 6 =Ely J

42 2 26ft/sec E IE

t t Yt

1 ft Isec; I El IE4

The effect of this excitation is to increase the predicted

covariance of the state error matrix.

The excitation covariance matrix is given by

a r.EEW(k) WT(k)]PT (4.3)

and

2 2 2 2 2q ~ ~ + )

4.y

t t 0

2 54

2 Y__ )2 2 2 2E " = t t+VX 6) .'"

+ Vm-

= V V E - 3x y 2tt

where the states -are evaluated at the current state

estimates X(k/k). Substituting these expressions in the 0

matrix results in

2 2 192 T2 2T912 2 T' 2 2 T *

" 2. o

T T

92 2 T9- Y -,. c0 t,.'L

2 2 o6v

2 2symmetric T '

A more detailed derivation of the excitation covariance

matrix is given in [Ref. 83.

The excitation matrix serves not only to take into

account the possibility of maneuvering, but of model ___

'p

inaccuracies as well. Q also used to prevent the gains o+

26

F%

F.,

I ,.o ." .

#' " '.".'."". . -,-,.',-.'. ,' •. , , • ,". - , . " " ,',", r .",",".".-. . . - .". - . . .-. . .r -4'd'.," . , ,,.., .. , ,,, .,,,,. ... , . .. .. ,.. .-. ..p. ., ,... ... ..,,,.• . .. ,... ... .., ...,, . . . .

%d

the filter from approaching zero as more and more data is

processed, by insuring some uncertainty in the predicted

state values [Ref. 3, 4, 53.

In the state form, the plant state equation is

X(k+l) = I X(k) + r W(k) (4.4). -

where:

1 T 0. 0 0 1 1 T /2 0 O11

o 1 0 0 0 1 T 0

0=to 0 1 T 0 and r = 0 T /2 0t

o 0 0 1 0 10 T of

0 0 0 0 0 1 0 of

B. DEFINITION OF MULTIPLE ARRAY TRACKING

The coordinate system is defined as shown in Figure 4.1.

These 72 positions, an XYZ position for each of 4

hydrophones in 6 arrays, are placed into a 6 x 12 matrix

HYDRO and referenced throughout the program.The torpedo -

position is referenced to a central level rectangular

coordinate system. The non-linear observation equations

become

TZ(k) E IT (k) T (k) T (k) T (k)] T + V(k) (4.5)- c X y z -

where IVA

2 2 - 1/2T (k)=- [X(k)-Xi +(Y(k)-Yi) +(Z(k)-Z )3"C vel iC ic iC

.

, °.

-4

4. " " % ' .-. - % " %. ." . % " . ' ' % % % " % " . ." ." . .,. . . "."• .% "".• -, . ,, "% . "-

43~~~ -77 -

)2 2 2 1 /2T (k)=-l-[(X(k)-X ) +(Y(k)-Y ) +(Z(k)-Zi) I

x vel ix iX ix

T (k)= I-E(Xk)-X 2 +(Y(k)-Y 2 +(Z(k)-Z 2 3J1/2

y vel iZ iZ iZ

T_()i= I) 2)2 1 /2

Tz k) v a [(X(k)-XZ +(Y(k)-Y i z)+(Z (k) -Z iz 3

and the subscripted variables X, Y, and Z are the

coordinates of a particu .-r array being used.

The decision parameter used to determine the switching

from array to array is a straight handoff. If the predicted

X position, Xk+/k ' is greater than 3,000 feet from the

array in use, then index is incremented and the next r6w of

HYDRO is implememted. This placed into the program the X, Y,

and Z positions of the hydrophones in the next array. The

handoff can easily be utilized in real range operations, as

the transit times from adjacent arrays are present at the

computer for a particular time slot.

28

%N!%. ," . ' ', ." ', - - " , , - . - . -. - ., ," 4 - . . . "' -'' -" "" - ''-'' - '' *

k-. ..

v .v-%

n ~~Y (f e et ) 1

, .ar ray4.i .-array(. +array+.: 4array-: 4array-: -array.

6 : 5 : 4 3 : 2 :

a Corint Syse fo MutpeAryTakn

ak .... ------- -------- ---- '---------*----":

* , .....

----- --- -

I 7 : -- : ---- l---: ---- 7 .--- .:-- - --- X--1 t ) _

6k 12k 18k 24k 30k 3k,

a ) Coordinate System for Multiple Array Tracking" 0 0 0

iurx y z x y Z y z x y M Ara .

29iI I I

130060::63: 0O, 12000: 60301 0,:31000 ,:6000 ,30 , -

!3ooI&o 00o i ao3o t ooo ;o ; 3ooo t 6o3o to!3ooo ; ooo ! 30 i .'i a Ii i i .% '

* a , I I I a I I,

5'...

Figue 4. Gemetr ofMultple rra Trakin

_.. -,,..,.,...-.. ....,-._ _. ... ... ... ...... .:,' -_...,,_..: _...:- _.. ....... ., .v . .-. . ,j ... _€ , ,. ,..-,_ ' '.,- ' '.-% ,: ,.- .. .-.--.. ' ..- ,._..:,_

-

d C. SEQUENTIAL EXTENDED KALMAN FILTER

In the sequential approach, after 'modifying the basic

Kalman filter equations, calculations are performed on each

of the four independent transit times in the following

order: T , T , T and T for each 1.31 second time slot.c y z

Since the four transit times are independent and processed

sequentially, the covariance of error matrix and the state

vector are updated four times during each time slot. Thus

more accurate estimates of the filter states are achived.

Modification of the filter equations for the sequential

approach circumvented the matrix inversion in the gain

N equation. An invalid transit time measurement will result in

the filter ignoring the update information for that

particular measurement only..4

The estimate of the states X(k/k), based on one transit

time measurement are used as the prediction X(k/k-1) for the

calculationson the next measurements. Thus for the first

time measurement T only the first row of the linearized Hc

matrix is calculated and then the first gain column

corresponding to the first time measurement T is calculatedc

by

TP(k/k-1) H..row (4.6)% ool T---"H. P(k/k-l) H + R.

irow irow ii

30P,%"

• 4

;....4-.

where i = 1 to 4 corresponding to the four measured transit

times.

An estimate of the particular observation time is

calculated by using Equation 3.9 evaluated at the predicted

state Xlk/k-1). The difference between observed transit

times and the estimated transit times forms the residual

which is used in the estimate equation .-.

X i = X(k/k-1) + 6. [Residual] (4.7) ":,."icol

This equation gives an estimate of the states based on one I

of the four time measurements. %v%-

The covariance of error is calculated based on one

measurement by

P. [IG. H. .(4_i icol irow i-I (4.8,

where: I is identity matrix;

P is the covariance matrix calculated from the

previous transit time measurement or if i = 1, ,...-%

the predicted error covariance P(k/k-1).

Editing erroneous time measurement is achieved by

implementing a three sigma gate using the covariance of the

measurent noise (R) and the covariance of the estimation

%1 1. . .. .... . . ..... .t...... ....... " .......-.,' ' ' -. ' ' L'L _ -

. . .- . 4 t,4 t -,1.

error P(k/k).The gate then is written for each time

measurement i = 1 to 4:

% gate = 3* C(P. .maximum)/(4860.) I + R..) (4.9)

where j = 1, 3, 5. The gate expands or decreases depending

L- on the confidence level of the position estimate and the

transit time. If the difference between the actual transit

.K¢ time received and predicted transit time to a particular

hydrophone exceeds the gate, the measurement is considered

unacceptable and the filter gain is set to zero causing the

filter to ignore the data and take the prediction of the

states as the estimate X(k/k) X (k/k-1).

Bounding the residual bias error is achieved by making

comparison between the average of the absolute value of the

time residuals and the preset threshold. If the average of

the time residuals exceeds the preset threshold, 0 is

.' - calculated and added to the last updated covariance of error

matrix P. Then filter reiterates the gain, covariance, and

state estimate equations for the same time slot. This

procedure continues until the average of the time residuals

falls below the preset threshold at which time an acceptable

state vector estimate has been obtained for the time slot.

32

' ° °°!1

D. OPTIMAL SMOOTHING ALGORITHM

The smoothing solution starts with the filtered estimate

at --the last point and calculates backward point by point

determining the smoothed estimate as a linear combination of

the filtered estimate at that point and the smoothed

estimate at the previous point ERef. 63.

It can be seen from the error covariances that the

filter has reached.a steady-state condition by the end of

the forward sweep. As an-example, let us enter the backward

sweep at the end point where k - 20. Here we have X(20/20)

and P(20/20). Since the filter solution at this point is

conditioned on all the measurement data, it is also the*1*

..

smoothed estimate at k = N = 20. We are now ready to compute

the smoothed estimate one step back at k -19. From

Equations 3.11, 3.12 and 3.13 we have

X(19/20)= X(19/19)+ A(191X(20/20) - X(20/19)]stored stored

X(20/19) = X(19/19)stored

J.

A(19) = P(19/19) T p- (20/19)stored stored

". . °

3a-, "

... ,-

P(19/20) = P(19/19) + A(19)[P(20/20) - P(20/19)] A (19)

stored stored stored

and to compute the smoothed estimate two step back at k 18

X(18/20)= X(18/18)+ A(18)[x(19/20) - X(19/18)]stored

X(19/18) - X(18/18)

.stored

T -

A(18) = P(18/18) iF p (19/18)stored stored

44,,

P(18/20) = P(18/18) + A(18)[P(19/20) - P(19/18) AT(18) ,

stored stored

This procedure continues until the time k reaches to 1.

• o%

*7s

, %

'p ,p .p

i: "::,4,

707-. W- V o'

V. SIMULATION RESULTS

,.*

A. MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN

The true trajectory of the torpedo is a straight line

with a 50 ft/sec velocity toward the origin of hydrophone

array parallel to X-axis, drawing two tangent circles with

10 dog/sec turn rate, in the horizantal X-Y plane through a

multiple array.

In the first part of this run, the initial position of

the torpedo is 38000 ft in X, 7000 ft in Y, and 300 ft in Z.

Figures 5.1 and 5.2 depict the filtered and smoothed

estimate of the trajectory, with zero initial velocity

errors and 25 ft initial position errors in X and Y. The

errors in the filtered and the smoothed estimate of

positions in X, Y and Z are drawn in Figures 5.7, 5.4, 5.5,

5.6, 5.7 and 5.8. For the Kalman filter, errors ranged

between -1.2 and 2.6 ft in X, -5.9 and 1.9 ft in Y, 0.1 and

2.5 ft in Z. After smoothing, the errors occured in smaller

range, which is, between -1.4 and 2.4 ft in X, -5.0 and 1.9

ft in Y, 0.1 and 0.7 ft in Z. The diagonal terms of the

filtered and smoothed error covariance matrices are shown

pictorially in Figures 5.9, 5.10, 5.11, 5.12, 5.13, 5.14,

5.15, 5.16, 5.17, 5.18. 735.

, *%1-o

7. . . . . . . . . . . .. "

In the second part of this run, the initial position of

the torpedo is 35000 ft in X, 7000 ft in Y, and 300 ft in Z.

The filtered and smoothed estimate of the trajectory are

drawn in Figures 5.19 and 5.20. Taking this different

initial geometry made the errors in the position of the

torpedo to take place in bigger values during first time

slot of the filtering and last time slot of the smoothing.

As seen in Figures 5.21, 5.22, 5.23, 52, 5.2..d 526

errors ranged between -16.3 and 1.9 ft in X, -15.1 and 4.6

ft in Y, -5.0 and 1.0 ft in Z for the Kalman filter and for

the smoothing this error range is between -12.6 and 1.3 ft

in X, -12.3 and 4.8 ft in Y, -1.9 and 1.0 ft in Z. rhe

diagonal terms of the filtered and smoothed error covariance

matrices displayed slightly different magnitude, as seen in

Figures 5.27, 5.28, 5.29, 5.30, 5.31, 5.32, 5.33, 5.34, 5.35

and 5.36.

B. MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN

In this run, the true trajectory of the torpedo is a

straight line with a 50 ft/sec velocity toward the origin of

hydrophone array parallel to X-axis in the horizantal X-Y

plane through a multiple array. .

With the initial position of the torpedo is 38000 ft in

X, 7000 ft in Y, and 300 ft in Z. The filtered and smoothed

estimate of the trajectory, with zero initial velocity

", ..16

36 "..°. - ,

errors and 25 ft initial position errors in X and Y, are

depicted in Figures 5.37 and 5.38. Figures 5.39, 5.40, 5.41,

5.42, 5.43 and 5.44 give the errors in the filtered and the

smoothed estimate of positions in X, Y and Z. For the Kalman

filter, errors ranged between -1.6 and 2.6 ft in X, -5.9 and ~t4.7 ft in Y, -0.2 and 2.5 ft in Z. After smoothing, the

errors occured in smaller range, which is, between -1.1 and

2.4 ft in X, -5.0 and 1.7 ft in Y, -0.2 and 0.6 ft in Z. The .

diagonal terms of the filtered and smoothed error covariance

matrices are shown pictorially in Figures 5.45 through 5.54.

C. SINGLE ARRAY ADAPTIVE MANEUVERING RUN

The previous tests described the filter and smoothing

performance for both straight and maneuvering runs through

multiple array. Using the same basic torpedo trajectories as

in multiple array, similar tests are performed for

maneuvering run through single array.During the single array

tracking, the initial position of the torpedo is 7500 it in

X, 1300 ft in Y and 0 ft in Z, which gives different initial

geometry. The filtered and smoothed estimates of the

trajectory and the corresponding position errors in X, Y and

Z are pictorially given in Figures 5.55 through 5.62. For

the Kalman filter, errors ranged between -1.6 and 3.3 ft in

X, -19.1 and 8.9 ft in Y, -0.3 and 1.6 ft in Z. After

smoothing, the errors occured in smaller range, which is, "

37

%- . .

, i~,

between -1.1 and 3.1 ft in X, -17.9 and 5.0 ft in Y, -0.1

and 1.6 ft in Z. The diagonal terms of the filter-ed and

smoothed error covariance matrices are shown pictorially in

Figures 5.63 through 5.72.

D. SINGLE ARRAY STRAIGHT RUN

The purpose of this last series of tests is to

functionally demonstrate the performance of the filter and

smoothing during a straight run through single array using

the same initial torpedo position as in single array

adaptive maneuvering run. The filtered and smoothed

estimates of the trajectory and the corresponding position

errors in X, Y and Z are pictorially given in Figures 5.73

through 5.80. For the Kalman filter, errors ranged between

-1.6 and 3.3 ft in X, -19.1 and 8.9 ft in Y, -0.3 and 0.8 ft

in Z. After smoothing, the errors occured in smaller range,

which is, between -0.6 and 3.1 ft in X, -17.9 and 3.5 ft in

Y, -0.2 and 0.7 ft in Z. The diagonal terms of the filtered .1

and smoothed error covariance matrices are shown pictorially

in Figures 5.80 through 5.90.

...-..- )

- '.5I

* . .. '=- .5. . . .

-. - * . * * * - *..* =*.- --. -

, , ,- *%.

VI. CONCLUSIONS

The sequential extended Kalman filter and smoothing

routine sufficiently generated the filtered and smoothed

estimates of the states, which specify the motion of the

torpedo. Errors generated by running the routine on the

IBM-PC are comparable to those given in the previous search,

which was done on a large IBM computer [Ref. 13.

In the smoothing problem, computing the predicted

estimates of the states, X(k+I/k), from the estimates of the

states, X(k/k), eliminates the storage problem for X(k+i/k).

In future studies, an algorithm for computing P(k/k) from

P(k+/k+I) and hence eliminating the storage problem for

P(k/k), should be invesigated.

Examining the errors and their covariances, it is

evident that the uncertainty in position exist only in the

Y direction for the case where the torpedo is moving along

the X axis. The results of the straight run analyses shcw

that the propagation of the filtered error covariance is

dependent on the path of the torpedo with respect to

hydrophone array. Upon observing the error propagation it is

apparent that the position errors exhibit approximately

equal oscillations about zero indicating that the

39

. . . .. - -. .. • - . . . - . . . . . .. . -. . . . . .. . . .. - . . . ,.. .- . -, '% %11 "

measurement noise is the dominant error source driving the

filter.

The smoothed estimates of the states are at least as

good as or better than the filtered estimates. The filter .71performance was dependent on system noise and the distance

from the torpedo to the hydrophone array. Errors get bigger .

as the torpedo approaches the tracking limit of the

hydrophone array. '-

Additional work should be done using trajectories

generated from actual torpedo runs on the Dabob test range.

The rotation and reduction of the error ellipsoids should 4be also included in future studies.

The filter should be of use in range safety in warning

for possible collisions. Also it may prove invaluable in

torpedo recovery when there is a malfunction and the torpedo

is sometimes buried in many feet of muds.

... . . .

*-.4 °-

op

1ULTIPLE RRRqrf RORPTIVE MrNEUVERING R U--

FILTERED ESTIMATE OF TRAJECTOAT WITH NOISE ". '-A

0 ~. . .

30 340 31 50 360 35 80

oE /K (FT **10*

- *~* ,*s%

Dui g aeuein-untrogMlipeAra

3360 3 440 3520 3600 3680 3760 3840 ......XEK/N-I (FT) ,410 1

%

Figure 5.2 Smotehed Estimate o+: Trajectory o+ the Torpedo"-'""During a Maneuvering Run through Multiple Array '

41

o2

0UTPEARTAATV AEVRN U "-*-0 '%"

3350 3430O 3510 3590 3670 3750 3q

XEK/N3 (FTJ I 8O

Figure 5.1 Smoltred Estimate of Trajectory of the Torpedo ""'

During a Maneuvering Run through Multiple Array-.,

b &/,

MULTILE RIRAT RAPTIE MRNUVERIG RU

SMOOTED ETIMAE OFTRAJETORTWITHNOIS0 ° -

o. ..

!" * " " .. - "". '" . . ." . . " ... .. : . ° . "" . '*'' ' . "' . ' '' . " '. '- . " " ,' " - , .- , ". J -'." J . ',0" =

MULTIPLE ARRAY AOAPTIVE MANEUVERING RUN I-,'

ERROR IN FILTERED ESTIMATE WITH NOISE

LU

0 20 q0 60 80 100 120

TIME SLOTS

5-. __ _ __ __-_ __ _ __ __-_ _

TRRI S LET WT N"S

i .iriP n

V.D

* -I? ..>

MULTPLERRRTIM SOTS REVRN U

Figure 5. Error in Smooteed Estimate of Position in X of.

the Torpedo During a Maneuvering Run through Multiple Array,.

42. +U . 0" ...

..l

0 20 1Qo 60 80 100 120 ,,.

TME SLOTS ,. _

SFigure 5.4 Error in Smoothed Estimate of Position in X of,-'€the Torpedo During a Maneuvering Run through Multiple Array .. :

I4.,..t=

42• i

[%[• ",..,.[ - . . . . . ' '.. ~ ."'- '- " " " '" " + .' "* -' -' -' -" "- '+ ". "- ' -"-' .' '. ". '-" '+ *'" '" '. "/ """ ' "" " "" +'" "+ '."" -" .".'+,." ., '+ '" +" ." " '. '-" ' ." " C." .. ; ' '. "+: " ".4

MULTIPLE ARRRT ROAPTIVE MANEUVERING RUN


ct--

0

,U

r

e

I i

=

0 20 40 60 80 100 120

TIME SLOTS

Figure 5.5 Error in Filtered Estimate of Position in Y ofthe Torpedo During a Maneuvering Run through Multiple Array

MULTIPLE ARRAT AORPTIVE MANEUVERING RUN

ERROR IN SMOOTHED ESTIMATE WITH NOISE

9

-- 20

40 60 80

100 120

%T

I M E S L O T S,

".

F i g u r e 5 6 E r r o r i n S m o o t h

e d E s t i m a t e + P o s i t i o n

i n Y a +'

the Torpedo During a Maneuvering

Run through Multiple

Array .,

L .. 0

t.d

%:0*14

MULTIPLE ARRRT RORPTIVE MANEUVERING RUN

ERRO IN ILTRED STIATE ITH NOISE

.N;%.64-S

0 20 40 60 s0 100 120

TIME SLOTS

Figure 5.7 Error in Filtered Estimate o+ Position in Z ofthe Torpedo During a Maneuvering Run through Multiple Array

MULTIPLE RRRRT ROPTIVE MANEUVERING RUN


eD0cc

LU

o i,

0 20 40 60 80 100 120TIME SLOTS

Figure 5.8 Error in Smoothed Estimate of Position in Z of

the Torpedo During a Maneuvering Run through Multiple Array

44

.1Ad

%,

MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN . "

FILTERED ERROR COVARIANCE P(K/K) WITH NOI5E

4 0.0 5%

-o v

0 20 40 60 80 100 120

TIME SLOTS

Figure 5.9 Variance of Filtered Position Error in X of theTorpedo During a Maneuvering Run through Multiple Array

MULTIPLE RRRAT ADAPTIVE MANEUVERING RUN -4.:..,

SMOOTHED ERROR COVARIANCE P(K/N) WITH NOISE

-C3

0 - -.z j

L 0

0 20 40 60 80 100 120TIME SLOTS

Figure 5.10 Variance of Smoothed Position Error in X of theTorpedo During a Maneuvering Run through Multiple Array

45

4-. '''' ... , z "v '''' . ,.,., '' , w ., . ~ . . . z ,j ' .j - _""""° .Z "- ', " ' - - , . z . - ". Z " . """"' / - """

"*1,

. .%. %* -o,

MULTIPLE PqRRPT PqORPTIVE MANEUVERING RUN

.,, , FILTERED ERROR COVPRIANCE P(K'K} WI1TH NOISE Z

LJ w W A "".

0 20 40 60 80 10 120v

TIME SLOT5

Figure 5. 11 Variance of Filtered Velocity Error in X of the ..

Torpedo During a Maneuvering Run through Multiple Array-,-,

MULTIPLE ARRI SOPTIVE MANEUVERING RUN

SMOOTHED ERROR COVARIRNCE P(K/N) WITH NOISE i " "

(:3

: o~.4.-.

0 20 40 60 80 100 120--'

TIME 5LOTS il ii

Figure 5.12 Variance of Smoothed Velocity Error in X of theTorpedo During a Maneuvering Run through Multiple Array

464

-> S:

#9 °.% o / . " " ,, . ' # " . " m . , " . " , " ," . " o , " # p . - .- . ' . - . " . / l" W P" "" ""' "'

" • tl • MULTIPLE • RPR RORIVE MRNEUERIN RUN •%l%% . , =.l.

S. .

,," .°

%

0 ."v'.

MULTIPLE RRRRT ADAPTIVE MANEUVERING RUN

FILTERED ERROR COVARIANCE P[K/K) WITH NOISE

- 0 _ _

0 20 40 60 80 I 0 1 0,.-.-TIME SLOTS

'.'.120

Figure 5.13 Variance of Filtered Position Error in Y of theTorpedo During a Maneuvering Run through Multiple Array

I'-.W,""k.1

MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN

SMOOTHED ERROR COVARIRNCE PUK/N) WITH NOISE

- T..I-,

0 20 40 60 80 10 1Ot20 .,- -]-TIME SLOTS;"" "

c'J-

Figure 5. 14 Variance of Smoothed Position Error in Y of the"-.''

Torpedo During a Maneuvering Run through Multiple Array-'--.

47 " "''

04

..... "....

0° °=

• ". m%

",o" °',

-, ,,.-A1

_ -.o..-.-0

"z ".• -..

MULTIPLE ARRAY RORPTIVE MRNEUVERING RUN

FILTERED ERROR COVRRIRNCE PIK/K) WITH NOISE

LU NV0

~L 0CLL

0 20 40 60 8 0 100 120

- TIME SLOTS ...

Figure 5.15 Variance of Filtered Velocity Error in Y o theTorpedo During a Maneuvering Run through Multiple Array

MULTIPLE ARRAT RORPTIVE MANEUVERING RUN

SMOOTHED ERROR COVPRIANCE P1K/N) WITH NOISE

C3

.... ,"

-j L - '

0 20 0 60 80 100 120

TIME SLOTS".,.,

Figure 5.16 Variance of Smoothed Velocity Error in Y of theTorpedo During a Maneuvering Run through Multiple Array

48

4b...'.,

S- . .- -,7. - . , , * .~ a ~ . ~ . * ~ - - ... .

S. -7'. - * ' ' '' ~'' -.'. '.________________-______________ a a a ..D P ., - & - , p . a .- a .

MULTIPLE ARRAY ADAPTIVE MANEUVERING RUNF I LTERED ERROR COVAR IRNCE Pf K/'K I WITH NOISE a

- 7'

0 20 4 0 8 0 2

TIE SL T

Fiur 5.7 Vrac0fFlee Psto ro nZo hTorpedo DuigaMnueigRntruhMlil0ra

MUTPEARYAATV0AEVRN U

a -

0 2 40 60 60 100 120,TIME SLOTS

Figure 5.17 Variance of Filtred Position Error in Z of theTorpedo During a Maneuvering Run through Multiple Array

:% 4

.4 MULTIPLE ARRAYT ADAPTIVE MANEUVERING RUN

FILTERED ESTIMATE OF TRAJECTORT WITH NOISE

p.

-

0%

C~o

0'0

3040 3120u 3200 3280 3360 3440u 3520

XCK/KJ (FT) *10'

Figure 5.19 Filtered Estimate of Trajectory of the TorpedoDuring a Maneuvering Run through Multiple Array


SMOOTHED ESTIMATE OF TRAJECTORT WITH NOISE

C!

Co

z 000

3040 3120 3200 3280 3360 3440 3520XCK/N] (FT) W10'

Figure 5.20 Smoothed Estimate of Trajectory of the TorpedoDuring a Maneuvering Run through Multiple Array

50



J. CC-:T.>

0 20 40 60 80 10O0 120 ,-.-.

TIME SLOTS 2 '-

Figure 5.21 Error in Filtered Estimate of Position in X of

the Torpedo During a Maneuvering Run through Multiple Array!#. ,'.,

MULTIPLE RRRAT ADAPTIVE MANEUVERING RUN

ERROR IN SMOOTHED ESTIMATE WITH NOISE ,

U-

. 0 0-'

p.0 20 L40 60 60 1o0 120TIME SLOTS

v

Figure 5.22 Error in Smoothed Estimate of Position in X ofI~. the Torpedo During a Maneuvering Run through Multiple Ar-ray

51Sp%

nr-. . ' .

U ×

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

MULTIPLE ARRAY RDRPTIVE MRNEUVERING RUN


LL 0

- -. r

0 20 4 0 0 U e

TIE SLT

CD

0C0 LD6D8 0 2

TIME SLOTS

Figure 5.24 Error in Smiltred Estimate of Position in Y ofthe Torpedo During a Maneuvering Run through Multiple Array

.152

F.a

MULTIPLE ARRAYT RORPTIVE MANEUVERING RUN

ERROR IN SMOTHED ESTIMATE W'ITH NOISE

CC 0LL 0

0 20 40 6 8 o 2

TIM SLT

Fiue .6 ro i mote Etmaeo+PstininZo

th TrpdoDuig Mneveig unthouhMutileAra

a53

MULTIPLE RRRRT RORPTIVE MANEUVERING RUN

FILTERED ERROR COYRRIRNCE P(1K/Kl WITH NOISE

--

CD

L.

a 20 40 60 80 100 120TIME SLOTS

I

Figure 5.27 Variance of Filtered Position Error in X of theTorpedo Durfng a Maneuvering Run through Multiple Arra%/

MULTIPLE R9RA ADAPTIVE MANEUVERING RUN

SMOOTHED ERROR COVARIRNCE P1K/N) WITH NOISE

- 0 .. ,.,

CD..

03

0 20 L40 60 80 100 120,,

TIME SLOTS

Figure 5.28 Variance of Smoothed Position Error in X of theTorpedo During a Maneuvering Run through Multiple Array

5 ..

..

* 6,' ".

i i i *..I?*- J.-.,-~- - .*'~ -

MULTIPLE ARRAYT ADAPTIVE MANEUVERING RUN

FILTERED ERROR COVARIANCE P(IK/K] WITH NOISE

o,

C!

0j

L- - C3

o .

20 4 6 0 10 2TIESLT

Figure5.29 arineo itrdVlct ro nXo hTorpedo DuigaMnueigRntruhMlil ra

- -. .-.

(\j

-

Mi LU C3

0 20 LI0 60 80 100 120

TIME SLOTS

Figure 5.29 Variance of Smiltred Velocity Error in X of theTorpedo During a Maneuverinig Run through Multiple Array%

_ _ _ _ _ _ _ _ _ _ _ _ _ __55

- aN

-.;...-.

MULTIPLE ARRAT ROAPTIVE MANEUVERING RUN

FILTERED ERROR COVRRIRNCE P1K/K) WITH NOISE

t, %=

-I -;.II... I.• ....

MULIPL ARRLOOR T ",.UERGRU -

a-

0 20 L40 60 s0 100 120*TIME SLOTS

Figure 5.32 Variance of Filtered Position Error in Y of theTorpedo During a Maneuvering Run through Multiple Array

56

4.d

-.

MULTIPLE RRRT ADAPTIVE MANEUVERING RUN ...SMOOTHED ERROR COVRRIRNCE P1tK/N) WITH NOISE "'

, ... .a -: ..-m a .: .'.

-- .

ri-2

a°. z .-.. -.. .:

oaU ',

0 2 " 60 80 100 12

kI "

MULTIPLE ARRAT ROAPTIVE MANEUVERING RUN V -

FILTERED ERROR COVRRIRNCE P(K/K) WITH NOISE

oZN

r- LU (V.

~U_

N_,, NI

0 20 40 60 80 100 120TIME SLOTS

I.

Figure 5.33 Variance of Filtered Velocity Error in Y of theTorpedo During a Maneuvering Run through Multiple Array

MULTIPLE ARRRT ADAPTIVE MANEUVERING RUN

SMOOTHED ERROR COVRRIRNCE P1K/N) WITH NOISE

o 20 4 6 o 10 2

Figure 5.34 Variance of Smoothed Velocit Er:o in Y of theTorpdo urig aManuveingRunthrughMultiple Array

6 .m-,4

- I X - -4 - *.- "-s,-

. ;: "- ,,':

ZO.__L..J .L..

," .%-7


FILTERED ERROR COVRRIPNCE P1K/K) WITH NOISE

Q- 0

0 20 -'6 0 0 2

TIME SLOTS 00 12

Figure 5.35 Variance o4 Filtered Position Error in Z o+ the

Torpedo During a Maneuvering Run through Multiple Array

MULTIPLE RARAT ADAPTIVE MANEUVERING RUN

SMOOTHED ERROR COVARIANCE P 1K/N) WITH NOISE

o3

02

U

If,

z7

4 4

MULTIPLE ARRAT ADAPTIVE STRAIGHT RUN

FY.TERED ESTIMATE OF TRAJECTORT WITH NOISE

U-C3

I- . q ,

~A

300 320 340 360 380 400 420XEK/K (FT) *10.

Figure 5.37 Filtered Estimate of Trajectory of the TorpedoDuring a Straight Run through Multiple Array

4


StOOTHED ESTIMATE OF TRAJECTORY WITH NOISE

-. 4.,

ziL 0n

300 320 340 360 380 400 420XEK/NJ (FT) .0

1*-*

Figure 5.38 Smoothed Estimate of Trajectory of the TorpedoDuring a Straight Run through Multiple Array

59

* - °

,' ., . . . . , ' ] - ' '. '. . . ; ." . .L . .' ' '. . .' ' ; ' ' '- . ." " . - . . . ." " " . " ' ' " , " " '. . " -' ' '. ' '. . " .€ . . ." " " . ". ." " ". ." " . ." " " . " -" " -4 "

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

°'-.

MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN


I. :- 0 ' '

C3

0crcc

20 40 60 80 100 120TIME SLOTS

Figure 5.39 Error in Filtered Estimate of Position in X ofthe Torpedo During a Straight Run through Multiple Array

MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN 1ERROR IN SMOOTHED ESTIMATE WITH NOISE

C-v

to0- 9

cr 2

0 200 0 8 0 2

TIME SLOTS ..,.;

Figure 5.40 Error in Smoothed Estimate of Position in X of -the Torpedo During a Straight Run through Multiple Array,'.,

06

• %-

0 20 40 60 80 100.12-'"%,], ?-",):. ., ',", ,".,-' >' '" ' "," ..'.,-" ",,.'P." },. .,'-,TIME' SLOTSU' ." ' } ""'.L,. . "'" "

~ ~ ~4'.. ' .'~. ,.. ~ ..'. "= •

-. -.."..

MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN


o,-... .

I.. 0 3I .. .CC 0

0

0 20 LID 60 80 100 120

TIME SLOTS

Figure 5.41 Error in Filtered Estimate o Position in Y ofthe Torpedo During a Straight Run through Multiple Array

MULTIPLE ARRAY ADAPrIVE STRAIGHT RUN


I - -

I 0 .

0 20 40o 60 80 100 120

TIME SLOT5S.--0. _____ __-__,,__-__.__

Figure 5.42 Error in Smoothed Estimate of Position in Y ofthe Torpedo During a Straight Run through Multiple Array

61

4kI

.".



V.-

20 TM 60 80 100 120 ''

T IME SLOTS ''"'

-. -

Figure 5.44 Error in Smootered Estimate of Position in Z of -the Torpedo During a Straight Run through Multiple Array .

62S .

ERROR IN SMOOTHED ETIMRTE WITH NOS .* ,

II

Fiue54 rorin Fitre Esimat o I Positon i oth opdoDrn a o StaihtRu thoug Multiple,:Ara.

MUTPE RTIM ROTESTR".','.RU

4.' .'-.,ERO I MOTE 2STMT WIHNIS.....

,'.;..

o ." ,. -. - .. - . , . . -. . .,%-.-.. .-. .-... ' -... " .. . " ,

MULTIPLE RRAY ADAPTIVE STRAIGHT RUN1FILTREDERRR CVARINCEPIKK) ITH NOISE

a

-Cuj

U U-

0T

Figuri 5.4 RUNanc ofFlee oiio ro nXo the

MULTIPLE ARRAY ADAPTIVE SRIH U


Z ".

oJ

.C, P-a

Cu

0 2nl 40 60 s0 100 120TIME SLOTS

Figure 5.46 Variance of Smoothed Position Error in X of theTorpedo During a Straight Run through Multiple Array

.5. 63

:5..-... S~~*'4~ o%* % \_.

2Lik~IIIA JJ .. . ~. *.... v.5.'- ~ cc~\'.%v .

MULTIPLE RRRRT RDRPTIVE STRAIGHT RUN*FILTERED ERROR COVRRIANCE P(K/K) WITH NOISE

N C!

I0 2 0 4 0 60 80 1 00 1 20TIME SLOTS

Is.-e

*Figure 5.47 Variance of Filtered Velocity Error in X ofthftftftTorpedo During a Straight Run through Multiple Array

0% MULTIPLE RRRRT RORPTIWVE STRAIGHT RUN


C2

I0L W

0 20 40 60 80 100 120TIME SLOTS

Figure 5.48 Variance of Smoothed Velocity Error in X of theTorpedo During a Straight Run through Multiple Array

64

7 Y, Y*-1.7F.

MULTIPLE ARRT ADAPTIVE STRAIGHT RUN

FILTERED ERROR COVARIANCE P(K/K(1 WITH NOISE

C3

CL

0 20 q0 60 s0 100 120

TIME SLOTS

Figure 5.49 Variance of Filtered Position Error in Y of theTorpedo During a Straight Run through Multiple Array


SMOOTHED ERROR COVARIRNCE P(K/N) WITH NOISE

0 20N 0 s 0 2

TIESLT

Fiue .0 aiac o mote PstinEro i 0 hTorpedo ~~Drn tagtRntruhMlil ra

C65

MULTIPLE RRRRY RDRPT EVE STRRIGHT RUN

r-1 W

0 20 LI0 60 80 100 120

TIME SLOTS

Figure 5.51 Variance of Filtered Velocity Error in Y of theTorpedo During a Straight Run through Multiple Array -

MULTIPLE ARRAT ROAPTIVE STRRIGHT RUN

SMOOTHED ERROR COVARIRNCE P(K/N) WITH NOISE

Ca

- I'.

0 20 40 60 80 100 120

TIME SLOTS

Figure 5.52 Variance of Smoothed Velocity Error in Y of theTorpedo During a Straight Run through Multiple Array

66

L ____AC

-iA

5,'%

%%

MULTIPLE RRRAT ROAPTIVE STRAIGHT RUN

FILTERED ERROR COVPRIANCE P K/K) WITH NOISE

Co

a3

O 20 40D 60 so 100 120

,''.TIME SLOTS

0

J. I

,.- Figure 5.54 Variance of Smootehed Position Error in Z of the-, Torpedo During a Straight Run through Multiple Array

.o67

4.

-~ a I I,].0 20 40 60 80 100 120

'-j::TIME SLOTS

Figure 5.54 Variance of Smiltred Position Error in Z of theTorpedo During a Straight Run through Multiple Array

* .~.7

" % """ " % "" " % % o" -" " ' ' - ""=" °" " " " " "" '% =% " - -° .' .' " '° " % % ° "- - " %"% " %"%" W" . % %" ' 5

SINGLE RRAPY RORPTIVE MANEUVERING RUN

FILTERED ESTIMATE OF TRAJECTORY WITH NOISEN..

o C;

30 380 460 54 20 70 8

XC /K (F ) 1

0

300 390 '460 SI0 620 700 780

XEK/N] (FT) *101

Figure 5.55 Smilthred Estimate of Trajectory of the TorpedoDuring a Maneuvering Run through Single Array

SIGL ARA RRPIE ANUVRIG8U

~-. . . . . . . . . . . . . ...' -. ., -i -1.

3.7 %.* --

SINGLE ARRAY ADAPTIVE MANEUVERING RUN - -

ERROR IN FILTERED ESTIMATE WITH NOISE-%

0V

0 20 40 60 80 100 120TIME SLOTS

Figre .57Error in Filtered Estimate of Position in X ofteTorpedo During a Maneuvering Run through Single Array

SINGLE ARRAY ADAPTIVE -MANEUVERING RUN

ERROR [N SMOOTHED ESTIMATE WITH NOISE

€%

0 20 4 0 8 o 2

699

,,rr

! I i tI.4 r. "

TIME SLOTS": :'

Figure 5.56 Error in Smothed Estimate of Position in X of:the Torpedo During a Maneuvering Run through Single Array

69 ..

SIGE*llT OP IE .AEUE IGRN"-."

-, * .,ERO NSOTE SIAT IH NIE"."

7.17 717 -Vr-irr e _

SINGLE ARRAY ADJAPTIVE MANEUVERING RUN VERROR IN FILTERED ESTIMATE WITH NOISE

0

LL- 0

0*

0 20 LI0 60 80 100 120TIME SLOTS

Figure 5.59 Error in Filtered Estimate of Position in Y of

the Torpedo During a Maneuvering Run through Single Array

SINGLE ARRAY ADAPTIVE MANEUVERING RUN4

'4ERROR IN SMOOTHED ESTIMATE WITH NOISE

C3

-. r

O 20 LID 60 80 100 120TIME SLOTS

'V Figure 5.60 Error in Smoothed Estimate of Position in Y of-. the Torpedo During a Maneuvering Run through Single Array

70

- ......- .

SINGLE ARRAY ADAPTIVE MANEUVERING RUN b~ERROR IN FILTERED ESTIMATE WITH NOISE

I.0

N%-%

'cc

CC 0

NJ0

0 20 40 60 80 100 120

TIME SLOTS

Figure 5.61 Error in Filtered Estimate of Position in Z ofthe Torpedo During a Maneuvering Run through Single Array ZI

• "%.,.

SINGLE RRAT ADAPTIVE MANEUVERING RUN

ERROR IN SMOOTHED ESTIMRTE WITH NOISE

- , S

I.-- 3

LL

: . ..-

• -S°. ,.

C3

0 20 40 60 80 1oo 120TIME SLOTS

Figure 5.62 Error in Smoothed Estimate of Position in Z of %

the Torpedo During a Maneuvering Run through Single Array

71

'

SINGE RRRT DAPIVE ANEVERIG RN "-.4,

ERRORS' INS .S ..- SMOTHD ETIAT WITH.~-~-- NOS - "- ". .

_ 0A

SINGLE ARRAT ADAPTIVE MANEUVERING RUN

FILTERED ERROR COVRRIRNCE P1K/K ] WITH NOISE

.17

0

-. U1

0 20 40 60 80 100 120TIME SLOTS

Figure 5.63 Variance of Filtered Position Error in X of the

Torpedo During a Maneuvering Run through Single Array

SINGLE ARRAY ADAPTIVE MANEUVERING RUN

SMOOTHED ERROR COVRRIRNCE P1K/N) WITH NOISE

0%

0 20 40 60 80 100 120 "'" ""

T IME SLOTS "'''

Figure 5.64 Variance of Smoothed Position Error in X o+: the .'

Torpedo During a Maneuvering Run through Single Array "-

7° 2

7o•-"-

*. 0-,,,".,"-',- .'". "-..,,"'.e"'".""""- . ""' ..""""'°. -"""", "" -""""" -[ ""-.-.'""'',""''. - """''-."'TIME"- "SLOTSW' -, -,,. . .. .',- " ..-'."',' .,

Q~ ~ ~ ~ ~ ~ ~ ~ .° V° 7 rA 747-7R -

%* 4.-.

,.


FILTERED ERROR COVARIANCE P1K/K) WITH NOISE

0 20 40 60 80 100 120 .,..TIME LOT. .

Figure 5.65 Variance of Filtered Velocity Error in X of the . ";

Torpedo During a Maneuvering Run through Sinlge Array .

SINGLE ARRAYT ADAPTIVE MANEUVERING RUN, .

SMOOTHED ERROR COVRRIANCE P(K/N) WITH NOISE ,•

0

LU

0 20 40 60 s0 100 120

TIME SLOTS

4...'

Figure 5.65 Variance of Smoothed Velocity Error in X of theTorpedo During a Maneuvering Run through Single Array

-" ~73 T' '

."1

,0..- _ -.. :::.?

SINGLE ARRAYT RORPTIVE M'RNEUVEMING RUN

*FILTERED ERROR COVARIANCE P1K/K) WITH NOISE

0 20 0.6 ..100 12

5TIM SLOTS''

0e

Z C'

U

4.74


FILTERED ERROR COVRRJANCE P(K/K) WITH NOISE

1-0

U- akL-(L- I

0 20 110 60 80 100 120

TIME SLOTS

Figure 5.69 Variance of Filtered Velocity Error in Y of theTorpedo During a Maneuvering Run through Single Array '



;7a3

U

- C

Fiur 5.7 Varineo mohdVlct ro nYa h

Torped Duin a aevrn u hruhSnl ra

z7

%

** s. "*

SINGLE ARRAY ROAPTIVE MANEUVERING RUNFILTERED ERROR COVRALRNCE P(K/Kl WITH NOISE

aC!

-Ca

LA;

0 0 LO 60 80 100 120

TIME SLOTS

Figure 5.71 Variance of Filtered Position Error in Z of the


SINGLE ARRAT ADAPTIVE MANEUVERING RUN

SMOOTHED ERROR COVRRIANCE P1K/N) WITH NOISE

- o

.

(C. !- • i I. I%

a 20 L&0 60 80 100O 120

TIME SLOTS

Figure 5.72 Variance of Smoothed Position Error in Z of the


-.p. 76

A.. . . . . . . . . . .. . .. ." , t #' ,/ ,, - - " ," , . *Pw"W'.PC..".' .*C". *: .... . . ..-.-.. , . ....... . .

SINGLE ARRAY RORPTIVE STRAIGHT RUN

FLTERED ESTIMATE OF TRAJECTORY WITH NOISE90 "-

U C3>- 0 -

-20 0 20 40o 60 80 100 "

S2

X['K/K3 (FT) 4,10 ' 'Z?

Figure 5.73 Filtered Estimate o+ Tr'ajectory o+ the Torpedo ]"]

During a Straight Run through Single Array-.'-

SINGLE A:RRAY ADAPTIVE STRA:IGHT RUN "' -

Sqt,0THED ESTIMTE OF TRJECTORY WITH NOISE ',-.-

FLC2- .,.-.. .

-20 0 20 40 60 80 100XCK/NJ (FT) 410 2

_. m ,. oO,,,

Figure,5.7 Smiltred Estimate o-f Trajectory of the TorpedoDuring a Straight Run through Single Array

4 ..'?77

SIGE RRY AATIE SRIH U

"°SOH0ETMT F RJCOYWT OS

-- -(a

-. ,F,

O'qlZN,,.,i'

Fiue57-mote siat JTaetoyo h0oreoe .'

Durin a0 tagtRntruh igeArye -

• P ..

*- . . ..

*.1

ILI,

SINGLE ARRAY ADAPTIVE STRAIGHT RUN1IERROR IN FILTERED ESTIMATE WITH NOISELI. •

cc 0 *

><0

0 20 LI0 60 80 100 120

TIME SLOTS

%%.%

Figure 5.75 Error in Filtered Estimate of Position in X of ...

the Torpedo During a Straight Run through Single Array

SINGLE ARRAT AOAPTIVE STRAIGHT RUN


%0

ac

a: a

cc0

I I I I I % .

0 20 40 60 80 100 120

TIME SLOTS %

Figure 5.76 Error in Smoothed Estimate of Position in X ofthe Torpedo During a Straight Run through Single Array

760- .

(%..-

SINGLE RRRRY RRPTIVE STRAIGHT RUN V .ERROR IN FILTERED ESTIMATE WITH NOISE

I- 0 "".'1

-°,. % -

0 20 40 60 80 100 120TIME SLOTS

Figure 5.77 Error in Filtered Estimate o+ Position in Y ofthe Torpedo During a Straight Run through Single Array

SINGLE RRRT ORPTIVE STRRIGHT RUN -


,,;.. - % i

>- 0

- I- 0

02 0 4 0 6 0 8 0 1 0 0 1 2 0,. -• -"TIME SLOTS i:~ !

Figure 5.78 Error in Smoothed Estimate o+ Position in Y o+ ...-.the Torpedo During a Straight Run through Single Array " "'..-

u-s o

79e0'.,4

Fiue..8Ero iomote Etmteo-Pstoni Yo1• . .. .,".,. the - Torpedo,, .. ''W .. , • . .. , .Duri g.aStr igh Run.... - . -. ,,,. .,,. thro gh. S ng ,....,+...+,:...rr .,.. .. .=-. .,. ., +."°y ' ' '

___________________':= v Th' .,.

SINGLE ARRAY RORPTIVE STRAIGHT RUN 1ERROR IN FILTERED ESTIMATE WITH NOISE

I- C3

LLI

0 20 40 60 So 100 120

TIME SLOTS

Figure 5.79 Error in Filtered Estimate of Position in Z othe Torpedo During a Straight Run through Single Array

SINGLE ARRR'I ADAPTIVE STRAIGHT RUN


'cc

ccN ,

.. . . .. -,. -iLEO

rj

0 20 40 60 80 100 120TIME SLOTS

Figure 5.80 Error in Smoothed Estimate of Position in Z ofthe Torpedo During a Straight Run through Single Array

E30* ,.Z:SO'

*,* .

4m

- °

FILTERED ERROR COVRRIANCE P(K/K) WITH NOISE %

" Jq I IT i i i;-

0 20 ,40 60 80 100 120 -.TIME SLOTS

Figure 5.81 Variance o+ Filtered Position Error in X.o+ the

Torpedo During a Straight Run through Single Array--

-" SINGLE ARRAY ADAPTIVE STRAIGHT RUN"""

'-"SMOOTHED ERROR COVAIAqNCE PK/N) WTH NOISE""

9

17-1

0 20 40 . 60 80 too 20 .'."

TIME SLOTS"

) Figure 5.82 Variance o+ Smoothed Position Error in X o+ the""", Torpedo During a Straight Run through Single Array

81 .'N - e

2 0

SINGLE ARRAY ADAPTIVE STRAIGHT RUN

FILTERED ERROR COVRRIRNCE P(K/K) WITH NOISE%I C,

o

. -N

JL

0 20 40 60 80 100 120

,, TIME SLOTS " ''

Figure 5.83 Variance of Filtered Velocity Error in X of the

Torpedo During a Straight Run through Single Array

SINGLE ARRAY ADAPTIVE STRAIGHT RUN

SMOOTHED ERROR COVRRIRNCE P1K/N) WITH NOISE -

U-

0 20 40 60 80 100 120" "

TIME SLOTS

Figure 5.84 Variance of Smoothed Velocity Error in X of the~i!Torpedo During a Straight Run through Single Array :

(E32 82 - Cf

0

p... .:

,p.....

SINGLE RRARY ROAPTIVE STRRIGHT RUNFILTERED ERROR COVRRIRNCE P(K/IK) WITH NOISE

r-1p0 20 40. 60 so 10 2

p....'

Oi 20 IME L60 80 1100 120

TIME SLOTS

Figure 5.85 Variance of Filtered Position Error in Y of theTorpedo During a Straight Run through Single Array

SINGLE RRRR RDRPTIVE STRAIGHT RUN

SMOOTHED ERROR COVRRIRNCE P(K/N) WITH NOISE

{0

C2

C3

M

en

0 20 40 6 0 80 too 120TIME SLOTS,

Figure 5.86 Variance of: Smooth~ed Position Error in Y of the ,i :

Torpedo During a Straight Run through Single Array ,

-- W0

N- -, .z R-i.

, - ' . -- - . . . . ' . _ . . . : , . , _ , ' _ . _ .' . . _ - . _ € . . . ,, . . , - _ . " _ . .. . . ;' - . . , . u , .. . . . - - -'--N, '

-7 . am 7 77- - - - , 7 - - - - -.. - - - , .-7, . --- . --a.- -* -a -7.7-7... - -.

SINGLE ARRAY RDAPTIVE STRAIGHT RUN

FILTERED ERROR COVRRIRNCE P(K/K) WITH NOISE a.,,

S. a '- 1

.. 1- A t.

0'' 20 40O 60 80 1O0 120' "" TIME SLOTS ' ;:

V~i Figure 5.87 Variance of Filtered Velocity Error in Y o+ the '' -"Torpedo During a Straight Run through Single Array

v-'a c\- II .

,.'. SINGLE ARRAY ADAPTIVE STRAIGHT RUN .,€'.

,,- . SMOOTHED ERROR COVARIANCE P(K/N) WITH NOISE . -.

-U N.

0 20 40 60 80 100 120 a,'.

TIME SLOTS

Figure 5.88 Variance of Smoothed Velocity Error in Y of the '.""

Torpedo During a Straight Run through Single Array

..

k". "-%(.,1

*.' • •

-,% -- .% ". .- , , , .* " ." , ." % •" . . ,. - , .,% "l a ,•. ,•l e

p% %°

____ ___ ___ ___ ____ ___ ___ ___...,

SINGLE ARRRT ADAPTIVE STRAIGHT RUN

FILTERED ERROR COVRRIRNCE P(K/K) WITH NOISE

9

CA - -- --

"- o -w..

20 LID 60 80 100 120 - -

TIME SLOTS

,....'1

Figure 5.89 Variance o+ Filtered Position Error in Z o+ theTorpedo During a Straight Run through Single Array .-

SINGLE ARRAT ADAPTIVE STRAIGHT RUN

SMOOTHED ERROR COVRRIRNCE PtK/N) WITH NOISEC2o

C2

L . ...-p. ('.8-°.

0 20 4O 60 80 100 120TIME SLOTS

Figure 5.90 Variance o Smoothed Position Error in Z of the ,Torpedo During a Straight Run through Single Array

85

l.~d .'** *~'* :*. * --. . . . . . .

APPENDIX APROGRAM DESCRIPTION

A. GENERAL

The sequential extended Kalman filter and Smoothing

routine is described in detail by [Ref. 1]. Implementation

is done by using FORTRAN77 compilers on IBM-PC. [Re+. 10,

11, 12, 13]. .-S

B. RUNNING THE PROGRAM ON THE IBM-PC

These directions apply for the IBM-PC computer or other

computers(compatibles) with two floopy disk-drives, 640P",

memory, color/graphic board, math coprocessor and paralel

dot matrix printer or printer/plotter. The software utilized

during the simulation studies are:

1. Operating System DOS 2.10 with required files to -create virtual disk and full screen editorutilities. -

2. IBM Professional FORTRAN Compiler 1.00.

3. Microsoft FORTRAN77 3.20.

4. Plotworks PLOT88.LIB.

5. Source files.

Getting the sequential extended Kalman filter and

smoothing routine started is essentially a five step

process: start your computer; edit the source file and make

required changes and then compile; run the executable file

8.6-.-. .4

...,. ,,

%- h" %"

and get the dat to be avilbl for plttn routine; edit... . .. .

and getute data ob vilo plotting routinadmketenec editr

changes for plotting titles and then compile; run plotting

routine. Start the computer up with an operating system and

* get the program running simply by typing "RUN", which isf. 6 .

-P, given in Appendix E, at promt "A>".

-pZ

APPENDIX BSEQUENTIAL EXTENDED KALMAN FILTER AND OPTIMAL SMOOTHING

PROGRAM LISTING .1

C PROGRAM THESISC

REAL*8 XKKM1 (5) ,PKKM1 (5,5) ,PHI (5,5) ,GAMMA(5,3) ,GATEREAL*8 GAMMAT(3,5),COVW(3,3),CDYV(4,4),QTEMP(5,3),PRf--AL*e TRUX(121),TRUY(121),TRUZ(121),ZI(4),HROW(5)REAL*8 ZHAT,GD8 -NOM,LGDTEMP,PDUM(5,5) ,PI(5,5) ,WHTN,A14REAL*8 ZDIFF(4),ZIC(4),XI(5),XKK(5),PKK(5,5),ZDIFAVREAL*8 DATR(17) ,WINIT,PHIPKK(5,5) ,PKTEMP(5,5) ,SIGACCREAL*8 SIGDIV,SIGCC,XKERR(121),YKERR(121),XP6(5,121)REAL*8 HYDRO(6,12),XB(4),YB(4),ZB(4),XSERR(121),TD(3)REAL*G SIGCCC,SIGAAC,SIGDDI,XP(5,121),SMTH(121),GI (5)REAL*8 P5(121,5,5),SSI(121,5,5),PI(121,5,5),Q(5,5)REAL*e YSERR(121),ZSERR(121),GNUM(5),PHIT(5,5),XPI(5)REAL*e ZDIFTO,CH(5,5),TEMP1(5,5),XNNM1(5),TEMP2(5)REAL*8 AK(5,5),AKT(5,5),TEMP3(5),TEMP4(5,5),XKKS(5)REAL*6 PNNM1(5,5),TEMP5(5,5),TEMP6(5,5),PKKS(5,5)REAL*8 SS2(5),P2(5,5),SSS(5,5),SS3R(5,5),SIGREAL*e ZKERR (121) ,XlKERR, X2KERR,YlKERR,Y2KERR, ZlKERRREAL*8 Z2KERR,XISERR,X2SERR,YISERR,Y2SERR,ZlSERRREAL*8 Z2SERR

C COORDINATES OF HYDROPHONE ARRAY, FOR MULTIPLE ARRAYDATA HYDRO/36000. ,30000. ,24000. ,16000., 12000. ,6000.

+ ,6*6000. ,6*0.0,36030. ,30030. ,24030. ,18030. ,12030.+- ,6030.,6*6000.,6*0.0,36000.,30000.,24000.,18000.+ ,12000.,6000.,6*6030.,6*0.0,36000.,30000.,24000.+- le600. ,12000. ,7*6000. ,6*30. /

CDATA PKKM1/1000.0,5*0.0, 1000.0,5*0.0,1000.0,5*0.0

+- 1000.0,5*0.0,1000.0/DATA PHI/i. ,4*0. ,1.31,1. ,5*0. ,1.,4*0. ,1.31,1. ,5*O.

DATA GAMMA/0.858,1.31,5*0.0,0.858,1.31,5*0.0, 1.31/DATA COVW/1.0,3*0.0,1.0,3*0.0,1.0/,WINIT/0.49/DATA COVV/1 .OD-8,4*0.0, 1.OD-6,4*0.0, 1.OD-6.,4*0.0

C DATA FOR MULTIPLE ARRAY TRACKINGDATA DATR/38000.,7000.,300.,-5O.,0.,3*0.,3*0.

+-,4. 712-'389,. 1745329,. 1,8. 1,600. ,800. /DATA XKKMI/37975. 0,-5O. 0,6975.0,0.0,300.0/

C SECOND DATA FOR MULTIPLE ARRAY TRACKINGC DATA DATR/35000.,7000.,300.,-50. ,0.,3 *0.,3*0.C 4,4.712389,.1745329,.1,8.1,600.,800./ J.

C DATA XKKMI/34975. 0,-5O. 0,6975.0,0.0,300.0/

1B

*~~. W-* WT P.: WT- J'.'

%- V. .

C FIRST DATA FOR SINGLE ARRAY TRACKING- '.-.

C DATA DATR/7500.0,1300.0,0.0,-50..0,O.0,3*0.0,3-*0.0 '-

C 4,4.712389,.1745329,0.1,8.1,600.0,800.0/C DATA XKKMI/7475.0,-50.0, 1275.0,0.0,0.0/C SECOND DATA FOR SINGLE ARRAY TRACKINGC DATA DATR/2000.0,1000.0,300.0,-50.O,0.0,3*0.0,3*0.0C i,4.712389,.1745329,0.1,8.1,600.0,S00.0/

C DATA XKKMI/1975. 0,-50. 0,975.0,0.0,300.0/ .C DATA FOR SUBROUTINE GFIND

DATA SIGACC/36.2/,SIGDIV/1.0/,SIGCC/22.2/,NZDIFF/4/DATA 1I/1/,IJ/2/,IK/3/,IL/4/,IM/5/,MINE/1/,JTIME/119/

C OPEN STATEMENTS FOR OUTPUT FILES

OPEN(4,FILE= 'TRUDI.DAT') *OPEN(13,FILE-'RKK.DAT')OPEN (11 ,FILE-'PKN. DAT')OPEN (7 ,FILE-' XKK. DAT*OPEN (B,FILE- XKN. DATE)OPEN (9,FILE- XKERR. DAT*)OPEN(10,FILE-'XSERR.DAT')OPEN (12,FILE= 'OUTPUT. DAT )

CSIGCC - (SIGCC * 3.141592654) /180.0

C TRANSPOSE OF GAMMA MATRIXCALL TRANS (GAMMA, IM, 1K, GAMMAT)

C TRANSPOSE OF PHI MATRIXCALL TRANS(PHI,IM,IM,PHIT)

C USE THESE STATEMENTS TO CALCULATE CONSTANT 0-MATRIXC CALL PROD(GAMMA,COVW,IM,IK,IK,QTEMP)C CALL PROD(QTEMP,GAMMAT,IM,IK,IM,Q)

ITIME -JTIME 1C USE THIS STATEMENT FOR MULTIPLE ARRAY TRACKING

17 =0

C TIME SLOTS START HEREDO 128 KK = 1 , ITIMEWRITE(*,562) KK

562 FORMAT(/,IOX,'TIME SLOT IN FILTERING :',15)C

C CHOSE THE HYDROPHONE ARRAY FOR MULTIPLE ARRAY TRACKINGIF(XKKM1(1).GE.33000.0) 18 1IF( (XKKM1W). GE. 27000. 0).*AND. (XKKM1( 1) .LT. 33000. 0))%.%

4+ Is=IF( (XKKM1 (1) .GE.215000.0) .AND. (XKKM1 (1) .LT.21000.0))

4-+=

IF( (XKKM1(1) .GE.19000. 0) .AND. (XKKM1( 1) .LT.1000. 0))

+ I - 5IF(XKKM1(1).LT.9000.0) 18 6

89

. Z

'F:.

207 DO 205 13 - , IL14 - 3 * 1315 = 14 -216 = 14-- 1XB(13) - HYDRO(I8,I5)YB(13) = HYDRO(I8,I6)

ZB(13) - HYDRO(IS,I4)205 CONTINUE

C WRITE THE COORDINATES OF CHOSEN HYDROPHONE ARRAYIF(I7.NE.I8) THENWRITE(*,217) I8,KK

217 FORMAT(/,10X,'ARRAY ",12+ , STARTS TRACKING AT TIME ',13)

WRITE(*,216) KK , 18,(I3,XB(I3),YB(I3)+ ",ZB(I3),I3 = 1 , IL)

216 FORMAT(I5,15,4(TII,15,3(2X,D14.8),/))17 - 1"

ENDIF

C CALCULATE THE TRUE TIMES AND THE TRUE TRAJECTORY --C USE THIS CALL STATEMENT FOR MULTIPLE ARRAY TRACKING

CALL TRJC3(KK,DATR,ZI, TD,XB,YB,ZB)C USE THIS CALL STATEMENT FOR SINGLE ARRAY TRACKINGC CALL TRAJEC(KK,DATR,ZI,TD)

A14 = PHI(1,2)TRUX(KK) = TD(1)TRUY(KK) = TD(2)TRUZ(KK) - TD(3)

CWRITE(4,306) KK,TRUX(KK),TRUY(KK),TRUZ(KK)

C .... " . . . . . . . . . . . ... .. .. .. .... .......... .... - .. . .........

C ROW OFH - MATRIXMINE = 1 -

163 DO 132 IROW = 1 , ILNZDIFF = 4

C USE THIS CALL STATEMENT TO RUN NOISE SUBROUTINECALL NOISE(WINIT,WHTN)WHTN 1.0/3.0) * WHTN

C ZERO NOISEC WHTN = 0.0 .* .'C USE THIS CALL STATEMENT FOR SINGLE ARRAY TRACKINGC CALL CHROW(IROW,XKKM1,HROW)C USE THIS CALL STATEMENT FOR MULTIPLE ARRAY TRACKING

CALL CHROWZ(IROW,XKKM1,HROW,XB,YB,ZB)

C 6 I K I C P C K / K - 1 ](5x5) * HT(5xI) / H(Ix5)C *P E K / K -1 ](5x5) * HT(5x1) + COVE V ](1)

CALL MMULT(PKKM1,HROW,IM,IM,GNUM)CALL VMULT (HROW,GNUM, IM,GDTEMP) 1-A

90

V

.,.. . . .. . . . . . . . . . . . . . . . . .

GDENOM = GDTEMP + COVV(IROW,IROW)DO 134 IX = 1 , IMGI(IX) = GNUM(IX) / GDENOM•

134 CONTINUE- -.

C###################################################### .

C P I K / K I = I(5x5) - G C K ](5xl) *H (Ix5) I .,.

C * P I K / K -1 ] .- ,DO 135 IP 1 , IMDO 136 JP 1, IMPDUM(IP,JP) - (-1. * GI(IP)) * HROW(JP)IF(IP.EQ.JP) PDUM(IP,JP) = 1. + PDUM(IP,JP)

136 CONTINUE135 CONTINUE

CALL PROD(PDUM,PKKM1,IM,IM,IM,PI)

C CALCULATE THE PREDICTION OF MEASUREMENTSC USE THIS CALL STATEMENT FOR SINGLE ARRAY TRACKINGC CALL CZHAT(IROW,XKKM1,ZHAT)C USE THIS CALL STATEMENT FOR MULTIPLE ARRAY TRACKING

CALL CZHAT3(IROWXKKM1,ZHAT,XB,YB,ZB)ZIC(IROW) - ZI(IROW) + WHTN * 0.00001ZDIFF(IROW) = ZIC(IROW) - ZHAT

C THREE SIGMA GATEP=DMAX1(DABS(PI(1,)),DABS(PI(3,3)),DABS(PI(5,5)))SIG-DSQRT( (P/((4860.)**2) )+(DABS(COVV(IROW, IROW))))GATE - 3.0 * SIG

CIF(KK.LE.4) GO TO 149IF(DABS(ZDIFF(IROW)).LT.GATE) GO TO 149

CWRITE(*,147) KK,IROW,GATE

147 FORMAT(//,1OX,'THREE SIGMA GATE HAS BEEN EXCEEDED'+ , AT TIME ',14,' IN ROW ',12,' GATE : ',D14.8)

DO 148 LGJ - 1 , IMGI(LGJ) = 0.0

148 CONTINUEC TAG INVALID TIME MEASUREMENT

ZDIFF(IROW) = 999.CC<<<<4,<<<< << <<<<<<<<<< <<<<< <<<<<< < :<<<<<<<<<<<<< <<,<:.

CX I K / K 3 = X [ K / K - 1 I + G I K I * (Z [ K 3D CMZ C K / K-I ]J.

149 DO 150 1 = I IMXI(I) = XKKM1(I) + GI(I) * ZDIFF(IROW)

150 CONTINUE

C< < <<<< < << , < ' < <<<< <<<<<<< <<<<<<<<<<K,.IF(IROW.EQ.4) GO TO 152DO 153 1= 1 , IMXKKMI(I) = XI(I)

153 CONTINUE

91 ,"-S..

" "i %:S *

DO 155 1 1, IMDO 154 J =1, IMPKKM1(I,J) -PI(I,J)

154 CONTINUE155 CONTINUE132 CONTINUE

152 DO 156 I = 1, IM

XKKMI(I) = XI(I)

C USE THIS ADDITIONAL STATEMENT FOR SMOOTHINGXP6(I,KK) = XI(I)

CDO 157 J = 1 , IMPKK(I,J) = PI(I,J)PKKM1(I,J) = PI(I,J)

C USE THIS ADDITIONAL STATEMENT FOR SMOOTHINGP5(KK,I,J) = PI(I,J)

C 157 CONTINUE I156 CONT INUE '

C>>>>>>>>)>>>>>>>>>>>>>>°>\ >>>°''.> >>>>>>>>>>>>>>> >, >>. >>

C PREDICTION OF MEASUREMENTS BASED ON XrK/K]

DO 158 1 = 1 , ILC EDIT INVALID TIME MEASUREMENTS

IF(ZDIFF(I).GE.999.) GO TO 159C USE THIS CALL STATEMENT FOR SINGLE ARRAY TRACKINGC CALL CZHAT(I,XKKM1,ZHAT)C USE THIS CALL STATEMENT FOR MULTIPLE ARRAY TRACKING

CALL CZHAT3(I,XKKM1,ZHAT,XB,YB,ZB)ZDIFF(I) = DABS(ZIC(I) - ZHAT)

GO TO 158159 ZDIFF(I) = 0.

NZDIFF = NZDIFF -1158 CONTINUE".,>.,,,.,,.,,,,. >> >>> ;,>>>>>>>>> "" > > > > ,> >,, .>- ,,,, > "., > .. ,., , ,.. ,... ...

C ABSOLUTE AVERAGE VALUE OF THE DIFFERENCES IN MEASUREMENTIF(NZDIFF.EQ.O) GO TO 160 .4ZDIFTO = DABS(ZDIFF(1)+ZDIFF(2)+ZDIFF(3)+ZDIFF(4))ZDIFAV = ZDIFTO / NZDIFFIF(KK.LE.4) GO TO 160IF(MINE.EQ.I) SMTH(KK) = ZDIFAVIF(MINE.GT.3) GO TO 160

C USE THIS CONSTANT (2.OD-6) GATEIF(ZDIFAV.LT.2.OD-6) GO TO 160

CWRITE(*,1473) KK

1473 FORMAT(//,1OX,'CONSTANT GATE HAS BEEN EXCEEDED AT ',.*,+ ,"TIME ',144 '

C( ( ( ((( ( ( (((( ( ((((((((C(( (((( ( ( C( ( CC(( C( ( ( ( ( ( (( ( C( ( C ( C t. -

C USE THESE STATEMENT TO INCREASE THE GAIN IN MULTIPLE &"-

91..,1

92 ,- '

S.. , ."l

... ] 5.':

• . ", ",. "I

-. 6

C SINGLE ARRAY TRACKINGSIGAAC = 3.0 * SIGACCSIGODI = 3.0 * SIGDIVSIGCCC = 3.0 * SIGCC

CC USE THIS CALL STATEMENT TO CALCULATE ADAPTIVE 0-MATRIX

CALL QFINDCKK,XKK,PKK,SIGAAC,SIGDDI ,SIGCCC,A14,Q)CALL ADD(PKK,Q,IM,IM,PKKM1)MINE -MINE + 1%Ga TO 163

160 MINE I

C

WRITE(7,301) KK,(XKK(J)1 3 1 ,IM)WRITE(13,301) KK,(PKK(I,I),I I , IM)

301 FORMATCI5,5(4X,D14.8))C

XKERR(KK) = XKKC1) - TRUXCKK)YKERR(KK) - XKK(3) - TRUY(KK)ZKERR(KK) - XKK(5) - TRUZ(KK)r. .

CWRITE(9,306) KK,XKERR(KK'),YKERRCKK) ,ZKERRCKK)

306 FORMAT(I5,3(4X,D14.8))C DETERMINE MAX & MIN ERRORS AND THE TIME SLOTS

IF(KK.EQ.l) THENKXlK = KKKX2K = KKKYlK - KKKY2K = KKKZIK = KKKZ2< = KKXIKERR = XKERR(KK)X2KERR - XKERR(KK)Y1KERR - YKERRCKK)Y2KERR = YKERR(KK)ZIKERR -ZKERRCKK)Z2KERR = ZKERRCKK)

ENDIFIF(XKERR(KK).GT.XIKERR) KXlK =KK

IF(XKERR(KK).GT.XlKERR) XlKERR = XKERR(KK)IF(XKERR(KK).LT.X2KERR) KX2K = KKIF(XKERRCKK).LT.X2KERR) X2KERR = XKERRCKK)IF(VKERRCKK).GT.Y1KERR) KYlK = KK -..IF(YKERR(KK).GT.YlKERR) YIKERR = YKERR(KK)IF(YKERR(KK).LT..Y2KERR) KY2K = KKIF(YKERR(KK).LT.Y2KERR) Y2KERR - YKERR(KK)IF(ZKERRCKK).GT..ZlKERR) KZlK = KKIF(ZKERR(KK).GT.ZIKERR) ZlKERR = ZKERR(KK)IF(ZKERRCKK).LT.Z2IKERR) KZ2K = KI(IF(ZKERR(KK).LT.Z2KER7) Z2KERR =ZKERR(KK)

93

C P C K + 1I K J=(PHI(5x5) *P E K /K ](5y5) *PHIT(5x5)"

C +4Q E I)C USE THIS CALL STATEMENT TO CALCULATE ADAPTIVE 0-MATRIX

CALL OFIND (KK, XKK ,PKK,SIGACC,SIGDIV ,SIGCC,A14 ,2)CALL PROD(PHI,PKK,IM.,IM,IM,PHIPKK)

V CALL PROD(PHIPKK,PHIT,IM,IM,IM,PKTEMP)

V C CALL ADD(PKTEMP1 Q,IM,IM,PKKM1)

CALL MMULT(PHI,XKK,IM,IM,XKKM1)

C USE THESE STATEMENTS FOR SMOOTHINGDO 302 I=1 , IMXP(IG,KK) =X.KK(I6)

302 CONTINUEDO 303 111 1 IMDO 304 JJJ =1 ,IM

SS1(KK,III,JJJ) =PPKM1(III,JJJ)

Pl(KK,III,JJJ) =PKK(III,JJJ)


C SMOOTHING STARTS HEREIF(KK.LE.JTIME) GO TO 128DO 500 K = I JTIME

YKI =JTIME-K+ 1WRITE(*,561) KI

561 FORMAT(/,I0X,,'IN SMOOTHING AT TIME :',15)DO 501 1 1 , IMXPI(I) =XP6(I,KI)

501 CONTINUEDO 502 1 1 1, IMDO 503 3 = 1 , IMP2(1,J) = P5(KI,I,J)SS3(I,J) = SS1 (11,I,3;IF(KI.LE.4) GO TO 503IF(SMTH(KI).GE.2.OD-6) SS3(I,J) =3.6 *SSZ(I,J)


C A(K) =P(K/K) * TRANSPOSEEPHI) INVEP(K"-1/K)JCALL TRANS(PHI,IM,IM,PHIT)CALL RECIP(SS3,IM,SS3R)CALL PROD(SS3,SS3R,IM,IM,IM,CH)CALL PROD(PHIT,SSZR,IM,IM,IM,TEMPI)CALL PROD(P2,TEMPI,IM,IM,IM,AK)

C X(K/N) = X(K/K) + A(K) * I X(K'-1/N) -X(K+1/K) IDO 504 1 1 , IMXNNM1(I) =XP(I,KI4-)

504 CONTINUE

49

CALL MMULr(PHI,XP1,IM,IM,SS2)CALL SUB(XNNM1,SS2,IM,1,TEMP2)CALL PROD(AK,TEMP2,IM,IM,1,TEMPZ)CALL ADD(XP1,TEMP3,IM,1,XKKS)DO 505 1 1, IM

XP(I,KI) XKKS(I)505 CONTINUE

WRITE(8,301) KI,(XKKS(J),J = I IM)XSERR(KI) = XKKS(l) - TRUX(KI)YSERR(KI) = XKKS(3) - TRUY(KI)ZSERR(KI) = XKKS(5) - TRUZ(KI)WRITE(1O,306) KI,XSERR(KI),YSERR(K'I),ZSERR(KI)

C DETERMINE MAX & MIN ERRORS AND THE TIME SLOTSIF(K.EQ.1) THENKXIS = KIKX2S = KIKYIS = KIKY2S = KIKZ1S = KIKZ2S = 1(1XISERR = XSERR(KI)X2SERR = XSERR(KI)Y1SERR - YSERR(KI)Y2SERR = YSERR(KI)ZISERR = ZSERR(KI)Z2SERR = ZSERR(KI)

ENDIFIF(XSERR(KI).GT.XlSERR) KXlS =KI

IF(XSERR(KI).GT.XISERR) XISERR =.XSERR(KI)IF(XSERR(KI).LT.X2SERR) KX2S = KIIF(XSERR(KI).LT.X2SERR) X2SERR = XSERR(KI)IF(YSERR(KI).GT.YlSERR) KYlS = KIIF(YSERR(KI).GT.YlSERR) YlSERR = YSERR(KI)IF(YSERR(KI).LT.Y2SERR) KY2S = KIIF(YSERR(KI).LT.Y2SERR) Y2SERR = YSERR(KI)IF(ZSERR(KI).GT.ZlSERR) KZlS = KIIF(ZSERR(KI).GT.Z1SERR) ZISERA = ZSERR(KI)IF(ZSERR(KI).LT.Z2SERR) K'Z2S = KIIF(ZSERR(KI).LT.Z2SERR) Z2SERR = ZSERR(KI)

C P(K/N) = P(K'/K)+A(K)*[P(K'+l/N)-P(K,+l/K))*TRANSPOSEEA(K)IDO 506 I 1 IMDO 507 J 1 ,IM

PNNM1(I,J) =P1(KI+1,I,J)507 CONTINUE506 CONTINUE

CALL SUB(PNNM1,SS3,IM,IM,TEMP4)CALL TRANS(AK,IM,IM,AKT)CALL PROD(rEMP4,AKT,IM,IM,IM,TEMP5)CALL PROD(AK,rEMP5,tm,im,im,rEMP6)

95

CALL ADD(P2,TEMP6,IM,IM,PKKS) IDO 509 J I ,IM

Pl(KI,I,J) =PKKS(I,J)

509 CONTINUE

509 CONTINUE $S5 55$

500 CONTINUE

129 CONTINUE NWRITE( 12,600)

800 FORMAT(I0X,' TLME%,4X,' MAX. ERROR ',4X,' TIME',4X+' MIN. ERROR )

WRITE(12,901) KXlK,XIKERR,KX2K,X2KERR,KYIK,YlKERR,+KY2KY2KERKZKZIERRZ2K,2KE1WRITE( 12,801) KX1S,X1SERR,KX2S,X2SERR,KYlS,Y1SERR,+KY2S,Y2SERR,KZlS,ZISERR,KZ2S,Z2SERR

S01 FORMAT(3(/,IOX,I5,4X,D14..8,4X,I5,4X,D14.8))STOP

END hiC

SUBROUTINE TRANS(AA,NR,NC,BB)REAL*8 AA(NR,NC),BB(NC,NR)DO 3 1 - I , NRN

99(J,1) -AA(I,3)


RETURNEND

C4SUBROUTINE PROD (AA,BB,NRA,NCA,NCB,CC)REAL*8 AA(NRA,NCA) ,BB(NCA,NCB) ,CC(NRA,NCB)DO 4 1 - I , NRADO 403 1I, NCBCCCI,J) =0.0


DO 411 -1I, NRADO 4103 J 1, NCBDO 411 K= 1I NCA

=CIJ C-C-I ) .4 A(IK) 88(K.1)

411 CONTINUE


RETURNEND

CSUBROUTINE TRAJEC(KK,DATR,ZI ,TD)

96

REAL*8 DATR(17),ZI(4),TD(3),COEFF,RANGE,VEL,TDATA VEL/4860. 0/,I111<3/, IIM/5/ ~T - 0.0COEFF = 1.0 ., 'dELZI(l)=COEFF*DSQRT(((DATR(1)+15.0)**2)+ 4((DATR(2)e-15.0)**2)+((DATR(3S)+15.0)**2))ZI(2)=COEFF*DSQRT(((DATR(1)-15.0)**2)

+ +((DATR(2)+15.0)**2)+((DATR(3)4-15.0)**2))ZI (3)-COEFF*DSQRT( ((DATR(1)+15.0)**2)

+ +((DATR(2)-15.0)**2)+((DATR(3)+15.0)**2))ZI(4)-COEFF*DSQRT(((DATR(1)+15.0)**2)

+ +((DATR(2)+15.0)**2)+((DATR(3)-15.0)**2))DO 5 1 = 1 , 111<TD(I) = DATR(I.)

5 CONTINUEC USE THIS STATEMENT FOR STRAIGHT RUNC IF((KK.LE.OATR(17))..AND.(KK.G3T.DATR(16))) GO TO 50C USE THESE STATEMENTS FOR MANEUVERING RUN58 IF((KK.LE.49).AND.(KK.GT.22)) GO TO 50

IF((KK.LE.98).AND.(KK.GT.71)) GO TO 50IF( (KK.EQ.50) .OR. (KK.EQ.99)) THEN

C

C FIRST DATA FOR TRUE TRAJECTORY IN SINGLE ARRAY TRACKINGC DATR(2) - 1300.0C DATR(3) - 0.0CC SECOND DATA FOR TRUE TRAJECTORY IN SINGLE ARRAY TACKING

DATR(2) - 1000.0DATR(3) - 300.0 -

DATR(4 = -50.0DATR(5) = 0.0DATR(6) - 0.0DATR(7) - 0.0DATR(S) - 0.0DATR(9) - 0.0DATR(10) - 0.0DATR(11) - 0.0DATR(12) - 4.71239DATR(13) - 0. 1743329DATR(14) -0.1DATR(15) - 8.1

ENDIFC57 DATR(7) = 0.0

DATR(8) - 0.0DATR(14) - 1.31GO TO 51VA

50 DATR(14) - 0.00553 DATR(12) - DATR(12) +DATR(13) *DATR(14)

DATR(7) - DATR(15) * DCOS(DATR(12))DATR(8) - DATR(15) * DSIN(DATR(12))

97

* %*~\

*~ ~ ~ ~ ~ ~~~4 :.~.* ~**9.~.

- r. v~r..w~ WW'.~ . w~ . ~w~ - w~.. ~ *.~ .-~r777.

51 DO 52 1 - 1 , IIM%DATR(I) -DATR(I) +DATR(14-3) *DATR(14)

+- + (((DATR(14))**2)/2) *DATR(I+6)

52 CONTINUET -.T + DATR(14)

IFODBTO - 1.31).LE.0.0001) RETURN

ENDc

SUBROUTINE TRJC3(KK,DATR,Z1,TD,XB,YB,ZB) __

REAL*8 DATR(17),ZI(4),TD(3),XB(4),YB(4),ZB(4),COEFFREAL*8 VEL,TDATA VEL/4860.0/,IIK/3/,IIL/4/,IIM/5/T - 0.0COEFF - 1.0 /VELDO 12 1-1I I-ILZI(I) - COEFF *DSQRT((UDATR(1) - XB(I))**2)+ + ((DATR(2) -YB(I))**2) +- ((DATR(3) - ZB(r))**2)) -

12 CONTINUEDO 120 1 - I , IIK

10 TD(I) - DATR(I) .10CONTINUE *...

C USE THIS STATEMENT FOR STRAIGHT RUN* IF((KK.LE.DATR(17)).AND.(KK.GT.DATR(16))) GO TO 121

C USE THESE STATEMENTS FOR MANEUVERING RUNC 126 IF((KK.LE.49).AND.(KK.GT.22)) GO TO 121

C IF((KK.LE.98).AND.(KK.GT.71)) SO TO 121C IF((KK.EQ.50).OR. (KK.Eg.99)) THENC DATR(2) - 7000.04C DATR(3) - 300.0 ZC DATR(4) -- 50.0

-C DATR(5) - 0.0C DATR(6) - 0.0C DATR(7) - 0.0C DATR(8) - 0.0C DATR(9) - 0.0C DATR(10) = 0.0C DATR(11) = 0.0C DATR(12) - 4.712389C DATR(13) - 0.1745329C DATR(14) - 0.1 ~w

9C DATR(15) -61.1C ENDIFC127 DATR(7) - 0.0 /.

DATR(8) - 0.0DATR(14) - 1.31GO TO 122

121 DATR(14) = 0.005 '124 DATR(12) = DATR(12) + DATR(13) *DATR(14)

DATR(7) -DATR(15) *DCOS(DATR(12))

* 98

t.M~~J %

aka

DATR(6) =DATR(15) *DSIN(DATR(12))

122 DO 123 1-=1 , IIMDATR(I) - DATR(I) + DATR(I+3) * DATR(14)

+ +- (((DATR(14))**2)/2) *DATR(14-6)

123 CONTINUET - T +DATR(14)IF(DABS(T - 1.31).LE..0.0001) RETURN8O TO 124END

SUBROUTINE CHROW(IROW,XKKM1 ,HROW)REAL*8 HROW (5) ,XKKM1 (5) ,COEFF,DENOM,DENOM1 ,DENOM2REAL*9 VEL,A1 ,A2,A3,DENOM3,DENOM4DATA VEL/4660.0./COEFF - 1.0 / VEL % ~DENOMl-DSQRT( ((XKKM1 (1)415.0)**2)-( (XKKM1 (3)4-15.0)**2)

+ +( (XKKM1 (5)4-15.0)**2))DENOM2=DSQRT( ((XKKM1 (1)-15.0)**2)-( (XKKMI (3)+15.0)**L))

4- 4((XKKMI(5)4-15.0)**2))DENOM3-DSQRT(((XKKM1(1)-15.0)**2)-((XKKMI(3)-15.0)**2-')

4- -( (XKKMI (5)4-15.0)**2))DENOM4-DSQRT(((XKKM(1)4-15.0)**2)4-UXKKM1(3)4-15.O)**2)

+ 4((XKKMI(5)-15.0)**2))Al = 1.0A2 - 1.0A3 = 1.0DENOM - DENOM1IF(IROW.EM.2) DENOM - DENOM2IF(IROW.EQ.3) DENOM - DENOM3IF (IROW. ED.4) DENOM = DENOM4IF (IROW. EQ. 2) Al - -1.0

HROW(1) - COEFF *((XKKM1(1) +- Al *15.0) /DENOM)IF(IROW.EQ.3) A2 - - 1.0HROWr(3) - COEFF *((XKKMI(3) +- A2 * 15.0) / DENOM)IF(IROW.ED2.4) A3 -- 1.0HROW(5) - COEFF *((XKKM1(5) 4- A3 *15.0) /DENOM)HROW(2) -0.0 -~

HROW(4) - 0.0RETURNEND

CSUBROUTINE CHROW3(IROW,XKKM1 ,HROW,XB,YB,ZB)REAL*g HROW(5),XKKMI(5),COEFF,DENOM,VEL,XB(4),Y(4)REAL*e XD,YO,ZO,ZB(4) -~-

DATA VEL/4B&0.0/COEFF - 1.0 / VELXO - XB(IROW) WYO - YB(IROW)ZO - ZB(IROW)DENOM DSQRT(((XKKM1(1)-XD)**2)4-((XKKM1(3)-Y0)**2)

4lb+ +- ((XKKM1(5)-ZO)**2))

99

HROW(1) - COEFF * ((XKKM1(l) - XO) / DENOM) *

HROW(2) - 0.0HROW(3) - COEFF * C(XKKMI(3) - YO) / DENOM)HROW(4) - 0.0HROW(5) = CflEFF * ((XKKMI(5) - ZO) / DENOM)bRETURN

END

SUBROUTINE MMULT(AA,BB,NRA,NCA,CC)* REAL*B AA(NRA,NCA) ,BB(NCA) ,CC(NRA)

DO 61 -1I, NRACCCI) = 0.0DO 60 J - 1 , NCA

* CC(I) - CC(I) + AA(I,J) * 6(3)60 CONTINUE6 CONTINUE

RETURNEND

CSUBROUTINE VMULT(AA,BB,NE,CC)REAL*e AA(NE) ,BB(NE) ,CCCC = 0.0DO 7 1 - 1, NECC - CC + AAMI * 9(I)

7 CONTINUERETURNEND

CSUBROUTINE CZHAT(IROW,XKKM1 ,ZHAT) IREAL*8 XKKMI (5) ,ZHAT ,COEFF ,VELDATA VEL/46.0/COEFF - 1.0 / VELIF(IROW.EQ.1) ZHAT=COEFF*DSQRT(((XKKM1c1).15.o)**2.-Ai+ ((XKKMI(3)+15.0)**2)+((XKKMI(5)-15.0)**2))IF(IROW.EQ.2) ZHAT=COEFF*DSQRT(((XKKM1(1)-15.)**2).

+ ((XKKMI(3)+15.0)**2)+((XKKM1(5)+15.0)**2))IF(IROW.Eg.3) ZHAT=COEFF*DSQRT(((XKKM1(1)+15.0)**)4

+ ((XKKM1(3)-15.0)**2)4-((XKKMI(5)+15.0)**2))IF(IROW.EO2.4) ZHAT=COEFF*DSQRT(( (XKKMI(1)+15.0)**2)+

+ ((XKKMI(3)+15.0)**2)+((XKKMI(5)-15.0)**2))RETURNEND

CSUBROUTINE CZHAT3(IROW,XKKM1,ZHAT,XB,YB,ZB)REAL*8 XKKM1(5),ZHAT,COEFF,VEL,XB(4),YB(4),ZB(4),XOREAL*8 VO,ZODATA VEL/4860.0/COEFF = 1.0 / VELXO =XB(IROW)

YO -YB(IROW)ZO - ZB(IROW)

100

ZHAT -COEFF *DSORT(((XKKM1(1) -XO)**2)

+ ((XKKM1(3) -YO)**2) + ((XK<'KM1(5) -ZO)**2))

RETURNEND

C b

SUBROUTINE NOISE(R,P)REAL*8 Y(6),X(6),S(5),R,P,BB,PIDATA Y/0.0,.0228,.0668,.1357,.2743,.5/DATA X/-3.0l ,-2.0,-1.5,-1.0,-0.6,0.0/DATA S/43.8596,11.3636,7.25689,2.891352,2.65887/BB - 1.0P1 - R * 317.0R =DMOD(P1 , BB)P R

IF(P.GT .0.5) P -1.0 - RB IF(P.LT.Y(I+1)) GO TO 80

GO TO 8830 P - ((P - Y(I)) * (I) *X(I))

IF(R.GE.0.5) P -- PRETURNEND

CSUBROUTINE ADD(AA,BB,NR,NC,CC)REAL*B AA(NR,NC) ,DB(NR,NC) ,CC(NR,NC)DO 91 -1I, NRDO090 J1 , NCCC(I,J) -AA(I,J) + BB(I,3)


RETURNEND

CSUBROUTINE QFIND(KK,XKK,PKK,SIGACC,SIGDIV,SIGCC,A,Q)REAL*8 XKK(5) ,PKK(5,5) ,0(5,5) ,SIGACC,SIGDIV,SIGCC.AREAL*8 A2,A3,B,C,D,E1,E12,E2,G1,G2,G3,SIGAAC.SIGDDIREAL*e SIGCCC,A1INTEGER KKIF (KK. NE. 1) GO TO I111DO 11 1 - 1 , 5DO 110 J 1 ,5Q(1,3) =0.0


SIGACC - SIGACC *420(5,5) - (SIGDIV **2) *(A **2)SIGCC SI51CC **261 = A *2) /2.0G"= G1 **2C- A *G1

101

14

A2 - A 4*2Ill Al = XKK(2) **2 + XKK(4) **2

A3 - XKK(2) /DSORT(A1)B - XKK(4)

* C - XKK(4) /DSORT(A1)D - XKK(2)El = ( A3 *42) * SIGACC + (B **2 )*SIGCC

E12 = A3 * C *SIGACC - B *D * SIGCC 4.E2 - ( C **2 )* SIGACC + (D 4*2) SIGCC..0(1,1) - El *G2

Q(1,2) - G3 *El0(1,3) = E12 *62Q(1,4) = 63 *E12

0(2,2) = A2 *El

Q(2,3) - 63 *E129(2,4) = A2 * E-120(3,3) = 62 * E2

D0 112 1=-1 ,4DO 113 J-I, I

z 0(1,3) = (3,1)113 CONTINUE112 CONTINUE

RETURNEND

SUBROUTINE SUB(AA,BB,NR,NC,CC)REAL*e AA(NR,NC) ,BB(NR,NC) ,CC(NR,NC)DO 12 I - I , NRDO 120 3 - 1 , NCCC(I,3) - AA(I,J) -BB(I,J)


RETURNENDjC SUBROUTINE RECIP(AA,NN,CC)REAL*8 AA(NN,NN) ,DD(5, 10) ,CC(NN,NN)DO14 K=-1 , NND0140 J= 1 , NNDD(K,J) = AA(K,J)


0O 141 K - 1 , NNI =K + NNDO0142 J3=6,10IF(I.NE.J) GO TO 143DD(K,3) = 1.GO TO 142

143 DD(K,J) =0.

1w 102

Ole%

142 CONTINUE141 CONTINUE .N.

M - K + 1

DO 145 J - M , 10DD(K,J) - DD(K,J) / DD(K,K)

145 CONTINUEDD(K,K) - 1.DO 146 L - 1 , NNIF(L.EQ.K) GO TO 146DO 147 I - 1 , 10

IF(I.EQ.K) GO TO 147

DD(L,I) DD(L,I) - DD(LK) * DD(K,I)147 CONTINUE

DD(L,K) = 0.146 CONTINUE

144- CONTINUEDO 148 K I, NN .....

DO 149 -1 , NNI - J + NNCC(K,J) - DD(K,I)


RETURNEND

103

, .,9..

I",

.,:..-

103 ''%.-,J.....

4~* ,. 9.

a..,.',

_j-.

#I

APPENDIX C t %

PLOTTING PROGRAM LISTING FOR HP PLOTTER

$STORAGE:2$DEBUGSNOLIST

CPROGRAM PLOTTER d"

C

CHARACTER*40 TITLECHARACTER*35 LEGEND,SUBT I TLE

CHARACTER*25 NAMEX, NAMEYREAL X(245),Y(245)REAL 0(245),P(245),R(245),S(245),T(245),U(245)INTEGER*2 IC

DATA IC/O/CC USE THESE FOR MULTIPLE ARRAY TRACKINGC TITLE -'MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN'

TITLE -'MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN'CC USE THESE FOR SINGLE ARRAY TRACKINGC TITLE -'SINGLE ARRAY ADAPTIVE MANEUVERING RUN'C TITLE ='SINGLE ARRAY ADAPTIVE STRAIGHT RUN'

OPEN(5,FILE='XKK.DAT' ,STATUS-'OLD')DO 32 LENG = I , 241READ(5,*,END=33) O(LENG),P(LENG),R(LENG),S(LENG),

+ T (LENG) ,U (LENG)32 CONTINUE

33 CONTINUELENG = LENG - 1

CLOSE (5,STATUS= 'KEEP')NAMEX -'X[K/KJ (FT)'NAMEY -'YEK/K] (FT)'SUBTITLE ".LEGEND -'FILTERED ESTIMATE OF TRAJECTORY'CALL DRAWER(TITLE,NAMEX,NAMEY,LEGEND,P,S,LENG,

+ SUBTITLE)OPEN(5,FILE-'PKK.DAT' ,STATUS='OLD')DO 34 LENG - I , 241READ(5,*,END=35) O(LENG),P(LENG),R(LENG),S(LENG), ,...

+ T(LENG),U(LENG)


LENG = LENG - 1CLOSE(5,STATUS='KEEP )NAMEX ='TIME SLOTS'

NAMEY -'(FT**2)' . ..

104

SUBTITLE =*PEK/K3(l,l)'LEGEND ='FILTERED ERROR COVARIANCE P(K/K)'CALL DRAWER (TITLE, NAMEX ,NAMEY ,LEGEND,0, P ,LENG,

+. SUBTITLE)NAMEY -. (4FT/SEC)**2)'SUBTITLE in'PEK/K3(2,2)'CALL DRAWER (TITLE,NAMEX ,NAMEY ,LEGEND,O,R ,LENG,

+- SUBTITLE)NAMEY - (FT**2)'SUBTITLE -'PCK/K3(3,3)'CALL DRAWER (TITLE,NAMEX ,NAMEY,LEGEND,O,S,LENG,

+ SUBTITLE)NAMEY - ((FT/SEC) **2)'SUBTITLE -'PIK/ 'K3(4,4)'CALL DRAWER (TITLE, NAMEX ,NAMEY ,LEGEND ,O, T ,LENG,

+ SUBTITLE)NAMEY -'(FT**2)'SUBTITLE ='PEK/KJ(5,5)'CALL DRAWER (TITLE, NAMEX ,NAMEY ,LEGEND, 0, U,LENG,

+ SUBTITLE) .*% .

OPEN(5,FILE-'XKERR.DAT',STATUS-'OLD') .--. 1DO 38 LENS - 1 , 241READ(S,*,END-39) O(LENG) ,P(LENG) ,R(LENG) ,S(LENG)

38 CONTINUE -~39 CONTINUE

LENS - LENS - I

CLOSE(5,STATUS- KEEP')NAMEY -*X ERROR (FT)'SUBTITLE -

LEGEND -'ERROR IN FILTERED ESTIMATE'CALL DRAWER (TITLE, NAMEX ,NAMEY,LEGEND,O,P,LENG,

+- SUBTITLE) -

NAMEY -VY ERROR (FT)* ~CALL DRAWER(TITLE,NAMEX ,NAMEY,LEGEND,O,R,LENG,

+ SUBTITLE)NAMEY -'Z ERROR (FT)'CALL DRAWER(TITLE,NAMEX,NAMEY,LEGEND,O,S,LENG,

+. SUBTITLE) "-

OPEN'5,FILE='XKN.DAT' ,STATUS-'OLD') '--;

DO 40 LENS - 1 , 241READ(5,*,END=41) O(LENG),P(LENG),R(LENG),S(LENG),

+ T(LENG),U(LENS)40 CONTINUE41 CONTINUE

LENS - LENS - ICLOSE (5,STATUS-'KEEP') %"

NAMEX -'XtK/N3 (F'T)'NAMEY in'VEK/NJ (FT)'LEGEND -'SMOOTHED ESTIMATE OF TRAJECTORY' .4***

CALL DRAWER(TITLE,NAMEX,NAMEY,LEGEND,P,S,LENG.4- SUBTITLE) .

105 4

OPEN(5,FILE='PKN.DAT',STATUS-'OLD')DO 42LENSG= 1 ,241READ(5,*,END-43) O(LENG),P(LENG),R(LENG),S(LENG),

+ - T(LENG),U(LENG)42 CONTINUE

43 CONTINUELENS - LENS -1CLOSE (5,STATUS='KEEP')NAMEX -*TIME SLOTS'NAMEY -, (FT**2)'SUBTITLE -'PtK/N3(l,l)'

*LEGEND -*SMOOTHED ERROR COVARIANCE P(K/N)'CALL DRAWER (TITLE ,NAMEX ,NAMEY ,LEGEND,0, P ,LENG,

+ SUBTITLE)NAMEY -'((FT/SEC)**2)'SUBTITLE -'PEK/NJ(2,2)'CALL DRAWER (TITLE, NAMEX ,NAMEY ,LEGEND, 0, R ,LENG,

A'+4 SUBTITLE)NAMEY -'(FT**2)'SUBTITLE ='PtK/N3(3,3)'CALL DRAWER (TITLE,NAMEX,NAMEY,LEGEND,O,S,LENG,

+ SUBTITLE)NAMEY -*((FT/SEC)**2)'SUBTITLE -'PCK/N3(4,4)'CALL DRAWER (TITLE,NAMEX ,NAMEY,LEGEND,O,T,LENG,

+- SUBTITLE)NAMEY ='(FT**2)'SUBTITLE. -'PEK/N3(5,5)'CALL DRAWER(TITLE,NAMEX,NAMEY,LEGEND,O,U,LENG,

+SUTTE

OPEN(5,FILE='XSERR.DAT' ,STATUS-'OLD' SBITEDO 44 LENG = 1, 241READ(5,*,END=45) O(LENG),P(LENG),R(LENG),S(LENG)

44 CONTINUE4- CONTINUE

LENS - LENS - 1CLOSE (5,STATUS='KEEP')NAMEY -'X ERROR (FT)' ..SUBTITLE -'I.LEGEND ='ERROR IN SMOOTHED ESTIMATE' .-

CALL DRAWER (TITLE, NAMEX, NAMEYLEEN, 0,P ,LENS,+ SUBTITLE)NAMEY ='Y ERROR (FT)'CALL DRAWER (TITLE ,NAMEX ,NAMEY ,LEGEND, 0,R ,LENG,

+ SUBTITLE)NAMEY ='Z ERROR (FT)'CALL DRAWER (TITLE, NAMEX ,NAMEY ,LEGEND ,O ,S ,LENG,

S+ SUBTITLE)STOPENDSUBROUTINE DRAWER (TITLE,NAMEX ,NAMEY,LEGEND, X ,Y,

106

.4. LENG,SUBTITLE)

CHARACTER*40 TITLECHARACTER*35 LEGEND ,SUBT ITLECHARACTER*25 NAMEX,NAMEYREAL X(245) ,Y(245)INTEGER*2 ICDATA IC/O/

CCALL EDCALL CUP(1,0)CALL PLOTS(0,9600,30)CALL SVMBOL(2.0,6.651 .20,TITLE,0.0,40)CALL SYMBOL(2.O,6.25,. 175,LEGEND,0.0,-5)

CC USE THIS FOR NOISELESS TRACKINGC CALL SYMBOL(6.84,6.25,.175,'WITHOUT NOISE',0.0,13)CC USE THIS FOR NOISY TRACKING

CALL SYMBOL(6.84,6.25,.175,' WITH NOISE',0.0,13)C

CALL SYMBOL (1.60,2. 45,. 20,SUBTITLE,90. 0,35)CALL PLOT(1.00,1.0O,-3) -

CALL PLOT(8.0,0.0,3)CALL PLOT(8.O,6.0,2)CALL PLOT (0.0, 6. 0,2)CALL PLOT(0.0,0.0,2)CALL PLOT (B. 0,0.0, 2)CALL SCALE(X,6.00,LENG,l)CALL SCALE(Y,3.00,LENG,1)CALL STAXIS(.1S0,.20,.15,.112,-l)CALL AXIS(1.5,1.5,NAMEX,-13,6.00,00.,X(LENG+1),

+ X(LENG42))CALL STAXIS(. 15,.20,. 111,. 112,2)CALL AXIS(l.5,1.5,NAMEY,13,3.00,90.,Y(LENG+1),

+ Y(LENG+2))CALL PLOT(1.50,1.50,-3)CALL LINE(X,Y,LENG,1,0,3)CALL PLOT(0.0,0.0,999)RETURNENDSUBROUTINE EDCHARACTER*1 Cl,C2,C3,C4INTEGER*2 IC(4)EQUIVALENCE (Cl,IC(l)),(C2,IC(2-)),(C3,IC(3>)),

+ '''s 1L ') )

DATA IC/16#IB,16#5B,16#32,16#4A/WRITE(*,1) Cl,C2,C3,C4 W

1 FORMAT(lX,4Al)RETURN,END

* SUBROUTINE CUP(N,M) N

107

'97 -7-7-7- '77 7.7

CHARACTER*l Cl,C2,C5,CS,LC(5)CHARACTER*5 CBUJFFINTEGER*2 IC(4)EQUIVALENCE (Cl,IC(l)),(C2,IC(2)),(C5,IC(3)),

+ (CS,IC(4)),(CBUFF,LC(l))__DATA IC/16#1B,16#5B,16#3B,16#66/ b

L- 10000+1 00*N4-MWRITE (CBUFF,2) L .'

2 FORMAT(I5)WRITE(*,1) Cl,C2,LC(2),LC(3,C5,LC4),LC(5),C8FORMAT(1X,8AI,\)RETURN .% *

END

to -7

APPENDIX DPLOTTING PROGRAM LISTING FOR MONITOR

SSTORAGE:2

SDEBUGSNOLISTc

PROGRAM MONITOR

CHARACTER*40 TITLECHARACTER*35 LEGENDCHARACTER*25 NAMEX,NAMEY :-_

REAL X(245),Y(245)REAL O(245),P(245),R(245),S(245),T(245),U(245)INTEGER*2 ICDATA IC/O/

CC USE THESE FOR MULTIPLE ARRAY TRACKINGC TITLE -'MULTIPLE ARRAY ADAPTIVE MANEUVERING RUN'

TITLE -'MULTIPLE ARRAY ADAPTIVE STRAIGHT RUN'CC USE THESE FOR SINGLE ARRAY TRACKINGC TITLE -'SINGLE ARRAY ADAPTIVE MANEUVERING RUN'

C TITLE -'SINGLE ARRAY ADAPTIVE STRAIGHT RUN'OPEN(5,FILE-'XKK.DAT',STATUS-'OLD')

DO 32 LENG - 1 , 241READ(5,*,END=33) 0(LENS) ,P(LENG) ,R(LENG) ,S(LENG),

+ T(LENG),U(LENG)


LENS - LENS - 1CLOSE(5,STATUS-'KEEP')NAMEX -'XEK/K] (FT)'NAMEY -'Y[K/K] (FT)' ""

LEGEND -'FILTERED ESTIMATE OF TRAJECTORY'CALL DRAWER(TITLENAMEX,NAMEY,LEGEND,P,S,LENG)OPEN(5,FILE='PKK.DAT',STATUS-'OLD')

DO 34 LENS - 1 , 241READ(5,*,END-35) O(LENG),P(LENG),R(LENG),S(LENG),

+ T(LENG) ,U(LENG)

34 CONTINUE, 35 CONTINUE

LENG - LENG - 1CLOSE(5,STATUS='KEEP') r-ANAMEX ='TIME SLOTS'NAMEY -'P[K/K](1,1)'

LEGEND -'FILTERED ERROR COVARIANCE P(K/K)'CALL DRAWER(TITLE,NAMEX,NAMEY,LEGEND,O,P,LENG)

314

109

S.-4 '#

' " °' " °° '°'" ° ° ; % " " . °o% °" ° °

°" °': " " .?

°" '" " '°" , ° " ' "" *° ' - ° " ° ° " ° ° ; i °

NAMEY -'PCK/K3(2,2)'CALL DRAWER(TITLE,NAMEX,NAMEY,LEGEND,O,R,LENG)NAMEY -'PCK/K3(3,3)'

N ~CALL DRAWER (TITLE, NAMEX ,NAMEY ,LEGEND, O,S, LENG)NAMEY -'PEK/K3(4,4) --

CALL DRAWER (TITLE ,NAMEX ,NAMEY ,LEGEND,O, T,LENG)NAMEY -'PEK/K3(5,5)'CALL DRAWER (TITLE,NAMEX ,NAMEY,LEGEND, O,U,LENG)OPEN(5,FILE='XKERR.DAT' ,STATUS-'OLD')0038 LENS 1 , 241READ(5,*,END-39) OCLENG) ,P(LENG) ,R(LENG) ,S(LENG)


LENS = LENG - 1.CLOSE (5,STATUS- 'KEEP')NAMEY -*X ERROR (FT)*LEGEND -'ERROR IN FILTERED ESTIMATE'CALL DRAWER (TITLE ,NAMEX ,NAMEY ,LEGEND, 0, P ,LENG) -

NAMEY ='Y ERROR (FT)'CALL DRAWER(TITLE,NAMEX,NAMEY,LEGEND,O,R,LENG)NAMEY -*Z ERROR (FT)'

P CALL DRAWER(TITLE,NAMEX,NAMEY,LEGEND,O,S,LENG,OPEN(5,FILE-'XKN. DAT' ,STATUS-'OLD')DO 40 LENS - 1 , 241READ(5,*,END-41) O(LENG),P(LENG),R(LENG),S(LENG),

+4 T(LENG),U(LENG)40 CONTINUE41 CONTINUE

LENS - LENS -1 ICLOSE (5,STATUS- 'KEEP')NAMEX -'XCK/N3 (FT)'NAMEY =*YEK/N3 (FT)'LEGEND ='SMOOTHED ESTIMATE OF TRAJECTORY'

-~~~ CALL DRAWER (TITLE, NAMEX ,NAMEY ,LEGEND, P,S ,LENG)OPEN(5,FILE='PKN.DATV,STATUS='OLD')

4D 042 LENG = 1 , 241READ(5,*,END=43) 0(LENG) ,P(LENG) ,R(LENG) ,S(LENG),

+ T(LENG),U(LENG)42 CONTINUE43 CONTINUE

LENS - LENS - 1CLOSE (5,STATUS- 'KEEP')NAMEX -'TIME SLOTS'NAMEY ='PCK/N3(I,l)'LEGEND -'SMOOTHED ERROR COVARIANCE P(K/N)'CALL DRAWER(TITLE,NAMEX,NAMEY,LEGEND,O,P,LENG) 0-NAMEY -'PEK/N3(2,2)'CALL DRAWER(TITLE,NAMEX,NAMEY,LEGEND,O,R,LENG) '7NAMEY '*PEK/N3(3,3)'CALL DRAWER (TITLE ,NAMEX ,NAMEY ,LEGEND ,O,S.,LENG)NAMEY -'PEK/NJ(4,4)'

110

CALL DRAWER (TITLE, NAMEX ,NAMEY ,LEGEND, 0,T,LENG)NAMEY -'PEK/NJ(5,5)'CALL DRAWER (TITLE ,NAMEX ,NAMEY ,LEGEND, 0,U ,LENG)OPEN(5,FILE-'XSERR.DAT',STATUS-'OLD')__DO 44 LENS I , 241READ(59*,END-45) OCLENG) ,P(LENG) ,R(LENG) ,S(LENG)


LENS - LENG - ICLOSE (5 ,STATUS- KEEP')

14

NAMEY -'X ERROR (FT)*LEGEND -'ERROR IN SMOOTHED ESTIMATE'CALL DRAWER (TITLE, NAMEX ,NAMEY ,LEGEND, 0,P ,LENG)NAMEY -'Y ERROR (FT)'CALL DRAWER (TITLE, NAMEX ,NAMEY ,LEGEND, 0,R ,LENS 4

NAMEY -'Z ERROR (FT)'CALL DRAWER (TITLE, NAMEX,NAMEY ,LEGEND, O,S, LENG) -

STOPENDSUBROUTINE DRAWER(TTL,NAMEX,NAMEV ,LEGEND, X,Y, LENG)CHARACTER*40 TITLECHARACTER*35 LEGENDCHARACTER*25 NAMEX ,NAMEYREAL X(245),Y(245) '

INTEGER*2 ICDATA IC/0/

.4 C

CALL EDCALL CUP(1,0)CALL PLOTS(0,99,99)

CALL SYMBOL(0.5,5.15,.20,TITLE,0.0,40)CALL SYMBOL(l.04,4.75,.175,LEGEND,0.0,35)

CC USE THIS FOR NOISELESS TRACKINGC CALL SYMBOL(5.38,4.75,.175,'WITHOUT NOISE',0.0,13)C

4.C USE THIS FOR NOISY TRACKINGCALL SYMBOL (5.38,4.75,.175,' WITH NOISE',0.0,13)

CCALL PLDT(1.00,1.00,-3) J .

CALL SCALE(X,6.00,LENG,1)CALL SCALE(Y,3.00,LENG,1)CALL STAXIS(.180,.20,.15,.112,-1)CALL AXIS(0.,0.,NAMEX,-13,6.O0,O0.,X(LENG+1)

+- X(LENG4-2))

CALL STAXIS (.15,.20,. 111,. 112,2)CALL AXIS(0.,0.,NAMEY,13,3.00,90.,Y(LENG+1),

+ Y(LENG+2))

CALL LINE(X,Y,LENG,I,0,3)CALL PLOT'(0.0,0.O,999) " NRETURN

4li

ENDSUBROUTINE EDCHARACTER*l C1,C2,C3,C4INTEGER*2 IC(4)EQUIVALENCE (ClIC(l)),(C2,IC(2)),(C3,IC(3)),

+ (C4,IC(4))DATA IC/16#1D,16#5D,16#32,16#4A/WRITE(*,1) CI,C2,C3,C4FORMAT(lX,4Al)RETURNENDSUBROUTINE CUP(N,M)CHARACTER4I C1,tC2,C5,CB,LC(5)CHARACTER*5 CDUFFINTEGER*2 IC(4) -

EQUIVALENCE (C1,IC(l)),(C2,IC(2)),(C5,IC(3)),+ (CC,IC(4)),(CBUFF,LC(l))

DATA IC/16#IB,16#5B,16#3B,16#66/L-10000+100*N+MWRITE(CBUFF,2)L

2 FORMAT(I5)WRITE(*,i) C1,C2,LC(2),LC(3),C5,LC(4),LC(5),C8FORMAT(IX,BAi,\)RETURNEND

4;4

112

APPENDIX EBATCH FILES

A. LISTING OF AUTOEXEC.BAT FILE ON OPERATING SYSTEM DIS'-.ECHO OFFGRAPHICSTIMER/SCOPY A:RUN.BAT C:COPY A:KEDIT.EXE C:/VCOPY A:PROFILE.KED C:/VC:RUN

B. LISTING OF RUN.BAT FILE ON VIRTUAL DISK(C)ECHO. Insert the disk, which has the source file of theECHO. sequential extended Kalman filter and Smoothing,ECHO. into drive APAUSECOPY A:THESIS.FOR C:KEDIT C:THESIS.FORCOPY C:THESIS.FOR A:ERASE C:KEDIT.EXEERASE C:PROFILE.KEDECHO. Insert the disk, which has PROFORT.EXE andECHO. LINK.EXE, into drive A, and the disk, which hasECHO. PROFORT.LIB into drive B.PAUSEA:PROFORT THESIS /L /EA:LINK THESIS, ,NULL,PROFORTERASE CaTHESIS.FORERASE C: THES IS. OBJTHESISECHO. Insert the disk, which has the source file of theECHO. sequential extended Kalman filter and Smoothing.ECHO. into drive A, and the disk labeled "DATA" intoECHO. drive B.PAUSECOPY C:THESIS.EXE A:COPY C:*.DAT B:ERASE C:*.*ECHO. Insert the operating system disk into drive A, and .. -

ECHO. the disk, which has the plotting routine sourceECHO. file into drive B.PAUSECOPY A:KEDIT.EXE C:COPY A:PROFILE.KED C:COPY B:GRAPH.FOR C: A.. *%*

KEDIT C:GRAPH.FORCOPY C:GRAPH.FOR B:

113

%*%~

M: Z

ERASE C:KEDIT.EX

ERASE C:PROFILE.KEDECHO. Insert the disk, which has FOR1.EXE and PAS2.EXEECHO. into drive A, and the disk, which has PLOTBS.U.IBECHO. into drive B.%PAUSE

* A:FORI GRAPH;A: PAS2ECHO. Insert the disk, which has FORTRAN.LIB, MATH.LIBECHO. and LINK.EXE into drive A.PAUSEA: LINK GP, , NULL,B: PLDTTSB+A: PORTRAN+A: MATHECHO. Insert the 'disk, which has the plotting sourceECHO. file into drive A and the data disk into drive B.PAUSECOPY C:GRAPH.EXE A:ERASE C:GRAPH.FORERASE C:GRAPH.OBJCOPY B,.*.DAr C:GRAPH

.11

LIST OF REFERENCES

1. Isik, M., An Application of Kalman Filterinc andSmoothing to Torpedo Tracki ng, M.S. Thesis, NavalPostgraduate School, Monterey, California, 1983. 6".

2. Technical Manual, NAVORD OD 41964, NAVTOPRSTAKeyport Ranqe Complex and Associated Data, May 1970.

3. Maybeck, P. S., Stochastic Models, Estimation. andControl , Academic Press, 1982.

4. Brown, R. G., Introduction to Random Signal Analysisand Kalman Filtering, Jhon Wiley & Sons,Inc., 1983.

5. Gelb, A., Aoplied__Optimal Estimation, M.I.T. Press.1974.

6. Rauch, H. E., Tung, F. and Striebel, C. T., MaximumLikelihood Estimates of Linear Dynamic Systems, AIAAJOURNAL, pp. 1445-1450, August 1965.

7. Rauch, H. E., Solution *to the Linear SmoothingProblem, IEEE TRANSACTION ON AUTOMATIC CONTROL,pp. 371-372, October 1963.

8. O'Brien, P. A., An Application of Kalman Filtering toUnderwater Tracking, M.S. Thesis, Naval PostgraduateSchool, Monterey, California, September 1980.

9. IBM, Technical Reference for the IBM Personal Computer,P*_Fonal Computer Hardware Reference Library, 1983. '' .

10. IBM, Personal Computer Professional FORTRAN Reference.Installation and use, Personal Computer Software, 1984.

11. Microsoft, Microsoft FORTRAN77 Reference Manual,Microsoft Corporation, 1984.

12. Peerles Engineering Service, Fortran ScientificSubroutine Library, John Wiley & Sons, Inc, 1984.

13. Young, T., L., and Van Woert, M., L., PLOT88 SoftwareLibrary Reference Manual, PLOTWORKS, Inc, 1984.

115

, ..: .

.1 " l

V -,. Ill 1.X

P". INITIAL DISTRIBUTION LISTp ' ....

No. Copies1. Defense Technical Information Center 2

Cameron Station -'Alexandria, Virginia 22304-6145

2. Library, Code 0142 2Naval Postgraduate SchoolMonterey, California 93943-5000

3. Department Chairman, Code 62 2

Department of Electrical and ComputerEngineering

Naval Postgraduate School , -Monterey, California 93943-5000

4. Professor H. A. Titus, Code 62Ts 5Department of Electrical and Computer "

a-, Engineering - "Naval Postnraduate SchoolMonterey, California 93943-5000

5. Associate Professor A. Gerba, Code 62Gz I

Department of Electrical and Computer -

EngineeringNaval Postgraduate SchoolMonterey, Cal i forni a 93943-5000

6. Commanding Officer 2

Naval Underseas Weapons Engineering StationKeyport, Washington 98345

7. Deniz Kuvvetleri Komutanligi 5

KutuphanesiBakanliklar - ANKARA / TURKEY

8. Deniz Harp Okulu Komutanligi 2

Elektrik/Elektronik Bolum KutuphanesiTuzla - ISTANBUL / TURKEY

9. LTJG. Sadi Karaman 2'Runguc Pasa Mahallesi 121 Sokak No 8Karacabey - BURSA / TURKEY

10. Istanbul Teknik Universitesi 1Elektrik Fakultesi DekanligiIstanbul / TURKEY

116

o.. , ." t '~ *Jh "

-" a.•S". .".]", . " .:-,'..: " ".'". :/. .', '.2 . ''.2 ""... , ' :"""' ", ;.:. .

%$ %

11. Orta Dogu Teknik Universitesi IElektrik Fakultesi Dekanligi .4/,

Ankara/ TURKEY Z

12. Bogazici Universitesi1Elektrik Fakultesi DalcanligiIstanbul / TURKEY

13. Ookuz Eylul Universitesi1Elektrik Fakultesi DelcanligiIzmir /TURKEY

117*

znaval postgraduate school · znaval postgraduate school monterey, california-- ,. % ~,.,...

Documents