TWO DIMENSIONAL MATCHED SCHOOL FILTERING FOR …Three-dimensional to two-dimensional projections are...
106
AD-AO84 413 NAVAL POSTGRADUATE SCHOOL MONTEREY CA F/6 17/9 TWO DIMENSIONAL MATCHED FILTERING FOR ATTITUDE MEASUREMENT WITH-ETC(U) MAR Ao R H WILLIAMS UNCLASIFID M E1hhEEEE2 EEEohhEmhEEmhI mhhhhhmhhhh7i EoEEEElh
Transcript of TWO DIMENSIONAL MATCHED SCHOOL FILTERING FOR …Three-dimensional to two-dimensional projections are...
AD-AO84 413 NAVAL POSTGRADUATE SCHOOL MONTEREY CA F/6 17/9
TWO DIMENSIONAL MATCHED FILTERING FOR ATTITUDE MEASUREMENT WITH-ETC(U)MAR Ao R H WILLIAMS
UNCLASIFID M
E1hhEEEE2EEEohhEmhEEmhI
mhhhhhmhhhh7i
EoEEEElh
NAVAL POSTGRADUATE SCHOOLMonterey, California
" DTICELECTE.
E
THESISWo IMENSIONAL JATCHED JILTERING... FORTTITUDE ASUREMENT WITHAPPLICATIONS TO TARGET TRACKING,
by
Q ' Robert Howard/il iams
IMar 9 8 01Thesis Advisor: H.A. Titus
' o~Approved for public release; distribution unlimited
I8
-. 80 5 20 080
UNCLASSIFIEDSECURITY CLASSIFICATION OF THIS PAOE (Uhi Does Eatae4__________________
REPOR DOCUMENTATION PAGE San DES11LTRellPC"1. *SPORT NUMEEN 2. OWT ACCESSJON NO: S. RE1CIPIENT'S CATALOG HUMSCR
4. TITLE (and Subiffsj S. TYPE OF REPORTS 6P9mOo COVEED
Two Dimensional Matched Filtering Master's Thesis;for Attitude Measurement With March 1980Applications to Target Tracking a. 011R1161111ING0 Oe. 01111001T hulleRn
7. AUTHOR(a) 9. CONTRACT 0R GRANT NUM111011aj
Robert Howard Williams
S. PERFORMING ORGANIZATION NAME AND ADDRESS AREA n TOAUITNME SK
Naval Postgraduate SchoolMonterey, California 93940
11. CONTROLLING OFFICE NAME AND ADDRESS 12. REPORT DATE
Naval Postgraduate School March 1980
Monterey, California 93940 102illff r AE
14. MONITORING AGENCY NAME a ADDRESS(U fE. tera as CenhoiU.1 Offloo) IS. SECURITY CLASS. (ofthi Alaen
UNCLASS IFIEDIS&. V15-1=1111C ATI 006/DOWN 00ADI 06
IS. DISTRIDUTION6 STATEMENT (of tis Repeat
Approved for public release; distribution unlimited.
17. DISTRIGUTION STATEMENT (of 00 a68irmt AWmrd I* 11106 ". it iffSer"# ho ASPaei
10. SUPPLEMENTARY NOTES
IS. KEZY WORDS (Catims an ,.uaeaa aide it Recaaamr owd #dme#& b ook nuabse)
Two-Dimensions, Matched Filter, Fourier Transform Correlation,Attitude Measurement, Target Tracking
$0 UTRACT (Canifim se ,aewaa Side 010u060004117 ad idauli0P 6F 111166 Ou5e)
20-An application of two-dimensional Fourier Transform correlationfor the attitude measurement of three-dimensional objects isconsidered. Included are geometric optical equations and geo-metric translation and rotation equations necessary for theimage error calculations. Minimum attitude resolution equationsare developed to estimate image quantization necessary for a
DD I FSOR7 1473 EDITION% OP I NOV 65 1S ODSOLETE UNCLASSIFIED(Pag 1)SiN 102OH- 401I 1 SECURITY CLASSIFICATION OF TNIS PAGE (111000 Dats bSI~d
TI T.Aqq TVT~f6u'c6ASgsIVCAYION or THIS 06*afwm ft... sBofte.
given measurement error. These techniques are thenapplied as an additional measurement to the radartracking probem 7
Accession ForNTIS GIAIMDC TABunannouncedJustiricoatiol
Diatributon/-
Avail mid/or
DD 3orra 3 1473 UNCLASSIFIED
S14 Arj%1-014-6601 ~2 .WIAGOOFT1Do
Approved for public release; distribution unlimited.
Two Dimensional Matched Filteringfor Attitude Measurement with
Applications to Target Tracking
Robert Howard WilliamsB.S., Oregon State University, 1967
Submitted in partial fulfillment of therequirements for the degree of
MASTER OF SCIENCE IN ELECTRICAL ENGINEERING
from the
NAVAL POSTGRADUATE SCHOOL
March 1980
Author (± /wa -I
Approved by: . . .--- ,:' X'4 - --- ----S. . Thesis Advisor
~Seconl'Reader
Chairman, Department of Electrical Engineering
Dean of Science and Engineering
3
ABSTRACT
An application of two-dimensional Fourier Transform correlation
for the attitude measurement of three-dimensional objects is considered.
Included are geometric optical equations and geometric translation and
rotation equations necessary for the image error calculations. Mini-
mum attitude resolution equations are developed to estimate image
quantization necessary for a given measurement error. These techniques
are then applied as an additional measurement to the radar tracking
problem.
4
TABLE OF CONTENTS
I. INTRODUCTION- - --------------- 8
II. IMAGE THEORY- - --------------- 9
III. IMAGE RESOLUTION ---------------- 12
IV. IMAGE FILTERING --------- --- ---- 21
V. MATCHED FILTER EQUATION ------------ 32
VI. AIDED AIRCRAFT TRACKING ------------ 46
VII. CONCLUSIONS ------------ ------ 62
APPENDIX A: TWO-DIMENSIONAL DISCRETE CORRELATION - - 63
APPENDIX B: COMPUTER PROGRAMS ------------ 65
BIBLIOGRAPHY ------- ----------- --- 101
INITIAL DISTRIBUTION LIST ------------ -- 102
5
LIST OF FIGURES
1. Camera Geometry ------ --- ----- --- --- -- 10
2. Maximum Image Dimensions, Orthogonal Views ------ -- 13
3. Discrete, Orthogonal Views ---------------- 14
4. Minimum Detectable Rotation, Orthogonal View ----- -- 15
5. Minimum Detectable RotationRotation Axis in Viewing Plane ----------- --- 16
6. One-Dimensional Rotation Errors ------ --- --- -- 19
7. Three-Dimension to Two-Dimension Projection ------ -- 23
8. FACIT Model of F-102 ----------- --- --- -- 24
9. Sample 64 x 64 Pixel X-Y Correlation Plane Obtainedfrom 32 x 32 Pixel Image and 32 x 32 Pixel Reference - - 26
10. Image Processing Flow Diagram ------ --- --- --- 27
11. Azimuth Correlation Function ------ --------- 30
12. Implementation of Two-Dimensional Correlation ------- 35a
13. Boundary Points of FACIT Model ------ --- --- -- 37
14. Boundary Points of FACIT Model with Added Noise ------ 40
15. Photograph of F102 Aircraft ------ --- --- ---- 41
16. Digitized 64 x 64 Pixel Image of F102 Aircraft ------ 42
17. Best fit of Stored Image Pixels to Video Outlineof Fig. 16 -------- --- --- ----- --- -- 44
18. Attitude Measurement and Tracking System ----- -- -- 48
19. Three Dimensional Aircraft Trajectory,Velocity = 175 yards/second ------ --- ------- 53
20. Three Dimensional Trajectory,Velocity = 100 Yards/second ------ --- ------- 54
6
LIS- OF FIGURES (Continued)
21. Range Error and Measurement Noise --------- ---- 56
22. Estimated Range Velocity and Differential Range Dividedby the Sampling Interval, Close-In Maneuver -------- 57
23. Estimated Range Velocity and Differential RangeDivided by the Sampling Interval, Distant Maneuver - - - - 58
24. Range Acceleration Estimate and Range AccelerationMeasurement ------ -------------- ---- 59
25. Range Acceleration Estimate Without Range AccelerationMeasurement -------------------- ---- 61
7
I. INTRODUCTION
The development of sophisticated naval weapons has resulted in the
requirement for high accuracy measurement tests and discrete analysis of
complex system parameters. Typical of this precise test and evaluation
requirement is attitude measurement of three-dimensional airborne objects
by processing multiple two-dimensional images.
The most common method used to estimate attitude is by making
absolute or precisely relative three-dimensional geometric trajectory
measurements of target object extreme structural features (nose, tail,
right wing tip, left wing tip, etc.) from camera images. By appropriate
camera, range coordinate system, and target coordinate system trans-
formations these measurements will yield an attitude measurement.
A more direct method of attitude measurement involves the correlation,
or comparison, of an image of an object at an unknown attitude with a
second image of a similar object at a known attitude. At the Naval
Weapons Center, an operator controlled film and television correlation
assessment technique (FATCAT) facility use this more direct technique
for attitude estimation [1].
This thesis presents an enhancement of the correlation/comparison
method of attitude measurement by fully automating the technique incorp-
orating a pattern recognition method of the directly digitized image.
Image theory, resolution, and filtering leading to the two dimensional
matched filter attitude measurement concept is discussed and this concept
is applied as an additional measurement to a radar tracking problem.
8
II. IMAGE THEORY
Three-dimensional to two-dimensional projections are obtained by
viewing the real world as recorded by a camera or other imaging device.
Image theory defines projection as the transformation of each point in
the field of view, through a focal point, and onto a plane. This is
also equivalent to locating the focal plane in front of the focal point
and changing the sign of the projected points [2]. These geometrical
relationships are illustrated in Fig. 1. The transformation of point
x, y, z in the X, Y, Z coordinate system of Fig. 1 is given by the
following equations:
fu- x (1)
y+ f
fv - z (2)
y+f
Where u = projected x dimension
v = projected z dimension
Iff << y, then the following approximations hold:
fxU = - kx (3)Y
fzv - ky (4)
9
Therefore, for a given focal length f, and target range y, a constant
image scaling factor k, is calculated.
Rear Point inFocal ViewingPlane (v , I~ v Space (x,y,z)
(O,-2f,O) y
u'-v)FrontCamera
F Focal Optical
Point Plane Axis, Y
(o,-fO) z
Figure 1
Camera Geometry
A second coordinate system X', Y', Z' can be used to describe a
three-dimensional object in the viewing space. If the origins of these
two coordinate systems are coincident, and if all the axes are parallel,
then the transformation of points from one coordinate system to the
other would be simple.
L =[ (5)
10
If the axes of the two coordinate systems are rotated by an elevation
angle 0 around the camera Y axis, by an azimuth angle around the camera
Z axis, and by roll angle around the camera X axis, the transformation
equation would be [3], [4]:
x Cse6 0 sine [cosp -sin~ 0l1 0 0 1 x
y 1 sin cos 0 H cos -sin I I (6)
cz -sina 0 CoseJ 0 0 1j-0 sin cos Z]
If in addition there was a translation of (A,B,C) of the origin of the
object coordinate system relative to the origin of the camera coordinate
system, the transformation equation would be:
y 1 co + B (7)
where [° ;[o0 ~ o ]
= sinp cos 0
L 0 0 1l
cos -sin0
sin cos 0
11
III. IMAGE RESOLUTION
The three-dimensional object can be considered to be an aircraft,
with maximum orthogonal dimensions on the focal plane of Xmax, Ymax,
Zmax. These dimensions are illustrated on the top and side views of
Fig. 2. Since the image is to be digitaized, the discrete maximum
dimensions can be given in terms of pixel sampling intervals, x and y.
A pixel is defined as the smallest unit sampling area (x,y) of resolution
for the digitized image.
Xmax(quantized) = XN (8)
Ymax(quantized) = YN (9)
Zmax(quantized) = ZN (10)
These dimensions are illustrated in Fig, 3. The quantization of the
image would have a minimum rotation that could be detected by a 1 pixel
change at extreme points in the image and would be equivalent to the
following angle:
tan KN (11)
This relationship is illustrated in Fig. 4. KN would be equal to XN, YN,
or ZN depending on the angle of rotation.
12
L
Ymax
Zma
Xmax
Figure 2
Maximum Image Dimensions, Orthogonal Views
13
I pixel
YNN pixels
XN pixels 0
Figure 3
Discrete, Orthogonal Views
14
I pixel Pivot Point I
T 1 K- pixels
Figure 4
Minimum Detectable Rotation, Orthogonal View
If the axis of rotation was not orthogonal to the focal plane, the
minimum detectable rotation of an equal sized object would be much
larger, as indicated in the top view of Fig. 5.
KN-1ces mi n KN (12)
1KN = (13)
1-cos e mi
Sample values of KN from equations (11) and (13) for minimum detectable
rotation are given in Table 1.
15
Side View Geometry
b. 05pixel 0. 5 pi xel s
Rotation Axis
Viewing Plane ProjectionI
- *-0.5 pixel [*4 K .5 pixel
Figure 5. Minimum Detectable Rotation
Rotation Axis in Viewing Plane
16
TABLE 1
Axis Orthogonal Axis Parallelto image plane (11) to image plane (13)
e (degrees) KN (pixels) KN (pixels)
1 58 6566
5 12 263
10 6 66
If it is assumed that the rotational angles are measured by lines
through the two extreme pixels, the geometry and error sensitivity would
be as shown in Fig. 6. For a maximum length of KN, and a 1 pixel rota-
tion:
Itan - (11)
KN
For horizontal errors H:
1e = arctan + (14)
KN - H
1de = d arctan KN + (15)
For a change of variables:
-n (16)KN ±H
17
dm = dH (7
dH
do (KN t H)2 (17)
1de--- -2-dm (18)
1 +m
dH
(KN - H)2+1
For example, if KN = 58, H = 1, then de/dH = 0.0003077 radians/pixel =
0.01763 degrees/pixel. This sensitivity for horizontal errors is very
small, as expected.
For vertical errors:
I - V (20)= arctan
KN
1-V
m - (21)KN
dVdm - (22)
KN
dV KNde - 2 + 2 (23)
KN + (1 t V)
For example, if V = 1 pixel, and KN = 58 pixels, de/dV = 0.01722 radians/
pixel = 0.99 degrees/pixel. This is much larger than the horizontal
error, as might be expected by observing the geometry of Fig. 6.
18
. . . .
KN pixels
T V
- Reference Axis -,H = Horizontal Error H t H
V = Vertical Error
Figure 6.
One-Dimensional Rotation Errors
19
This section has derived equations (11) and (12) to predict the
minimum detectable rotation of a three-dimensional object. These
equations apply to a three-dimensional object with a two-dimensional
maximum projected dimension of KN pixels. Equation (11) applies if
the axis of rotation of the three-dimensional object is parallel
with the camera axis. Equation (13) applies if the axis of rotation
of the three-dimensional object is orthogonal to the camera axis.
Equations (19) and (23) predict the rotational errors resulting from
horizontal and vertical measurement errors in extreme points of the
two-dimensional image.
20
L. .A.
IV. IMAGE FILTERING
Two-dimensional matched filtering can be considered to be an
extension of one-dimensional correlation filtering [5]. The out-
put of the two-dimensional matched filter will not be a transformed
image with some type of improvement or enhancement, but a correlation
plane whose amplitude at a given location corresponds to the degree
of correlation of the input image (signal + noise) with the desired
image (signal). This is equivalent to sliding one photographic
transparency across another and adding up each black point that was
aligned with another black point on the second image, each white
point that was aligned with another white point on the second image,
and each grey point that was aligned with another grey point of
equal shade on the second image.
Several practical factors must be considered in the actual
simulation of the matched filter. The input image and the refer-
ence or stored image could both be compared (correlated) as contin-
uous grey scale images, but this would be a tremendous storage and
computational task. Therefore, the reference image in the computer
simulation is digitized and stored as discrete X,Y,Z points (FACIT
Model) in a three-dimensional coordinate system [6]. It is recon-
structed, after rotation and translation, as unit amplitude lines
projected on the focal plane. It is also assumed that f << y, and
21
f and y of equations (1) and (2) are known. This knowledge of
focal length and target range yields a constant image scaling fac-
tor (k). This type of projection is illustrated in Fig. 7. An
example of an aircraft FACIT Model is given in Fig. 8.
The input grey-level image is also transformed into a similar
format by use of an edge detector to extract outline information
[7]." The edge of the image is defined as the boundary between the
two regions of different grey level.
The reference image and the transformed input image are applied
to a matched filter to determine the degree of correlation. An
ideal correlation function C(x,y,k,e,p,o) would have an impulse
response that could be modeled by delta, (sin x/x), triangle or
exponential functions. The impulse would occur when the reference
and transformed images were:
1. aligned on the image plane (x,y),
2. equally scaled (k) and
3. at the same attitude (ep,@).
However, due to the presence of noise and the approximation of the
three-dimensional object with a finite element model, the correla-
tion response is not delta, (sin x)/x, triangle or exponential in
22
Image Plane
Projected Image
Stored Image
zFigure 7.
Three-Dimension to Two-Dimension Projection
23
L-
AZ--155.0 EL- -20.0 ROLL- -5.0C
Na.'
C
a'.I.'
0
N
0
a'.a"
0
N
o 5...* 5.
.1- . 1 *?~ jz 1
o . a.N a *.**
0
a-S
C
0
'-S
a"* a a a a a a a a-23.0 -18.0 -13.0 -8.0 -3.0 2.0 7.0 12.0 17.0 22.0 27.0 32.0 37.0
Figure 8. FACIT Model of F-102
24
shape. It is not necessary to know the e,,act correlation response
function or even the constant component due to a nonzero mean value
of the reference and input image variable function. It is only
necessary to know the values of the variables x, y, k, e , 0 and o
at peak correlation.
The following simplifying assumptions are made for an attitude
(e, ,¢) only correlation:
1. the images have been scaled to an equal size (k)
2. the peak correlation on the image plane will indicate
translational alignment (x,y)
Fig. 9 illustrates a typical x-y correlation between a noisy image
and computer-generated reference image. The peak correlation at
(0.0, 0.0) pixels indicates that both images are aligned in the
x-y plane. This is because the noisy image was generated from a
computer reference image.
The attitude of the input image is unknown. Therefore, the
measurement of the attitude of the input image is equivalent to
applying the transformed image to a bank of matched filters with each
"tuned" to a specific attitude. This process is illustrated in
Fig. 10.
25
X-Y CORRELATION PLANE
Figure 9
Sample 64 X 64 Pixel X-Y Correlation Plane Obtainedfrom 32 X 32 Pixel Image and 32 X 32 Pixel Reference
26
Stored Size Scale Rotate Three-AxisImage Transform Image Quadratic
Transform Interpolation
Range e 01age1 ....... I
Input Edge MatchedImge Detector Filter 1 Maximum
Matched Output - m ax max maxFilter 2
Matched DetectorFi I ter LMNel 1 m On
Figure 10. Image Processing Flow Diagram
If there was a priori knowledge of azimuth e , elevation i , and
roll 0 , to within 10 degrees each, and a 1 degree resolution of
measurement was required, a brute force approach would require
10 x 10 x 10 = 1000 correlations or 1000 matched filters. Three axis
quadratic interpolation is used to reduce the number of correlations
required for the given resolution of measurement [8].
It is assumed that the variables azimuth, elevation, and roll
are uncoupled in the joint correlation function C(x,y,6,t, 4). Assum-
ing that there is a near orthogonal relationship allows C(x,y,e,p,4)
to be sequentially maximized (minimized) with the independent vari-
ables: x, y, azimuth, elevation, and roll.
27
The pitch correlation function is modeled as a quadratic func-
tion C():
2
C(6) =a 2 + b e + c (24)
dCdC 2 a e + b (25)
Equation (25) is set to zero to maximize (minimize) C(0).
emax = b (26)2a
el = 0 degrees, 02 = 61 + 6e degrees, and 03 = 61 - A6 degrees are
substituted in equation (24) to solve for the unknowns a, b, and c.
The solution of equation (26) is:
C - (27)0max = 2C(e 2) + 2C(6 3) -4C(ej)
Similar equations are written for C(p), and C(-f).
The computer simulations listed in Appendix B implement equa-
ion (27). The three correlations C(81), C(62) and C(e3) are calculated
with Ae = 5 degrees for a rough approximation of 6max Then three
28
correlations C(e1 ), C(e2 ) and C(e3 ) are calculated with Ae = 2
degrees for a sharper quadratic approximation of Omax . In addition,
the +2 and +5 degree intervals are tested to make sure that the mid-
point correlation C(6i) was greater than or equal to the end point
correlations C(62 ) and C(63). If either is not, the interval is shifted
by 2 or 5 degrees in the direction of the larger correlation and the
mid-point is tested again. This dual pass computation reduces the
unwanted coupling effect of the attitude variables. The +5 degree
calculation detects the general angle of maximum correlation. The
+2 degree, calculation reduces the estimated angle error by not being
as sensitive to unsymetry and nonlinearities in the correlation func-
tion.
A sample of the dual pass azimuth estimation is illustrated in
Fig. 11. For this sample, noisy edge data of a simulated aircraft at
an azimuth angle of -45.0 degrees is used as input data. This data
is correlated with computer-generated edge data from a stored air-
craft model. The initial estimate of attitude is -48.0 degrees. The
final azimuth calculated is -44.11 degrees, a 0.89 degree error from
the true -45.0 degree aircraft azimuth attitude. This is a typical
maximum error. For example, if the initial estimate of azimuth is
-42.0 degrees, the final error would be 0.0 degrees.
29
LEGENDo = CORRELATION DATAo = FIRST INTERPOLATION
65- = SECOND INTERPOLATION
60'
55-
50-Z -0
45-J 45.Li]
0
40
35-
30-
25-55 -50 -45 -40 -35
AZIMUTH ANGLE IN DEGREES
Figure 11
Azimuth Correlation Function
30
This section has introduced the use of two-dimensional correla-
tion to determine the attitude of a three-dimensional object. Equation
(24) is used as a second order polynomial model for the correlation
function of each attitude variable. This approximation reduces the
number of computations necessary to determine the peak correlation for
each attitude variable. This quadratic approximation was demonstrated
with a computer simulation that tested for the maximum correlation of
an image as a function of attitude. The simulated image was previously
generated by adding edge noise to a computer-generated image at a known
attitude of -45.0 degrees. By starting the correlation algorithm with
a -3.0 degree offset, the final error was found to be 0.89 degrees.
31
V. MATCHED FILTER EQUATION
It is well known that one-dimension correlation can be calculated
by a simple multiplication of two functions that are Fourier trans-
formed [9].
c(t) = - x (T) h(t+T) dT = x(t) * h(-t) (28)
where T is the dummy variable of integration and * is used to
symbolize the convolution integral.
C(w) = X(w) H*(w) = X*(w) H(w) (29)
where w = 21Tf = frequency in radians and * is used to indicate com-
plex conjugation; that is, if
H(w) = R(w) + jI(w), then H*(w) = R(w) - jI(w).
c(t) <=> C(w) (30)
where <=> is used to indicate a Fourier transform pair; that is
C(w) is the Fourier transform of c(t).
32
This same relationship can be shown in two-dimensions and in discrete
form [5].
N-1 M-1
c(k,i) E z x (i,j) h(k+i,i+j) (31)
i=0 j=0
C(m,n) =X*(m,n) H(m,n) (32)
K-i L-1
,2 T1mk 2 11niC(m,n) z E x(k,i )e K e L
k=0 1=0
I-i ci-1
1=0 j=0 (33)
m = variabie in first dimension
n = variabie in second dimension
K = I = M = Period in first dimension
L = J = N = Period in second dimension
33
A complete listing of the transformation indicated in equations (32)
and (33) is given in Appendix A.
The reason the Fourier domain correlation approach is used is
that for large data arrays the Fast Fourier Transform (FFT) computa-
tion time is less than that of direct time or space domain correlation.
Also, Fourier transform and array processing hardware is available for
many scicntific computers.
The continuous correlation equation (28) states that an infinite
integration is necessary to determine the correlation at a particular
time. This is not the case for finite sampled functions. The summa-
tion interval for the sampled functions requires that the majority of
the information in the two functions be contained in the finite sampled
interval for the correlation to approximate the continuous correlation.
When x(t) and h(t) are sampled with an impulse function A(t) over a
finite interval, 0 <t<tmax = NT, their Fourier transforms x(w)*A(w)
and H(w)* A(w) will be periodic with period w = l/T, where T = sampling
interval of the impulse function A(t). When the continuous function
x(t) is sampled over the interval 0 < t <tmax and Fourier transformed,
this is equivalent to calculating the Fourier Series coefficients of a
sampled periodic sequence x p(nT) of period N, where N = t p/T and
T = time interval between samples. If x p(nT) is time shifted in
relation to x(nT) by m samples, and the first N samples are trans-
formed, it can be shown that the resulting complex Fourier transform
34
will change in phase but not in amplitude or period [9]. In the
time domain, this shift of m samples is due to the periodicity
of xp (nT) and is referred to as a circular shift of a sequence [5].
The sequence appears to be circular because the last sample of x(nT)
that is shifted beyond the interval N T appears at the beginning of
the sequence. Likewise, in the two-dimensional correlation there is a
circular shift in two dimensions when one image x(i,j) is correlated
with another h(i,j). The two-dimensional periodic or circular sampled
function is equivalent to joining parallel horizontal and vertical edges
of each image and rotating one image on each of its axes while the
other image remains stationary. This two-dimensional circular shifting
is depicted in Fig. 12. In this figure, image 2 has shifted n samples
in the horizontal direction with respect to the fixed image 1. Image 2
has shifted m samples in the vertical direction with respect to the
fixed image 1. To clarify that image 2 has circularly "wrapped around"
image 1, the corners are labeled A, B, C and D.
35
M-1T
Image 2 Being Swep Horizontally
C and Vertically AcrossD Image 1
g. ----- Neutral Background
n N-I
M-1--
of Neutral Background
D I
Image 2 ..-- Correlation Input Array (M x N)
A B
Figure 12. Implementation of Two-Dimensional Correlation
35a
The continuous images 1 and 2 are sampled at N and M discrete
points in the x and y axis respectively. This sampling results in
a periodic equation (33) in the transformed frequency domain of per-
iod N and M. If the correlation product of the two sampled images
has nonzero terms beyond the N and M dimensions, there will be an
overlap of nonzero samples in the frequency domain. This overlap
phenomenon of a high frequency component taking on the identity of
a lower frequency component is referred to as frequency aliasing.
Likewise, spatial aliasing would occur if there was an overlap of the
periodic spatial functions; for example, if the first and last samples
added.
To test the correlation method of attitude measurement, a com-
puter simulation program, FAC listed in Appendix B, was run. Discrete
boundary points were generated as input data for a known aircraft
attitude as illustrated in Fig. 13. A noisy test object was generated
that was within +5 degrees in azimuth, elevation and roll of the ref-
erence object. The noisy boundary, as described in the next paragraph,
was compared with the noise-free computer-generated edge data to esti-
mate the aircraft attitude. The representative errors of attitude
(azimuth, elevation and roll) were computed by comparing the attitude
that was iteratively predicted by equation (27) with the actual atti-
tude. These results are summarized in Table II.
36
AILft
AZ=-155.0 EL= -15.0 ROLL= .037-
32-
27-
22-
17-
2 -.................
12
73
Table II.
Representative Attitude Errors (Degrees)
Three Sample Average, 1 Pixel Noise Standard Deviation
Azimuth 0.12
Elevation 0.55
Roll 0.71
It was noted that the errors increased with each attitude measurement.
This is because of the sequential coupling of a previous attitude
angle error to the next quadratic angle interpolation.
The quadratic correlation attitude estimator (27) was initial-
ized with data to within 10 degrees of the known aircraft attitude.
Boundary points on a 32 x 32 grid were generated from the two-
dimensional projection of the test aircraft. The number of boundary
points generated were a function of the test aircraft attitude, but
typically 108 points were generated:
(27 each: maximum horizontal, minimum horizontal, maximum
vertical, minimum vertical).
38
To simulate detector noise, Gaussian noise with zero mean and one
pixel standard deviation was added to each boundary point, horizon-
tally and vertically. Since the boundary point function was on a
discrete 32 x 32 grid, this had the effect of quantizing the noise
to unit pixel intervals. For example, if the absolute value of the
Gaussian noise was less than 0.5 pixel, no noise would be added. If
the absolute value of the Gaussian noise was between 0.5 and 1.5
pixels, one pixel of noise was added. Likewise, two or more pixels
of noise would be added for greater outputs of the generator. These
noisy boundary points are shown in Fig. 14.
An uncalibrated system test with real data was made from a
digitized video image of a F102 aircraft. The intensity data from
the image was transformed into edge points. The edge points were
correlated with the model data stored in the computer memory. This
predicted the aircraft's attitude.
The x,y resolution of the digitization was 340 pixel columns
x 220 pixel rows and the intensity resolution was 1 to 256. As the
photograph in Fig. 15 shows, the aircraft image makes up only a small
portion of the video image. Therefore, a 64 x 64 pixel window shown
in Fig. 16 was contrast enhanced, in that the original intensity
range of 103 to 156 was linearly transformed to a 1 to 256 intensity
level. It is noted that the digitized image is not displayed at the
39
a
:o;
* ** a
g* •
* oo
* "
• * • • •g~
0 0 0 0 0
0
C;0C, 0
Figure 14. Boundary Points of FACIT Model with Added Noise
40
* . . ... . . ..a,.... .. . ..
... . .. . .. . . ... ... . . . .. .. . . .. .. . . .
Figure 15
Photograph of F102 Aircraft
41
. .' ..
Fi gure 16
Digitized 64 X 64 Pixel Image of F102 Aircraft
42
correct aspect ratio due to the unequal sampling ratios taken verti-
cally and horizontally.
The edge data from Rosenfeld's Detector [7] was applied to a
variable threshold iteration that was adjusted until greater than
200 pixels were above the threshold. This adjustment was necessary
so that low contrast edge pixels typically at wing, nose and tail
extremities would exceed the threshold. These extremities are most
important in obtaining a given attitude accuracy. Fig. 17 shows the
results of the best computer-generated match of the stored image
superimposed on the edge data obtained from the video image. The
computer predicted the attitude to be:
Azimuth = -115.0 degrees
Elevation = 45.0 degrees
Roll = - 5.0 degrees
There was no other data available to establish the attitude of the
F102 aircraft at the instant the camera recorded the image. It is
presumed that further investigation would show this attitude to be
correct to within the limits of the image resolution and the edge
noise present.
43
LEGEND*=STORED IMAGE EDGE P IXELS*=VIDEO EDGE PIXELS
-J 60-
0 c 0 oooooo a0- CodoooooooooO
-440. 00 0: 0 c0 0o000opc 00000 00
oooooooqoadoooooooooo 0q Am- oadocc 0000000 a0 .
L 20.......aooa 0 0> 'd' -- 000000000 0
0000000000000 00
010 20 KO 46 50 60 7HOR IZONTAL P IXELS
Figure 17
Best Fit of Stored Image Pixels to Video Outline Pixels of Fig. 16
44
The lack of sharpness in the transformed edge data shown in
Fig. 17 can be attributed to several factors;
1. Low x,y resolution (64 x 64).
2. Low intensity range (103 - 156).
3. Uneven lighting and shading on edges.
4. A simple edge detector.
The last factor would be the only one that could be significantly
changed given a free-flying aircraft, fixed camera locations, and
natural lighting. A review of methods used for extracting features
based on edges and contours is included in a survey by Lavine [ill].
More recent theories and experiments on edge detectors have been
demonstrated by Abdou and Pratt [12].
This section has introduced the use of two-dimensional Fourier
domain correlation as described by equation (32). Representative
results of three computer correlation simulations were listed. The
simulation compared a computer generated edge model to a previous
computer generated model with added edge noise. Finally, a sample
correlation of the computer generated edge model with the edge data
detected from a real video image of an aircraft at an unknown atti-
tude was described.
45
VI. AIDED AIRCRAFT TRACKING
An example of the utility of the attitude measurement is demon-
strated by utilizing an aircraft positional estimating Kalman Filter
with combined radar and electro-optic sensor measurements [10]. It
is difficult for the radar-only estimator to keep a close target in
track if it is highly maneuverable. The criterion of "highly man-
euverable" for a particular radar is dependent on the following:
1. The sampling interval of the radar.
2. The range gate period, or the sampling aperture.
3. The receiver sensitivity.
4. The return signal to noise ratio.
5. The mount azimuth and elevation dynamics, or time constants.
6. The measurement accuracy of azimuth, elevation and range.
7. The point source tracking of a finite length target.
These parameters and others contribute to the ability of the radar
to track with minimum error. By modeling the aircraft target to be
driven by an acceleration function that is dependent upon aircraft
attitude, more accurate range, range rate and range acceleration state
estimates are possible. The acceleration function is based on the
well-known aerodynamic coupling between the target attitude and the
direction of the target acceleration. This coupling is summarized
in the following comments:
46
1. Major accelerations (lift) are normal to the velocity vector
and the pitch axis.
2. Positive lift is more likely than negative lift due to pilot
physiological factors and to structural loading design.
3. Accelerations in the velocity direction (drag/thrust) are
generally smaller in magnitude and of shorter duration than
the lift accelerations.
A complete multiple measurement system using the previously discussed
imaging attitude measurement concept as a radar tracking aid to the
positional estimating Kalman Filter is shown in Fig. 18. In the upper
left corner of Fig. 18 there are n radars that provide range, azimuth
and elevation measurements to the Iterated Extended Kalman target
estimator. In the lower left corner there are m video cameras that
input data to m correlators. These correlators first scale the stored
model to a size equal to the predicted input image size. The scaling
is a function of target range and system gains such as telescope mag-
nification and image pixel sampling area. The correlators then apply
equation (33) as described in the previous section for the computer
simulation FAC. The attitude measurements azimuth, elevation, and
roll, from the correlators, are transformed into a common coordinate
system. The predicted attitude of a model aircraft in a coordinated
turn is combined with the m attitude measurements in a Kalman Filter.
47
S-4) S-C4
0-d 0 -44. -' 4- '0 0
-- ( 4- )(I-:k LL.-La
(%JU
-,D
"a (U CA CULA "aCICto -E 10 0~ to 0 ( v 06.:; aU 4-C- V U V
,<:>e ~ . 4)4
L 48
The estimated attitude from the Kalman Filter predicts an accelera-
tion vector that is used as an input to the target estimator.
The simulation of a target range (R) estimation was performed
with the program KAL listed in Appendix B. The simulation KAL was a
limited example of the radar tracking filter as illustrated in Fig. 18.
This simplification was made possible by not including some tracking
variables, such as azimuth and elevation, to the full three-dimensional
trajectory filter. The trajectory was truly three-dimensional, hence
there were changes in azimuth and elevation, but they were not used
as system measurements or estimated as parameters.
The Iterated Extended Kalman Filtering subroutine FTKALI is a
modification of the IMSL Kalman Filtering subroutine FKALM [14].
The system was modeled as a radar track of a nominal constant accel-
eration target. The assumptions for this trajectory are:
1. The average acceleration rate is zero.
2. The acceleration rate between radar scans is constant.
3. The acceleration between scanning intervals in uncorrelated.
The above assumptions are described in the well-known Alpha-Beta-
Gamma Filter equations [13]. Written in a discrete form, these
equations are:
49
TT2. _ T3 ar(34)Xn_1 +,T x n-i + 2 n 6 a6 1n-1
xn = XnI + T x n-l + -2--arn-i (35)
Xn Xn-l + Tar n-I (36)
where ar is a random sequence with zero-mean and a mean-square
acceleration rate of
E[ar ark] =a 2 for j = k (37)
0otherwise]
The target trajectory was simulated in three parts:
1. A target constant closing velocity of 100 or 175
yards/second at the same altitude as the radar.
2. A 10-g target turn in the azimuth plane of the radar.
3. A l0-g target turn "up" in the geometric plane perpendicular
to the azimuth plane and the ground projection of the radar
range vector.
50
The final, or third turn, starts when the target has rotated through
90.0 degrees of the lO-g turn in the azimuth plane. The maneuver
was started at three initial ranges:
1. Distant : 11275.0 yards
2. Mid-range : 1000.0 yards
3. Close-in : 150.0 yards
Inputs to the filter of two noisy radar range measurements and an
optional radial component of the predicted normal acceleration vector
were generated at a sample rate of 10/second. A one sigma standard
deviation Gaussian noise of 1.0 yard was added to the two range
measurements. These inputs were input to the range filter.
Typical results of range error for one simulation of all man-
euvers are listed in Table III. The addition of the predicted normal
acceleration measurement to the radar range measurements reduced the
peak and RMS errors for all maneuvers. The reduction in error was
most dramatic for the close-in maneuver because of the range reversal
for the particular azimuth and elevation geometries. At the mid-
range and distant range trajectories there was no range reversal
so the additional acceleration measurement was less helpful in cor-
recting the range and range rate estimates In addition, the high
radar sample rates and the averaging effect of using two radar measure-
ments reduced the difference in error between the radar-only estimate
and the combined estimate.
51
Table 1I.
Range Error (RMS Average/Peak - in Yards)
Maneuver Combined Measurement Two Radars Only
(Normal Acceleration
and Two Radars)
1. Distant 0.76/3.22 0.99/4.82
2. Mid-Range 0.89/4.25 1.06/5.53
3. Close-In 4.83/7.56 10.28/12.02
Fig. 19 shows the aircraft lO-g turn trajectory plotted in three
dimensions for a 175 yards/second air speed. As compared to the 100
yards/second air speed trajectory shown in Fig. 20, the distance
traveled down range is much greater, and the turning radius was much
larger. However, within the 5 second interval of flight, the air-
craft in Fig. 19 did not climb as high as that in Fig. 20. This is
because the third maneuver (turn up) did not start until approximately
3 seconds versus 2 seconds for the 100 yards/second trajectory.
52
TARGET TRAJECTORY
LEGEND= 3-D PLOT
o =GROUND PLOT~=ELEV.-OFF RANGE
I0-0-
0
Figure 19
Three Dimensional Aircraft Trajectory,Velocity = 175 yards/second
53
TARGET TRAJECTORY
LEGEND0 = 3-D PLOTo = GROUND PLOT~=ELEV.-OFF RANGE
v.C,0
Zo
020IIS
o)ob~l N , o bo
OFF RANGE -
Figure 20
Three Dimensional Trajectory,Velocity = 100 yards/second
54
In Fig. 21 the radial error and the measurement noise in yards
is plotted versus time for the close-in maneuver. The radial error
is equal to the true radial distance minus the estimated radial dis-
tance in yards. Gaussian noise with zero mean and a standard deviation
proportional to the square root of the estimated radial range variance
was the measurement noise added to the true range. For this example
the noise has a 1 sigma standard deviation of 2.0 yards. The very
large peak error at 0.8 seconds is due to the aircraft flying very
nearly "overhead". This then reverses the sign of the relative
velocity vector.
The velocity reversal is shown in Fig. 22 where X(2) is the Kalman
estimator velocity state, and VR2D is the first difference of range
divided by the sampling interval. The slow time constant of X(2) is
due to the lack of a velocity measurement. A more accurate velocity
estimate is shown in Fig. 23 for the distant turn maneuver. There is
no velocity reversal in this trajectory.
Fig. 24 shows the range acceleration state estimate, X(3), and
the measured range acceleration, Y(3). The two accelerations track
quite closely and describe two phenomenon:
1. An initialization.
55
5-
4.5-
4-
3.5-
C) 3-LiiU-i
- 2.5
2 LEGEND0 =ERRORo = NOISE
0.5-
01
-4 -2 0 2 4 6 a 10 12 14ERROR IN YARDS
Figure 21
Range Error and Measurement Noise
56
5-
4.5-
4-
3.5-
3-LiJ(I)
2.5'
LiJ
2-
1.5-
LEG ND
0.5-
-200 -150 -100 -50 0 50 100 150 200
VELOC IN YARDS/SECFigure 22
Estimated Range Velocity and Differential Range Dividedby the Sampling Interval, Close-In Maneuver
57
5
4.5
4
3.5
(3LUJ(A)
z 2.5
Lii
2-
1.5-
LEGEND1~ '= X(2)
o =VR2D
0.5-
0- 9-200 -150 -100 -50 0 50
VELOC I N YARDS/SEC
Figure 23
Estimated Range Velicity and Differential RangeDivided by the Sampling Interval, Distant Maneuver
58
5- LEGEND
4.5-
4-
3.5-
3-Li
2.5-
LUJ
2-
1.5-
0.5-
0-
-10 0 10 20 30 40 50 60
ACCEL IN YDS/SEC*SEC
Figure 24
Range Acceleration Estimate and Range Accelefation Measurement
59
2. Cyclic acceleration matching the period of the third
part of the maneuver.
The acceleration state estimate in Fig. 25 with no acceleration
measurement does not show this periodic acceleration, indicating the
uncertainty present in the state estimate.
This section applied the correlation attitude measurement tech-
nique to the radar tracking problem. The attitude is related to
acceleration and therefore a useful measurement in predicting target
acceleration. Recursive trajectory equations were used in a computer
simulation that estimated aircraft range. The simulation demonstrated
that the Kalman Filter was more accurate in tracking a target with
varying trajectories when the predicted acceleration was used as in
addition to the noisy range measurements.
60
5- LEGEND
4
3.5
(3Lii
z 2.5
LLJ
2
1.5-
0.5-
-1 0 1 2 3 4 56
ACCEL IN YDS/SEC*SEC
Figure 25
Range Acceleration Estimate Without Range Acceleration Measurement
61
VII. CONCLUSIONS
Manual optical image comparisons have been the primary method
of measuring attitude of three-dimensional objects. Section V lists
the general two-dimensional Fourier transform equations that are
applied to an images edge data and a stored model of the solid ob-
ject. The attitude of a three-dimensional object is estimated by
using a three-axis quadratic interpolation to sequentially maximize
the correlation output as a function of the attitude, position and
size variables. Moreover, this measurement ur estimate is equivalent
to the maximum output of a bank of matched filters.
This concept was then applied to realistic Kalman Filter track-
ing simulations and shown to give improvement over conventional target
state estimators that do not use the attitude measurement as a pre-
dictor of target acceleration.
Since the necessary attitude measurement is within the electro-
optical tracker and computer state-of-the-art, the use of this esti-
mator concept can improve present tracker system capabilities.
62
APPENDIX A
TWO-DIMENSIONAL DISCRETE CORRELATION
This appendix demonstrates that the two-dimensional discrete
correlation transform (31), (32) is equivalent in both the time domain
and the frequency domain.
N-1 N-1
c(k,l) z z x(i,j) h(k + 1, 1 +j) (31)i=O j-O
C(m,n) = X*(m,n) H(m,n) (32)
where *=complex conjugate
Equations (31) and (32) are verified by performing the Fourier Transform
and conjugation operations indicated.
M4-1 N-1 _jp- -,J --Nfl
c(k,l) = E1 X*(m,n) e ei=O j=O M N m=O n=O
(A-1)[ P-1 Q-1 2n 2ni- p(k+i) j - q (1+j)]
z z H(p,q) e M N7Fp=O q=O
1 M-1 N-i P-1 Q-1 2n~ 21 -c(k.1) = X*(m~n) H(p,q) e e N
mO nO p:=O q=O
(A-2)
N-i N-1 -j -g-i m j- -n j -g-p i j -L-1 E e e e e[ N 1=0 j=O1
63
The term in brackets in equation (A-2) is equal to;
Tri(M N) =1, if m p and n =q
0 ,if m pand n q
due to orthogonality of i and j. Therefore:
1 M-1 N-1 211 2H
c-k~l E X*(m,n) H(m,n) e m- mnk e NnlI
c~k~l) in=0 n=0
(A- 3)
= l[X*(in,n) H(m,n)
64
APPENDIX B
COMPUTER PROGRAMS
This Appendix is composed of the listings of the Fortran programs
and subroutines for the attitude measurement and target tracking
simulations. The programs were run on a Univac 1110 computer with
a Fortran compiler and a FLECS translator (Fortran Language with
Extended Control Structures). The graphical outputs were plotted
using DISSPLA programs that generated COM-80 crt images.
65
O!I NSIL~N AZ (( rA j)LL (,) C, )I Z
L (, m IL E X* o A( 16So4)
Do 111 J1,*2uCP-AX (J ) -l.N1P
ill CONTINUEI P L U IT 1
1 REAL 0., AZ,LLsvi~LLL.LA T
v. 'I T t (o .1l ) Al E~L ROLL.LAST
iI F ( m . Ea1 1 P L U T
L READ ARRAY SIZE IN PO.EWS OF
c REAL NUILER OF AkNAYS TO bY CiONRELATED (MAA OF 20)
foEADCiC 5*5) K K i J 1.5 FORpr.AT( 15)
READ0i.5) NA
N3= 1KK 2z K K4 1
N22 =2 *N2N14Ni%112
N J = N 2 12 4NV 4= NI 1 *N
NC=(Nl*Ni)/2d4+l
1A 2 11J2m( 3 L :P I 3.14159WF I Ti(6 s15) N1 ,N 2
15 FOWt ,AW~ IroPUT ARFkAy Djv-ktSONh
C
L READ IN TFE UATA AMIWAY
DO 11 N:1.,4C
K 24 44# 1
tI (t C.9o)K1 *(r.1 *: *4)
66
FOi,AT(' I 2('I).)9~ FOi- AT (1x * 5.*24(1 x.*F4 * ) )
C DOUL.LE S11 OF DATA. AkAYL w aINPUT Vt:CTQR. vid4k = UTPUT VE(.TOR* NY Ii4PUT DIIM6NSIOMd
CALL VDUOUL (.whRi~.NV .NV',)
L. PLOT DATA ARNAV
I. TRANSFER THE Wh (K) DATA TO THE COtqPLEX A MATftTXC
DO - 12=1.N24DO 3 11=1.Nl2IWhz (I Z-1) 'N14 IliA(IWH)=bHR( IwNi
.~CONTINUE
Lc COMPUTE THE DISCRETE FOURIER TRANSFORM OF THE WH(K) DATAC
CALL hAkM(AoM,INV#Zoo1.IPERR)CC READ IN THE SECOND DATA ARRAYLCC RuTATE IOCOEL IN AZiNJTti
N L C
DELAL =5.AZA (1 )=AZAZA (t)zAZ-DELALAZA(3)=AZ,' DELAL
290 CONTINUEDO 3CAU J=NAl.NA
CC GthERATE IMAGE uOtJNDAkY FRNOM 3-D1IENTIONAL I4OwELC wtR zCARD = OUTPUT VLCTOk (INCYM X INCYM)L
CALL SURFAC (AZACJ) *EL.ROLL.Y1iAX.ZMAX*1YqIN, IMIN.JIPLOTIPR14 T)CC CALCULATE CORRELATION bETWEEN 14PUT IMAGE AND GENERAT D TMA4.E 3UUNDA
.3 7 FOHI.AT(I,SX.'J ='AXCA = ,Fl7.ot1)CALL CALCOR(KK~JJ,*NV *A1AWJ)oEL.NOLL-J.N12.,N22)
30u.~ CONTINUENL teL'IF(KL.GT.0) wO TO 20bRITc(6.311))
31k FORMAT(I.5m.'J.93x.AZA(J)'.7x.*CAY(J) */ )DO 311 J=1.3WRI Tac.,3121 J ,AZA(J),C1PAX(J)
,11I CONTINUE
TtS T F(C.n11 ?A X IMUS (UR,-L A T I(v I P C IA A( I C 14A X? OW C'MAXt.))L CL,6kEL Ar T T ADo I T I 0.'d mL i-IIt iTS
1 F L I-, A 1 1L T C,,%AA 12 'u To0 314e00O Tfu .'144
141 IF(CMAX(Z).LT.C~iAxt,) C'u TO u142AZA 13)=AZ A( )AZA (1)=A2A(2)AZA(e)=Aio;(2)tJELALC r-A X (3) =C M~A X (1I)CM1AA x IfICfA AX ( 2)C MAX () 1 . E 10
NA=1
I.. CON TtI NU EAZA (2) =A ZA ( 1AZA (1)=AZAC3)AZA (3)=AZA( 3)4 i)LAL
CMA X (1)COAX (1)
CmAX(.5)=-7.El0
'1'.4 CONTINUE
L CALCULATE M1OST VRUvAdLE AZMUT14 FOR INPUT FN~AME BY wUDRATIC INTEqF
C
AZSTzACmAx(2)-Lf4AX(3) )*DELAL /(2l.'CMAX(!)4 .. CMAX(r-)-4.*CMAX (1))
IAZST1lFlx( AZST* .5 ) IF IX(AZ A'1)'2 2 1 FORN'AT(d'A6)
PRI NT 320,AZST. 1AZbT32 o FORi5'AT(95X,A1ST =" , F2.595X,'IAiST =.14,I)
L 54LDUCE INTEINVAL OF I.NTERP(JLATION iF AiS. VALUC OF P.ST > 1.C
I F ( AS(AZST) .L T .1 . b U To0 ~2 1D EL AL=2.u
NAI 3j
AZA((.)=AZA(l)-uELALAZA(3)=ZA(1)4 ViELALC MAX~(k) 1 *El 0
GO Tu 29uC CIN TINUh
Al: FLUAT( IAZST)
C kvjTATE ;- i. EL IN ELEVA1 ION
uLtLLS=5.NL a;C MA X ( 7) =CP~AX Mltv A~ 3N A-) =sELA ( 1 ) =kLELA 4)=EL-DELE.ELA (3)zkL*DELEPS
.59 CONTINUEDO 4uCi JmNAoNA91 It= J -t
LGt NER A IE ImAUE a OUND A AY FmOM 3-0 IrENT IONAL 10iEL
C WHiR = CAU=OUTPUT VtCTOk (NCYI4 X INCY-4)L
CALL SUhfiC (AZ.ELA(J4) .ROLL,Y14AXZM*AX.YMLN .ZI4N.J.1PLOT.IPRI NT)CC CALCULATE COkRELAT ION bE TWEEN INP~UT IMAGE AND (jENERATcU IMAujE 3 OUNDA
CALL CALCQR(KK .JJ. .V .AZ,.ELA(JMl).ROLL .J.Nl2.%22)Cf, CONTINUE
NL ?vL* 1IFC-L.61.o) (20 TO '.202
DO 411 j=799
.1 J -okiRITtCo.312) JLLA(JM) .CMAX(J )
411 CONTINUECC TEST FOR MAXIM4UM CORRELATION* IF CMOAX(7) <C1MAXCb) OR CMAA()C CORRELATE AT ADDITIONAL POINTSC
I F fCMAXE7).LT*C~IAX(8)) GO TO 4141IF(CMAx(7).LT.CmAA(9)) GO TO 414260 TO 4144
4141 IF(CMAA(b)sLT.Cp.Ax(9)) Go TO 4142ELA (3)=ELA( 1)ELA (1)=ELA(2)EA(2)zELA(?)-ELtlSCMAX(9)=CMAX (7)CMAX(7)mCiaAX Cb)CMAX(b)-1.E1ONA8=o~NA9 =aGO TO 390
414c CONTINUEELA(2)=ELA(l)ELA(1)zELA(3)ELAC3)zELA(3)4uELElJSCmAX(b)=C~AX (7)Cr4A x( 7 )ZCMAX( 9C"AX~(Y):- i.El12N AO .
69
:
AAt
b 0O Tu 3-iG.14.. CCNTINUt
L. CAL(.uLATE m~OST IFR~cAULL ELEVATION FOR INjUTI FPAME tY UuD6ATI C J'4T
IEL STIFIX(ELST* .5JtJIFIXK(ELA(1) )P RI id 321 .EL5T.IELbT
!21 FORPATCI.5W.ELST w.'Fl2.5.5'K.IELST ='14iCC R DUCE INTER VAL OF IN TLNPtLATl ON IF AbS . v ALU 4 OF ELS T > 1 .
IF(AuS(ELST).L.1.ou) laU TO 4202DEL EPS=26
ELA(2)=ELA(l)-ijEL~iSELA(3)=ELAC1)*bELEPSCMAXC a)=-1.E10
bO To 3904c-! CONTINUE
EL=FLUA T( IELS T)
C RUTATE MOODEL IN RULL
DEL IiH(,-5oNL= -CMAX(13)=CMAX(71ROLLA(1)=ROLLROL LA(2)=ROLL-DELkh0,R0LLAC3)=ROLL#DELRtONAl 4=14N A 15 =15
49C, CONTINUEDO 5(-O J=NA149NA15JRMZJ-12
CC GENERA~a: IIAb2 dOUNDAnY FnOM 3-DIMENTIONAL P9O&ELC WHR = CAR~D = OUTPUT VtCTOW (INCYMA X INCYM)
C
C CALCULATE CURNELATION HETwEEN IN~uT ImAkGE AND GENERATED IMA%;E 3OUNDAC
CALL CALCOR~(eiK2 .jj.*NV sAi *EL *HCLLA CIM) a*imo2*N22)50~U CONTINUE
NL=NL.IF(PiLebTeit) bO TO 9'O2wRI Tt(6.5 10)
51 C; F OR PA T I ,5 'J,3 A R UL LA(J It- 7 X C 14A x ( J) )00 511 Js13.15.1 fd . -12
AICO NTINUE
L
IF(CMAX(13').LT.CMAA(d.)) 620 TO 51411 F (LMAX (13)*LT CriAA(15)) (70 TO 514.abO TO 5144
A1IF(ChA(14)*LT*CPMA.A(l ) GO TO 51'.iROLLA( 5)=I3LLA(l)hOLLA( 1)=k3,LLA(&')HOLLA(2 '=fOLLA( 9 )-DELIdVOC MAXkl 5)2CM AX (13)CPMAXA(13) C MA A (I')CmAx(14)=-1.EluN A1 4 a14
(2Q Tv. 4 ,L
HOLLA Cl)=HGLLA (3)kOLLA (3 )=khLLA(3)* uELiOCf*A xC 14 ) =CAx (13)CM~A X (13 1 =C~'LA ~fjj
NA 4=15NAl 5=15GO0 TO 4S~C
5144 CONTINUECC CALCu.LATE MOST PROmAEILk NOLL FOR INPUT FhAtAE BY .UDRATIC INTE4PC
ROLLST(CAx(14)-L.qAX(15))*DELRIHOJ(2.*CM*AX 15)2.CI4AX(1..)*-4. aPAX( 1))
IRULST=IFI (RULLST4.5)4IFIVt(HOLL*A( 1)PRINT 323-ROLLbr. IROLST
.523 fOkrAT(l.5x.'RULLbr ='.F12*5.5x.* IROLST =*[14*/)
C REUCE IN~TERVAL Of IN1ERF'OLATIO.,4 IF AtwSe vALUt OF ROLLST > lo
IF(AbSC5iGLLST)sLT.1*,l) 6O TO 5 ''2
NAl 4214
kCLLA()o.LLA(1)#uLeo
C A A x15)=-1 *El U(i C Tu 4,
cl CON T I NULI P L (i I:
S TO 1
S L f A CL
C 4UmFA( kCTA TES A S TLk ED 3-0 1M tT IONA L MOD LL I N S A xIS AZu Tri. LLE VA T-C I oNv Tiht PROJECTiv Po~AL- tF ThL MU r~'L ON A kL A ti SUR FA C IS GUANTI ZtDC uN To A (3 2 A3i) L HI D. OUrPuT Ai'kAY ='~oC
Ikm INT)P AnAIPET LR hC$S7loK1=2-NCP iA4AMTER IN C YM =56P AWAAETER I Z k=C ,I E =P" AnA ME Tek N! 5*Ns =.NUPz7o ImkNSION AICPAX( INCT?) *P(3s.q.NCS) *T(3.5.NCS) .XNDRMCNtCS).'VK(2,NCS).
A Y NOWP( uN C S ) ZNURM(NCS),AKhIN(INLypi). aKf4AX(INCYN9 )b bKMIN(INCY4)
DIMENSION YLR(1).Z0l4)
C CMMCJN CARDC UMMON IF AC DA Ti PZ ERU~u.Uu NEZ I .L C TR =0D kC=AC0 b I 1 16 C
CC LEN~kAL I-REIPERATION OF DATAC
A ZRvAZLLCE Lk=LL*D.RCR L-LLpi=RUjLL*D kLS AZzSIN(AZR)C AZ=0l1S(AZR)S EL=SIN(ELP)CcL=COS(ELP)S kQ=SIN(POLLA)C hO=CQS(ROLLh)Y t.AX-lu.E OY FPINzl(JsElfl2 IAxz-lU.EIO
CC HUTAr E TAkGLTC
D~ G 1 K1oNCb
T (1.J.)=P(1.J.K)*1LELaCAI-Fi2.J.K)*CFL*SAZ~fl(3.J.K)ASEL1 (2,J,K)=P(1 .J .b 1*1CAU*bAZ4bRO.St:L.CAL )'P(2,JK) A(C tOeCAZ-SkU*SEL*
1IS Al) -V( .). S k~ ) *UC E LT(3.J.K)=P(1.J.v)aCSR.*AZ- OIEaCA)P(C'.JK)(C"qOeSELLSAZ
1 *sRU*CAL)* P(!*J.K) *CR4'CktL
72
I M((.j .K)sLT.YP IN) Y1MIN=T(2.J .%)
I (f (3 J KsL LT ZLI I &."IN=( I, j %I~
S sA VE FIRST Ym-- AND 14-- VALUtSL CTHaLCTR41I F(JA.NL.1) (.i TU 12
Y P1AX l=tpZ XY PIN lz I M IN
12 C LNT 1 NU E
C L~Ml-U It SURFACE NOINALS
0 0 15 Kz1.NC S
1 T ( Z. 4 .K))(E, K-(~ZY tiUP(K)C(TC3,1.s)-T(3,K))*(T(1.1.K)-T(1,4,K))-CT(3,1.K) -
1 T(3,,4.K ))*(T(1.1 vK)-T( 1.2.K))Z N,vif(K)=(T( 1,1,iK)-T( 12,K))*( T(2,1,K)-T'2,4.,X))-(T(1,1,K)
1 TC1.4,v )1'(r(2.1,K)-T(.,2-K))15 CQNT'lINUL
C CUMPLJ TE IYMAX AI4I 17MAXl
L CTk=LC TR. 1k ZlA X=Ab Sc ZMAX-Z AI N)R VtIA X=AbS(VMAX-VMIN)I ZMAXRZMAXI Y1AlmRYPMAX
C PREPARE PLOr PAGE
I F(IF LOI.EQ.C) tO TO 19C ALL TMPPLT(',)A XLbTH= AIAAX1((Zf-AX -ZM~iN ),(YM.AX -11 1)A XL2Tm = AlNT(AXLbTH/,e,) 4 I10XCNTH=AIT((YrAiAX4YPIN)/e)Y CNTft=AINT(( ZMAXfl1AIN)/2)U CNkZaxCNTR-AXLbT,Y iR(3YCNTR-AL(TPX lD=ACNvTR#AXL617)
V tN0i=YClNTR+AXI.THC ALL U~ia4IFL(-l)C ALL NOuPRC ALL -E.IGIFT( .l)
C ALL rlTLEtLAtjiL olk It ' g s ess.L ,PLL GRAF(XOFl.~ .XA.'4D ,FG. *YE'.(1)C o.LL iAAhKER(i )C ALL SCLPIC( .,,c)
1f %,)%MAT( Z'F.,X~*RL ,61~
73
i~C IUPJ 1 11 L LLC tT Ah T JiLjX FUR i-KAz)T~
i% i.C I . L OA T( I NC11-)
0 LVIhYM?XiRNCYPL) LZ A= IA A X I R No.VI
1,1 FikfATA(' PLT1. PLrZ. Z6x.'YPIN,.ILX.2pbf'N'.14X. DLY..luxo
K rPAX=U
K I I N= L
I Et i =
L) L~ 1 1z IZ=. I N C vZ tA k I IDLZA *ZMIN
C f-I INi T INTEhSECT IONSC
L i.ECA KtAX( 1Z)=-i '. E1LA KMI 1(li )=lG. El1C
~51 F kk14AT( ' Y ZEbAR* AKIAIN(IZ), AKMAX(IZ),DVIAIN.ZYMIN(INCY).DTMAX..2 yrEMP,imAX(INCY) .'.2Ox.'IZ.I * IFLG.!AKFL(INCY) .INCY'.1)
0 ~ ~ ~ G TO 4C I h
Y~ bIK 1 .I)=T IN-IU
K EZK 4 11 F(A.E904) KE z1IF(AIcS(1T3,KI)-T(3,KEOII).LE.l.E-6) GO TO 5
1 ( ex 01 )
A T 1,K ,I )r tkcy v-1P9 N ) L LY
C I~1% 1vTHSECTION bITHIN zjOUNL1S Ut TPE SURFACEC
I E(Y ebTT(29P1 s) AND.Y.bT.T(2.KE*I)) GO TO 45I F(Y.LToT(2* 1 1 )ANQ.T.Lt.T(2.KE.1)) 60 TO e.5
IF(ZLAR.62T.T(3.K.l'.ANDaaAnGT.T(3,KE.1") uO TO 25I fC(YVLE AKMA XCI1) (20 TV i loA 10-'A ) ( I )z
A KPIINI ~2=Yl1y6 C ONTItUE
Z CC LtrTINUE
.95 C (,NT INUk
C
C LLu r A P, M N CI b TO IY-(tL
C ~LCI HI-uNrIS YL (I)
I F I iL 0T EQ.,l ~U 70 05I L( I )Z LA k1 L jZ u A k
otE(AKPAAX (Ii).L T.PL Ti .UJ.ArfoAA (li) .61 iL r)L ~NL E KPiN(Id LT.P , 14 bT .PL T2
Y L Y( I) zAlK1 I N ( 129 m KtIN =KP I N #I
**CALL Cbi&E(VL.ZL. l.-I)
E L~t* HEN(AK4IN~(li).LT.PLT1.OK.AKM~t4(IZ)o6CT-PLT2)* YL ( II=AKtwAX( IZ)
**CALL CUkVE(YL.ZL* 1.-i)
**YL( ANI AX( IZ)**YL(2)=AMMIN(IZ)
**KIAX=IMAX$1
KM * IN=KpIN4 1*.CALL CUHVE(YL.ZL, 2.-1)
Po F I N
05 C C..TrINUEIUj( C UNTINIJt
L LTH:LCTR,1
J 0 x C
C STA.qT IND~x FOR V-I4ASTERC
L PAA =0L hlIquwfITE(691222) LCTk
D C 5CU I =1, INCVPMV LAil ODLY *YVAIN
C fINU I IN1'EkSECTIrJNS
SU4 CU JIN C bI F ( XNOW M(J .L t U UO Tu 4,.,.U Q 1 LU ,1 4K L=K' # I
I FCA (i i.)T (A'. K). J iio u TO
75
C A K-)AI -uIZOf-TAL LI.M,TS F~gi SEiwUrtTIUL i. SCA NC v~i-JzvRIA LImITS FUR SLQUkNTIAL I SCAN
C C E h T k Ir'Au E W11)P RASPECT 70 FINST I1EAbE ANU USE GWItv INTERVALS UFC f ItS 7 1!- SLAN I I'AGE buTTO)m TO TOP (1) ANU LEFT TO RIGPT (J5) ATC D I aC lqTTE I NTE RYALS.*L C ('IV L iT PA a. A N lo fI K E D b C 00tD NA T kS To 1 bI o I NT LG tRbC
0 LL Va( v EY101 N4UE VIAAXI 12.I P IN V rI~I4D cLY10 L IF1 -A S ( V 11A 1.1-I M I NII JR NC 11StI mI N 11 1N -ZM IN I
D ELI ( DEZ~IAJN'DE I toAX 12.Z1NIkz I F4 IlNl14 UELZ0 LZA1ZAuS(ZMAA1-ZMIN1)lkNCIMI KCT kL
F Vz(AKIMA 9(1)-YMIN) JOLYII aL I I f (F IG x(AAMIN(I) -VmIN)/DLY1lb: IF IXm(GE)
c 24 CHARACTEk FLAGI h F =
C CC.NIhAST PIXEL FLAbI LFZCD L 1O' (5=1J I NC YMF Ya (b K 4A X J-ZPIN)JDLZAlJ1F=IFIX(FV)
J5 s.2:FIX(GY)
I F ( I k *N. GO6 TO 1O3.j1 Tm2 4I RF =1
1 10) C ONi'T I NU EI F(I X Nt J) GU TOU 10 33CAHOD (IN )0ON
I EF =1G 0 TO 10144
1 L33 C iNT INUE
CAaRD(ZR )zZE hiUI F(1G .NE.J) bO TO, 1,)3o
CAND(IfivONkI t F =1INSC TR=INCTR. 1GO o 1LL44
I L36 C GNT INULCAR (lk )1ZEk1u
I F(J F Nt *I) (JU TO 1)4.,LAK C (Ik~) 0N E
I tF:4
76
k- zC T. 1,EJ 7-T 1:. V )a (eR T 7. KJ)i )lTt K E J)-I 2K J)A I C 5'j)
lfiY~.T.T5.N,J)ANDZf.LT.T3.'& ,J)) ) b( O TD3
I F t iq. LT .TC(.ZKJ).AlD.YBAh&T9T(2oKEJ)) bO TJ 3niI F F .L r b K *4A XI ))GO TO o,u P ; x( I )=Z F
L 0 A A=L N , X1SC L;.T I !U I
t Kr(% (I )=Z F
L P-It,=LI IN+ 1,55 C L.N TI NU E' C r C LNT INU t4Lfl C iih II t.U E
C ADD NAtiuOM NOISE TO BKM-C(IL)C
C LOT I-L I KT S
IF I IPL0T E Q. C ) b. O TO 4 8sV LC YVd ANY LC )Y r4Ahq
die iN (b K oA X( I ) *LT.PLT3.HN~.b.PaAX(I ).GT.lo4T4)0 UtNLESSfeKt-IN(I )*LT-PLT3.OA.8K~IN( ).GT.PLT4.1
Z L Z( I) = - KM INC(I)o J ?,"I N=Jt-I NI+.I
o CALL CUt~vk CL.ZL. 19-1)* eF1N00 11
E LJLrc hN CtIKrIINC I ),L r.ILT3,UR tjKj*4N(I ).t.sT.PLT-)
* L L(I)t-KP.AX(I)*.JMAXZJPAX41
**CALL CUIkVE(YL*ZLo 1. -1.* .F I N
r ~L SEZ L Z(1)zLF.MAXCI)l L(2)=LK1MINCI
**JiA=Jp-AX4 I*I * 1 r,4=jpI INA I**CALL CUR~VE YL*ZL. 2.- i
C Lt T dur*.. t. t.i 1I
I .LvI 11,UL
L L T ZLC Tk~ .
77
L t IL 144
1 .'.c,.NrINut
I k(JG .NH.1) bO TC? 1143
I i F-=1
1 '43 C u~NIUt
I '.'. C CINTIrUEI ori, C UIeT INUt1 1. 5 ' C iCNT liUL
L CTtl=LCTR, II PUIPkItT*EQ .,) CUO TO 116~P iqrjT 2UC,AZ stLtPULLC ALL tNDP1L(U)L CTI(=LCTR.1
.,1 C ONrINUEL LIMLCTR41R ETUhit
,.UrC F~RATl SX,*'AZ1M*UTH ANGL~xz.F8o29.5x.A 'ELE'WATION ANtbLE'.Fd.*/q.)XqROLL AN6LEzEFb.2.idi
78
L VLOCUteL CUN EMYS ANi INFUT thkN) AIhmKtY VijTO A (,tvx.!02 Ar.RAY HY AD DINiGtA C x R LuN( 0AT At
Akl NINPULT AhIRY , idkO7 =UTPUT ARRAY, 14 M= INPUT OIMeLNTION
D114thSION ARIN(INbft,). AFOuTLNOD)NODC=k*'INUM~NOD :4*IN.4AFDf-FLOAT(INjT')
I Pi~ --* IV ii
L JzAL.w lhD FOR OuTPUr ARRAY I=CULUMN INJEX
IF( i GTI*IDR.) GO To 27r0CC i(UO 1=1.IDR
IJK=IDF'*(J-1 )4IAPOT(IJ)=AkIN(IJK)CON71INUE
L FILL REMAINDER VF ROw WITH LEROS
e2L DO Z56a I~l,IVR[jzJljRe*(J-1)+jDRtIAROT(lj )mt.
(60 TJ PjU27L CONTINUE
CC FILL REMAINDER OF hkOw. WITP ZEROSC
DO ZOO Ii.IUR..I J 1 R 2 *( J- 1) 4A RDT C 1 .1)
4bu CONTINUEtyX, CON 1'INUE
JE T oH
79
C THIS SUBROUTINE CALCULATES THE CORRELATION BE TWEENC THE A VECTOR(DFT COEFFICIENTS) AND THE 8 WECTUR(OFT COEFFICIENTS)C
SUBROUTINE CALCOR (KK2.JJ2.NV.AZ.EL .ROLL.J .N1Z.N22)DIMENSION WH(16384) WHR(16384)DIMENSION S(12b) ,INVf12s),M'3)D IMENSION CHAX (20)DIMENSION MAXX(2O) *MAXY(2O)DIMENSION W( 12b,12o)COMPLEX*8 Af16Sd4) .8(163b4)C0OMMON bHR WH ,A .C1AXM(1 )=KK2M ( 2 JJ 2M f 3 U=
2037 FORMAT(i.5X9 'J ='912,3X,'CMAX. ='qF17e6963 FORMATC5X9'N1Z =*914,'N22 =*,14.'NW =*14)
C DOUBLE SIZE OF DATA ARRAYC W14R x INPUT VECTOR. wH =OUTPUT VECTOR* NV INPUT 0114ENSION
NV4=4*NVCALL VDOUBL (hHR ,Wh NV ,NV4)
C PLOT DATA ARRAYCC TRANSFER THE WH (K) DATA TO THE COMPLEX B MATRIX
NDA 1A0Do 104 12=19N22DO 103 11=19N12IWP= (1-1) *N124 Ii8 (1WH ) WH (I WNIF( WHEIWH) .GToO.0) NDATA=NDATA41
103 CONTINUE
WRI TE(69?2) (WH(I),#IwIbHM. IWH)
104. CONTINUEWRITE(6*20b) NDATA
20o FOkPAT(E NDATA ='.17)I F(NtDATA*EQ .0) STOP
Cc COMPUTE THE DISCRiTE FOURIER TRANSFORM OF THE wH (K) DATA
71 FORMAT(16(1x,F7.Z) )
72 FORI'AT (1OIX.EIZ.5))73 FORPAT(7(2X,15))
CALL P.ARM(M,INV,S.1 .IPERR)CC COMPLEX MULTIPLYC
DO ZiUIlz11.NV4
8C! 1)A(I1)bCONJCI,(!1))IFCI1 .LT.N22) wRITE(69101) I1.ACI1)#8CI1)
220 CONTINUECC INVERSE FOJURIER TkANSFORMC
CALL HARMCB,M.INU.S .-1,IPERR)CC CALCULATE THE M4AGNITUDE OF THE FOURIER COMPONENTSC AND STORE ONLY TPE POS FREQ VALUES OF W(119.12)C
DO 3U 12=1022ZD0 30 I1=1.N12
IF(W(I1.I2)*LT.CMAX(J)) 6O TO 30CMAX CJ)zw (11.12)1MAX X WJ ) r-I 1MAX YNJ)=12
30 CONTINUECC PRINT THE DFT COEFFICIENTSC
IFCMAAXXCJ).GTo3) 6O TO 4.0M13=114M2=2MM1 =3MAXN(.1) 4MP1Z5MPZ =6MP3=7MP4=bGO TO 1.2
4CiJ CONTINUEN12M=N12-4ZFCISAXXCJ).GT.N12N) GO TO 41mm3=MAX X J )-3M142 MAX XC J)-2M1I =MAXX(J)-1MP1=MAX XCJi 41MP2%MAXX(J)*2MP3xMAXX(J) .3PP4 mAXXN J )+4G0 TO 42
41 CON7INUEMM3=N12-7MP? N12-61ml~ =N12-5MAX XCJ)=N1Z-4MP1 zN12 -3PP2N 12 -2MP3zN12-1P9P4 ao N2
4k~ CON71NUE
81
0;I O1 T (bX Id J 1 3,"~14205))YX 1P~ P P 1P
DO 61 l2zl,Nt2WRI U (6.1 1 I2 *(W Cli,!2), Ii MM3 .MP4.)
ol CONTINUERETURNEND
82
CC KAL IS A RADAR RANGE ESTIMATOR USING A KALMAN FILTER.C THERE ARE 2 RANGE MEASUREMENTS AND A RADIAL ACCELFRATION ESTIM4ATIONC MEASUREM4ENT.CC MODELC X(K*1) zH(K)*X(K)+GCK)OUCK)C YCK*1) X S(K.1)*X(K*1)*V(9+1)C KALMAN ESTIMATEC XE(K,1) = MCK)*X-MAT(K)C X-HAT(K*1) x='K-(*)((KI*'K-(*)C gK(gc.) = PE(K*1)*ST(K*1)*(S(K,1 )'(K.1)*ST(K.1).R(K.1) Vie-IC P'(K+1) a H(K)*P(K)*HT(K)*6(K)iQ(K)*GT(K)C P(K*1) 2 P'(K.1)-K(K+1)*S(K41)*P'(K*1)C P(G) a G(O)*Q(O)*GT(O)C T = TRANSPOSEC SUBROUTINES USED: INTCONC PANVRc FlKALI
DIMENSION 1(4),14(4,4)9G(4) ,S(3,4) ,P(d.,4),T1 (4q4) ,T2(4,4) ,T3(3),'Tx (4) 4(3),R (3, 3) ,G(4.4)
DIMENSION TP(100),EPI (100),XP1(100),RNOJS(100),VP1(l0O),VPZ(100),'RNOIS2(100),XTP(1OO),ZTP(100)DIMENSION API(100) ,AP2(100) ,AP3(IOO)DIMENSION WHEN(2),TMEOD(2)DIMENSION PAUV(3) ,ANUV(3) ,ANL(3) ,ANLR(3) ,TPA(3)DIMENSION IOPT(5),STAT(S)DIMENSION IPAK(150)COMMON XT,YT,ZT,VXT,VYT,VZT,VELOC ,TIANG,T2AN6,DELR,1G,TRATI,*DELTNPRFTRATZ, IINPH,R90, 162, ICTS.PI. ZR, hIM,10
COMMON/INTC1 AZ,ELROL1 ,ROL2,ROL4,AZDEGELDEG,ROL1DG .ROL2DG.*ROL4DG ,T6Sl ,TGS2qRTqRbOTT#RDOTT, DT,TRAD1 ,TRAD290ANGT,*RANGTM,XR,YR,ZR,VXTM,9VVTMVZTM.9VR2D,AXT,AYT,AZT,AXT6,AYTG,AZTG,'ANA ,ANE ,PAUVDATA IN,NoIT/3"./DATA IS,M1/2*31DATA IL,L/2*1/DATA ISEED/42573/DATA Pli3ol4l592654/
C lJ(S''2) MODEL STATE VECTORCALL FR8OIDC 6220 Rome WILLIAM4S 6262 '
NIzSN0=6TGS 1=10.TGS2Z1O.DTs0.05NPz31NPMaNP-1NPHXNP/1ORESaO.5R9O=PI/2.R270=3*.PI/2.GTDzIO*7252NRUNvrO
10 CONTINUE
CC 3-D TARGET TRAJECTORY DATAC ORIGIN AT INITIAL TARGET POSITIONC RT z TRUE RANGE RELATIVE TO RADARC RDOTT a TRUE VELOCITY RELATIVE TO RADARC RDOTT = TRUE ACCELERATION RELATIVE TO RADARC VELOC xTARGET VELOCITY RELATIVE TO ORIGINC TRAD- a TURN RADIUS OF T6S- 6. TURNC TRAT- a TURN RATE OF IGS- 6. TURN (PAD/SEC)C -R aRADAR LOCATION AT T(O)C -T z TRUE X,Y,Z TARGET POSITIONC V-T v TRUE XYZ VELOCITYC A-T a TRUE X,YZ ACCFLERATIONC A-TG x TRUE X.,Y,Z ACCELERATION WS5)C ANA aVERTICAL (A7) ACCELERATION RELATIVE TO TARGET ORIENTATIONC ANE HORIZONTAL (EL) ACCELERATION RELATIVE TO TARGET ORIENTATIONC PAUV(I) a PITCm AXIS UNIT VECTORC PAUV( I) zAC PAUV(2) z ZC PAUV(1) zCC ATC a TIME CONSTANT OF ACCELERATIONC TCK = TRANSITION MATRIX ACCELERATION TIME CON~STANT
ATC = 2.TC xATC/DTTCIC Z (1*-EXP(-TC))
C Q a COVARIANCE MATRIX OF U (LAL) - INPUTC z E[U*UTJ x (RMS RANGE/SEC*SECOSEC)''?
READ(5 .150) QRCV,ILASTDO 115 lOSal.?IF (IOS.EQ.2) DT=O.1Do 118 IRzl,3DO 116 IH=l.?
C H x STATE TRANSITION MATRIX (NAN)
DATA 1/16*0./C A6MTK = GAMM1A ACCELERATION TRANSITION MATRIX CONSTANTC ARTK zNORMAL RADIAL LOAD ACCEL. TRANS, MATRIX CONSTANT
IF(lHoEQo1) AGMT~a1 .0IF( lH*EQe2) AGNTX=ODIF (IHoEQ*3) AGMTKO.,5IF (I'.EQ.I) ARTKcaO.0IF(IH.EQ.2) ARTK1l.OIF (IN.Eu.3) ARTKm:O.5
14(1 1 2DT
1(1,3)zAGPMTK*DT*DT/2.14(1 4)=ARTK *DT* DT/?.
H(?.3)wDT6AGMTKM(?,4)*ARTK*0T
C ANLRK a RADIAL NORMAL LOAD ACCELERATION TRANSITION CONSTANTANLRK at.H(4,A)ANLRK
C 6 a STATE NOISE INPUT MATRIX (NXL)
34
300 FORIMAT Q =',F12.1,1 R w',FQ.3,' 0T m0 9 F10.1,E P001 *E
CALL DATE(WHEN)CALL TOFOAY(TIREOD)WRITE(NO9112) W,EN(l),WHENC2),TMEOD(1) ,TREOD(2)
112 FORNAT(2A6,2X,?A6)WRITE (6,90)
90 FORPIAT K',4X,ERT *9X-YX
*2x,'AXTGa,2m ,dAYT6',?X,'AZTGE,2X, 2N0 DIFF-VELOC'91)WRITE (6,130) xqRT#Y0l)qX(l),X(?) ,EPlf 1)*RNOIS(1)
CC MAIN LOOPC
DO 100 Ilz1,NPHIPzII,1IpsaoIF(II.EQ61) IPS1lPC INS=0 .7RNOIS(1I)zSQRT(R(1,1))*GGNOF(ISEED)'PCTNSRNOIS2(11)=SQRT(R(2,Z))*GGNOF(ISEED).PCTNSDELR=VELOC*DELT*FLOATCNPR )
C MANVR CALCULATES TRUE POSITIONS CXTYT,ZT) AND TRUE VELOCITIES (VXT.'C VYTVZT) TARGET STARTS 2 ORIGIN al T'09 AND FLIES TOWARD RADAR FIXEDC LOCATION RT ON X AXISC X - DOWN RANGE, REFERENCED TO TARGET & 1 20C Y a UP RANGE, REFERENCED TO TARGET 0 7 m0C Z a OFF RANGE, REFERENCED TO TARGET a 7 x0
CALL 04ANVRC V a TOTAL TRUE VELOCITY RELATIVE TO COORD SYSTEM 9 T(O)
V=SQRT (VXT*VXTVYT*VYT+VZT*VZT)AXTE(vXT-VXTRI)/(DELT*FLOAT(NPRF))AYT=(VYT-VYTM4)/(DELT'FLOAT (NPRF))AZTz(VZT-VZTM)/(DELT'FLOAT(NPR F))
C AT = TOTAL TRUE ACCELERATION RELATIVE TO COORD SISTE14 T (0)AT=SQRT(AXT.AXT+AVT*AYT*AZT*AZT)AXTGzAXT/10.7252AYTGzAYT/1O.7252AZTGzAZT/10.7252ANE3((AYT*VxT-VYT*AXT)*VXT-(AZT*VVT-VT*AT)*VZT)(V*V4)ANAM(AZT**VZT-VZT*AXT )IVH
C AH a HORIZONTAL ACCELERATION -RELATIVE TO COORD. SYS., T(0)AH=SQRT(AXT*AXTAZT*AZT)ELEUANE+COS (EL)EL4=SQRT (ANA*ANA+ELE*ELE)
C VH = HORIZONTAL VELOCITY -RELATIVE TO COORD. SYS., T(0)VHzSQRT(CV IT' VXT*VZT *VZT)AZzATAN? (VZT .VXT)AZDEG=AZ,*180o/PlELeATAN2CVYT ,V14)ELDEG*EL'1g0./PlANAEL=ANAIELE
C ICTS = COORDINATED TURN SWITCHIF(ICTS.EQ.1) ROL~aATAN2(ANA*10*72S?)ROL$ =ATAN2CANA,10.?25?)
85
G(1)=DTDT'DT/4.
G(3 )xDT6 ( 4) z G( 3
C R aCOVARIANCE MATRIX OF V (PIlXPI) P EASURE14ENT
DATA R/9*O01R (1I,1I ) =AC V
R(292)z RC191)R(393)alOO*R(1 91)
C S a OBSERVER MATRIX (MlXN) Y = SK
DATA S/12*0.
S ( 1,91 ) a1
S(?,1 )xl.
C INTCON INITILIZFS TRAJECTORY VARIABLES
CCALL INTCON
C TX aITERATED EXTENDED THRESHOLDS
T X ( I) aITX (2)=TX Cl)
TX (3)xTX (2)TX(4)xTX(3)K=ODT=O.LL=O
RNOIS(1)ZSQRTCR(ltl))*66N0F(ISEED)RNOIS2( 1)xSQRT(R (29?) )*GGNOF(ISEED)
C V INPUT OBSERVATION VECTOR (MlXI)
C MULTIPLE MEASUREMENTSY(1 )sRT+RNOIS(l)Y(2)=RTRNOIS2(i)Y(3)m0.
C PRESET ESTIM4ATOR INITIAL STATE
C i~l) aRAN6E(K)
C X(2) z RANGE RATE(KC X(3) a RANGE ACCELERATION
C X(4) a ESTIM4ATED RADIAL NORM4AL LOAD ACCELERATION
Xl00FF 1I
X(l)=Y(l)+Xl00FFK (Z)zx2OK*RDOTTX(3)800N (4)X0.EPiC 1)x RT-E1i)XPi (1 )aK(1)
VP2(1 )RDOTTAPI (l)vx(3)AP2(l )20.AP3 (1 )zO*EPMAXx3O.1 RDRX=RT%RUNzNRUN1lWRITE(69300) QRCV.RT,RDOTTA6MTK .ARTK,SC3.3),NRUN
8E
144 FORMAT( COOD. TURN Sw z'139' ROL4 zo,F9.3.E COORD. TURN ROLL
ROLoE6=ROL4'18O./PlANADOT2((-VXT.AXT-VZT.AZT).ANA)/(VH.Vn)AZDOTrANA/VNELDOTvANE IVROLDOT21O.?2SZ.ANADOTI(1O.7252*1O.7252,ANA*ANA)
C V-TN DELAYED TRUE X,Y,Z VELOCITYVXTMzVXT
VZ TMVZ TC -3 a xqY.Z DIFFERENCE BETWEEN RADAR AND TRUE TARCSET POSITION
13=XR-XTY3zyR-YTZ3zZR-IT
C RAN6T = TRUE RANGE RANGTN DELAYED TRUE RANGERANGTM=RANGTRAN T =S QR1(1 3 x 3 *.Y3 * Y3 Z 3 * Z3)VNOIS=RNOIS (II)RAN6NxRANGT+VNOISRANGN2=RANGT+RNOIS?( II)REDRZANOD(RANGk9UES)REmDR~zAMOD(RAN6NZRES)RES2=RES/2.
C RANGE = MEASURED RANGE WITH NOISE AND QUANTIZED TO RESOLUTZONRANGEzRANGN-REMDRRANGE 2xRANN-REPMDR2ST=RAN6T
C AMNL = MAGNITUDE OF NORMAL LOAD ACCELERATION G 6S)ALz 3.BEu-3.GAMO.e5EPz6GN0F (ISFED)AMNLzAL 'BE * EXP (6AM'EPS)
C AZ z AZDEGC EL a -ELDE6C ROLL 8 -ROLDEGC PAUV(1) a xC PAUV(2) a£C PAUV(3) a
SAZzSII( AZ)CAZZCOS( AZ)SEL=SIN(-EL)CEL2COS(- EL)SROBSZN(-ROL4)CROzCOS (-ROL4)TPA(1)XPAUV(1)*CEL*CAZ-PAUV(?)*CEL*SAZ4PAUV(3)'SELTPA (2) PAUV (1)'(C RO*SAZ *SRO*S EL'C AZ PAUVC 2)' (C RO'CAZ-SRO*S EL*SA;* -PAU V (3) S RO*C ELTPA(3)SPAUV(1).(SRO*SAZ-CRO*SEL.CAZ)*PAUV(2)*(CRO'SEL*SAZ4SRO*CA:
* *PAUV (3 ) CROOC ELC ANUV(I) a NORMAL ACCELERATION UNIT VECTORC a (TPA(I) X V-T)JV (VECTOR CROSS PRODUCT)C ( x, z9Y )
ANUV(1)2( CTPA(2)*VYT) -(IPA(3)*VZT))IV
ANUV(2)2( CTPAC3)*VXT) - (TPAC1).VYT))IVANUW(3)=( (TPA(1)*VZT) - (TPA(2)*VXT))iV
C ANL(I) = NORMAL LOAD ACCELERATIONANLM1 z ANUV(I)*APNLANL(2) sANUV(2)*AMNLANLM3z ANUV(3)AMN~L
C ANLR(I) RADIAL NORMAL LOAD ACCELERATION( G'S)C aPROJECTION (DOT SCALAR PRODUCT) OF ANL(I ON THE RADIALC VECTOR FROM4 TARGET TO RADAR/RADIAL RANGE
ANLR(I)x((ANL(1).x3)*(ANL(2)*Z3),(ANL(3)*Y3))/RAwGTy ( )xRANGES(?)zRANGE?Y(3)xANLR(1 )*6YD
C vR2D a 1ST DIFFERANCE, RANGE VELOCITY VR2DD = DELAYED VR2DVRDD:vRZDVRDz(RANGT-RANGTM)/(DELT*FLOAT(NPRF))
C AR a RANGE ACCELERATION, 2ND DIFFERANCEA R04 A RIARZ(VRZD-VR20D)/(DELT*FLOAT(NPRF))
WRITE(69133) RANGT,RANGTP,VR2DD,VR20,AR,YC3),X(3),*x3,139Z3
C TEST RADIAL 2ND DIFFERENCE: AR, & LT1MIT TO MAX. EXPECTEDC RATE OF CHANGE: SQRT(Q)
ADIF=ABS (AR-ARM)QLI$4SQRT(Q)*DTIF (ADIFeGToQLIM) AR=Sl6N (QLIM,AR)+ARM4
C TEST FOR RE-INITILIZATION OF KALMAN FILTER
SZG1USIGTH*SQRT(R(l,l))S16GSlGTH*SQRT(R (2,?))SlG3zAM4A1(SIGI ,SIG?)PMAXwO.
IFEEST eLTeSI63) GO TO 416.INCPZINCP*1IPSxlDO 42 JP21,N
PM4AXzAMAXICP(JP*JP),PM4AX)
42 CONTINUEPPCTsl.0DO 43 JP=1,N
PCJPgJP)zPAX*PPCT43 CONTINUE44 CONTINUE
WRITE(69133) AZ,ELROL4,TPA(1),TPA(2) ,TPA(3),ANUV(1) ,ANUV(2)..ANUV(3),AMNL,ANLR(I),AR
CALL FTKALI (K,X,H,G,Y,S,Q,R ,P,IN,IS,IL,k,141,L,Tl*T?,lT9T39*TI. FG, ER, IPS)TxTDTEPI(IP)z RT-W()XPI (IP)sx(1 )VPI (IP)=X(2)VP? (IP) SVRZDAPI (IP)=X(3)A P2(1 P) zAR
88
AP3 (IP)zY (3)
VPCTxX(2)*DT*1OO./EP1(IP)APCT=X(3)*IT*oT*5O.IEPI(IP)PcTh'.=1?(,1)*1OO./EP1(IP)hRITE(69134) IIVPCT*APCT#PCTMeT2(l ,1),PMAX
134 FORMATIX,139- PCT VELOV, ACC, PEAS x'.1O(F12.3))IF(A8S(EP1( IP)) .GT.ABS(EPMAX)) EPMAX=EP1(IP)IF (ABS(EP1 (IP)) .GT*40o.) LLZLL.1
IF(FG(1,1)oGE.1o) 60 TO 51WNSvFG(2,1)/((1.-F6(1,))*DT)IF(WNS.LT.0.) 6010o51wNzSQRT(WNS)
INzbN/2.'PI
194 FORMAW( NATURAL FREQ. (HZ) u',F9.29' DA4PIN6 RATIO z'.FB.3)51 CONTINUE
wRITE(69130) KRT,Y(1 ) x(1),Ux(2) ,EPI(IP),RNOIS(II) ,AXTC.AYTG,AZTi
*VR?D100 CONTINUE
CIOPTI1)alIOPT(5)a1NPPzNP*1CALL BEIU6R(EP1 ,NPP,IOPT,STAT,IER)WRITE(6,10?) STAT(1)oSTAT(S) ,EPMAX
10? FORMAT(C ARITHMETIC MEAN OF RANGE ERR z'*F7.3,
*0 VARIANCE (AVE. RMS ERROR) OF RANGE ERROR g5 F9.3,6; MAXIMUM RANGE ERR =',F9.3,/)
WRITE (6,104) LLINCP104 FORMAT(1,' NO* OF RETURNS OUT OF GATE z',I'.,
e' NO. TIMES P INCR. 2'914)I RPLOT1lIF(IRPLOT9EQo0) GO TO 113CALL INTAXSCALL IAXAN6(O.)CALL SIMPLXCALL TITLE( KALM4AN FILTERS',100,
'ERROR IN YARDSS',*100,'TIME IN SEC.S',100,6.,8.)CALL XTICKS(5CALL YTICKSM5XOR6SAPINI(ARM4IN(EP1,NP),ARMIN(RNOIS,NP))XmAN GEAlAX IC ARMAX ( EP I NP) #A ARMAX(CRNO IS , NP))CALL GRAF (XORGvSCALE',XNlAXG,
ARMIN(TP ,NP),'SCALE*,ARmAX(YP ,NP))
CALL CUAVECEPI, TP,NP,1O)CALL CURVE (RNOIS,TP,NP,10)CALL PESSAW(COV Q US'9100,1., b~.9)CALL REALNO( Q,0,'AeUTq'A8UT')CALL MESSAG(C COV R 2S',1009 'ABUIsABUTO)CALL RFALN0(R(1,1) ,2,AbUT',oABUT')
89
CALL NESSAW( 9=S', 100, 'APUT','AbUT')CALL INTNO(NRUN, ABUT','A8UT')CALL DATE(WNEN)CALL TOFDAY(TMFOD)CALL MESSAG(WtEN,1O,1 .,9.3)CALL MESSAG(TMEOD.10,'A8UT'9'A8UT')IbUMMY=LINEST(IPAKVIS0,80)CALL LINES CERRORS', IPAK .1)CALL LINES('NOISES-,IPAK .2)CALL LEGEND(IPAK .2,4.0,2.0)CALL ENDPL(0)
113 CONTINUEI vPLOT=0IF(IR *EQ*3) IVPLOTIl
IF(IVPLOToFQO) 60 TO 114CALL INTAXSCALL VAXANG(O.)
CALL SIMPLYCALL TITLE(' KAL14AN FILTERS',100,
aVELOC IN Y.ARDS/SECS'.o 100,'TIME IN SEC.S0,10096*98.)CALL XTICKS(5CALL YTICKS(S)YORGzAMIN1(ARMIN(vP1 ,NP),ARMIN( VP?,NP))XNAX6zAftAXI(AANAXCVPI,NP),ARPAX( VP?,NP))CALL GRAF (XONG,'SCALE',XNAX6,
*ARNIN(TP ,NP),'SCALE',ARMAX(TP *NP))CALL CURVE(VPl, TP,NP,10)CALL CURVE (VP? ,TP,NP,1O)CALL MESSAW(COV Q =5',100.1., b.9)CALL REALNO( 09O,'ABUT','ABUT')CALL MESSAWC COy R -S',100, 'AeUT'9'A6UT')CALL REALNO(R(1 ,1),2v'A8UT','ABUT')CALL MESSAWC 0 Wo 100. 'ABUT','ABUT')CALL INTNO(NNUN ,'ABUT', 'ABUT')CALL bATE(WHEN)CALL TOFDAYCTMFOD)CALL MESSAG(WHEN,1O,1 0,9.3)
CALL PqESSAG (TREOD,1O.'A8UT' ,'A8UT')I0UMmYLINEST(IPAK,150,bO)CALL LINES('X (2)3', IPAK,1)CALL LINES('VR20S',IPAK .2)CALL LE6END(IPAK 9295.0,1.0)CALL ENOPL(0)
114 CONTINUE
IAPLOT21IF(IAPLOT.EQ.O) 60 TO 1148CALL INTAXSCALL 'AXAN6CO.)CALL SIMPLYCALL TITLEC KALMAN FJLTERS',IC0,
ACCEL IN YDS/SEC'SECS',*100,'TIME IN SEC.S-,100,6.,P.)CALL XTICKS(SCALL YIICKS(5)
XORG=AMIN1 (ARMIN(APl,NP)ARMIN( APkvP))
11MAX~AA~.(ARRAXCAPlqNP) ARPAX( P29htP))CALL GRAF CNORG,'SCALE ,PA6
ARrqI N (IP .NP) .'SCALE'.*R'AU (TP ,Np))CALL CURVE(AP1. TPNP,10)CALL CURVE (AP2,TP,Npt1c)CALL CURVE (AP3,TP,NP. 10)CALL MESSAG('COV Q S.1009l., 8.9)CALL REALNOC Q00.ABUT-9-ABUT-)CALL MESSAG(' COW R =S'.ol0.D 'AEUT.sABUT')CALL REALNO(R(1 , ) .2q'ABUT'qA8UT')CALL MFSSAG(' 0 =S'. 100. 'ABUT'AOUT')CALL INTNO(NRuN, ART vAqUT')CALL DATE(WHEN)CALL TOFDAY(TIAE0D)CALL MESSAG(WI4EN001O1*.9.43)CALL MESSAG(7T4EOD,10,'AHU1'qABUT')I DUMmyxL INEST (IPA.K 150.80)CALL LINES('x(3)S.qIPAK 91)CALL LINESUAR S',IPAK92)CALL LINES(VY(3)S-, IPAK .3)CALL LEGEND(IPAK ,3,4.092.0)CALL ENDPL(O)
114.8 CONTINUECALL INTAXSCALL YAXANG(0.)CALL SIMPLYCALL TITLE(UTARGET GROUND PLOT ,10*OFF RANGE (Z) -YDS.S',1009
'DOWN RANGE (M VDS.S',100,6.,8.)CALL XTICKS(5CALL YTICKS(5X0R6z ARMIN(CZTP .NP)XMAX~u ARPAXCZTPNP)VMAXG&ARMAX(C KYR NP)CALL GRAF(XOR69'SCALE',XMqAXG,
* ARMINCATP ,NP),'SCALE ,VYmAXG)NPLSz 5CALL CURVE (ZTP.XTPNPoNPLS)
C DRAW + AT RADAR LOCATION. X7TRT v ZT=O.CALL MARKER('CALL SCLPIC(2.)RDRY=O.IF(RORXLE.YMAXG) CALL CURVE (RDRY,RDRX,1.1)CALL RESET(VSCLPIC')CALL RESETUmARKERD)CALL NESSAG(COV Q xS'910.91s. be9)CALL REALNOC Q.0.'APlUTP'AbU7u)CALL MESSAG(' COW A 2S',1009 .ABUT,ABUTE)CALL REALN(Rt,1 ).2qEABUTv,'A8UT')CALL MESSAG( 0 zS', 1009 ABUT.,'ABUT')CALL INTNO(NRUN,'ABUT' .'A8UT')CALL DATE(WHEN)CALL TOFOAT (TMFOD)CALL mESSAG(w*IFN,10,1 .*4 .3)
CALL MESSAG(TMiEOD,1O,'AbUT , 'ABUTo)CALL ENOPL(O)
116 CONTINUE118 CONTINUE115 CONTINUE
IF(ILASToNE.1) 60OTO 10CALL DONEPL
120 FORI"AT(1uIi.,1X,F6.1 ,3(F6.2),6(Fg.3).o.(F7.3))130 FORMAT (1X,J3. 4(Flfl.l),?CFl0.3))133 FORPA (lX ,14F9.2)150 FORIOAT (
ENtoD
92
SuPOROUTINE INTCONC INTCONd IN171LIZES TRAJECTORY VARIAbLES
DIMENSION PAUV(3)COMMNON XTYTZTVXTVYT,VZT,VELOC .TIANG,72ANG,DELR.I6,TRAT1,
.DELTNPRFTRAT2,Tl,NPH,A9U,IG2,ICTS,PI,IR,IH.IOSCOMINI/INTC1 AZ,EL.ROL1,ROL2,ROL4,AZDEG,ELDEGROLIbS ,ROL206,
*ROLI.06 ,T6iSI.T6S?,RT,RDOTT,RDOTT, DTTRAD1,TRAD29RAN6T,.RAN6TM,XR ,YR,ZR,VXTM,VYT04,VZT14*VR20,AX7,AYTAZTAxTG9AYTG,AZT6q'ANA ,ANE ,PAUVI TCS21AzZO.ELmO.AOL 1=0.ROL 230.R0L4=09AZDEGz0.E Lb ES=0.AOL 1 b60.ROL2DGzO.T1ANG2o.16=0o162=0DELT=DTNPRFa1
C 3-b TARGET TRAJECTORY DATA
C ORIGIN AT INITIAL TARGET POSITIONC RT z TRUE RANGE RELATIVE TO RADARC RDOTT = TRUE VELOCITY RELATIVE TO RADARC R200TT a TRUE ACCELFRA71ON RELATIVE TO RADAR
If (IR*EQ.1) RT=1lZ750IF(IR.EQ.2) RTlOOO0.IF(IR.EQ.3) RTmISO.
IF(IOSoEQ.1) RD0TTx -175.IF(IOS*E~o2) RDOT7-100.'R2bOTTzO.
C WELOC z TARGET VELOCITY RELATIVE TO ORIGINVELOC=-RDOTT
C TRAD- z TURN RADIUS OF TGS- 69 TURNTRADI=VELOC*VELOC/T6$1'1O.7252TRAD?2VELOC 'VELOC /TGS2*1O .7252
C TRAT- 2 TURN RATE OF TGS- Go TURN (RAD/SEC)7RAT1--TGS1*10.72S2/VELOCTRATi=T6S2*1O.7252/VELOCRANGTxRTRANGiTINRANGT-( RDOTT * ELT *FLO AT (NPRF)
C -R z RADAR LOCAT1ON AT TCO)XRzRTYR =0.ZR2O0
C -T a TRUE XY9l TARGET POSITIONXT0o.YTZO.ZT=G.
C V-T a TRUE X.YZ VELOCITYVXT=VELOCVYTGO.VZ T=0.VXTINVXTVYTPxVTVZTM=VZ T
VRD=RDOTT
C A-T z TRUE X*YZ ACCELERATIONAMT:fl,A x T zO0AYT:O.
AZT:O.
C A-T6 = TRUE XYZ ACCELERATION (G'S)AXTG=O.AYTG=O.AZTG=O.
C ANA VERTICAL (AZ) ACCELERATION RELATIVE TO TARGET ORIENTATION
C ANE = HORIZONTAL (EL) ACCELERATION RELATIVE TO TARGET ORIFNTATIONANAzO.ANE=0
C PAUV(I) M PITCH AXIS UNIT VECTORC PAUV(1) = XC PAUV(2) a ZC PAUV(3) a Y
PAUV(1)=: *PAUV(2)I.PAUV(3)=O.
E140
AO-AO84 413 NAVAL POSTGRADUATE SCHOOL MONTEREY CA F/6 17/9TWO DIMENS IONAL MATCHED FILTERING FOR ATTITUDE MEASUREMENT WITH- ETC(U)MAR GO Rt H ILLIAMS
UNCLASSIFIED NL
M2ENOMONEE
FLECSiS mvI ,Rc.mvP,m~vPT
C MAA.VR CALCULATES TRUE POSITIONS (XT,yT,ZT) A&V TRUE VELOCITIES IVXT,C VYT,VZT) TARCET STARTS @ ORIGIN 1 TsC, AND FLIUS TOWARC RADAR FIXED i
C LOCATION RT ON X AXISC X= DOWMN RANGE, REFERENCED TO TARCET a T rCC Y= uP PANGE, REFERENCED TO TARGET & T =rC Z OFF RANGE, REFEgENCED TO TARGET & T =0
SUBrOuTINE MANVRCOM-,ON XT,YT ,ZT,VXT,VYT,Vd'T,VELOC,TIANG,T2LANC-,OELRIG,TRAT!,
*DELT,NPRF,TRAT.-,II,NPH,mk9r,IG2,ICTS,PI,IO,IH,IOSCOMMON/,INTC/ AZ,ELPPOLIPOL,,ROiLiv',AZEG,ELOEG,POL ,[OG ,ROL20694ROL4OG ,T6S1,TGS?,PT,RflOTT,R200~TT, CT,TRA~l,TRAOZ2,QANGTq*RANJGTM,XR,YR,ZR,VXTM4,VYTmVZ7TM,VP2C DXT,AYT,AZT,AXTG,AYTC,DZTG,*ANAANEPAUV
C #POL4 ALON6 X-AXIS RGTATES PITCH VECTOR CLOCK(WISE* (FROM' XYZC C,Ul~, TO u,-AtS )C RR POLL RATE OF AIRCRAFT (DEG/SEC)
C RRR ROLL RATE OF AIRCRAFT (PAD/SEC)C ICTS COORDINATED TURN SUITCH
DT=OELTRR=8!.
PR=RR*PIj 18G.oCONDITIONAL
" fII.LToNPI)C 0 . INITIAL TRAJECTORY
* a ITCS=O0 .xT:xT*CELR
C & 'MEASURED* AIRCRAFT POLL, PRECEEDING* MANUVFh
0 ROL4=ROL+(RR*0T)oo..FIN
* TIANGeLToR901
C .*SECOND TRAJFCTORY* OL4=ROL4#,IF.R*DT)*.IF(ROL~I.GTsI.55)
* . *ROL4=1.r,5
*~ ~ I N.FIIF Of 1F6 9 T , l)
* 0 ICTS:3C * * MEASURED* AIPCPAFT RCLL, PPECFEDING MANUVEQ
* . .ROL4=ROL4-tRRR*DT3*~ ~ I .. FN
I G I G.1C .. TIANG = AZ ANGLE OF TARGrT
**TIANG:TRATl.DELT*FLOATI NPRFI*IG*.XT:XT*CELR*iCOSITIANC)
**ZT=ZT#VELR*SIN(TlANG)
C . R9'n V XT = ... VZT = VLflC
**vxT=VELOC.CCS(TlANG)*.VZT:VELOC*Sj:lTIPf-Gl
I :;.FIN* .OUE.
95
c TH I r G1 TR skECTC.YC " IA 't: 6SWt Lr' I IRCFAAF T PCLL
0 IF TFIWL49-T.e2CeC* * * RLA.:PGL4-lPRIA*DT)
**IF(FOL4.LT.Lut
Iu 12=IC?.10 T A,&:=TRAT'L*fLLT*FtOATrINPRF3*IGZ-
a 0 YT=YT+FiELR*SINII2A G)0 0 2T=&'T+CELP*COS(T2AN )* 0 vYTZvELOC*SINtT2ANG)
**VZT=VELOC*COS5A~NG
.. F I N
RETUPNEND
(FLECS VERSION. P22,6)
------------ ---- ---- ---- ---- ----
C SUPROUTJNE FTKALI (KomN.6.T.S.Q.RPIN.ISILN.ulLTl T2.C-FTKALI -------- S
C * IT,T3oTX.IER)
C FUNCTION - ITERATED KALMAN FILTERINGC USAGE - CALL FTKAIE(K,g.Hk6.YSotRPtINISlLN.M.ILC TIoT2,ITvT3qTXq1ER)C PARAMETERS K - INPUT STEP COUNTER. K20.120... WHEN K209C VECTOR X SHOULD CONTAIN THE PRIOR EST1MATEC OF THE MEAN OF X, AND THE PROGRAMC CALCULATES THE ESTIMATED VARIANCE OFC X AS P=GOG-TRANSPOSE AT STEP 0.C X - INPUT/OUTPUT VECTOR Of LENGTH N. ON INPUT,C X IS THE STATE VECTOR AT STEP K, AND ONC OUTPUT, X CONTAINS THE ESTIMATED STATEC VECTOR AT STEP K4loC H - INPUT MATRIX OF DIMENSION N BY No H IS THEC TRANSITION MATRIX AT STEP K.C 6 - INPUT MATRIX OF DIMENSION N BY L AT STEP K.C -- INPUT OBSERVATION VECTOR OF LENGTH PI ATC STEP K+I.C S - INPUT MATRIX OF DIMENSION MI BY N AT STEP K*C a - INPUT COVARIANCE MATRIX OF U. OF DIMENSION LC AT STEP K.C R - INPUT COVARIANCE MATRIX OF V. OF DIMENSIONC PI BY PI AT STEP K#1eC P - INPUT/OUTPUT MATRIX Of DIMENSION N BY N.C ON INPUT, P 15 THE VARIANCE MATRIX OF IC AT STEP K. ON OUTPUT, P IS THE ESTIMATEDC VARIANCE MATRIX OF X AT STEP K+I.C IN - FIRST DIMENSION OF H.G. AND P AS DIMENSIONEDC IN CALLING PROGRAM.C is - FIRST DIMENSION OF S AND R AS DIMENSIONED IN
C CALLING PkO6RAM.C IL - FIRST DIMENSION Of Q AS DIMENSIONED INC CALLING PROGRAM.C N - INPUT. SEE DESCRIPTIONS OF X*H,69M.P-C N MUST BE GREATER THAN 0.C M1 - INPUT* SEE DESCRIPTIONS OF YoS*R*T!oC M1 MUST BE GREATER THAN 0.C L - INPUT. SEE DESCRIPTIONS OF 6.0.C L MUST BE GREATER THAN OC TI - WORK MATRIX OF DIMENSION NM BY NM WHEREC NM IS THE MAXIMUM OF N AND Ml,C T2 - WORK MATRIX Of DIMENSION NM BY NML. WHEREC NML IS THE MAXIMUM OF NM AND L.C IT - FIRST DIMENSION OF TI AND T2 ASC DIMENSIONED IN CALLING PROGRAM.C T3 - WORK VECTOR OF LENGTH Ml.C TX - INPUT. ITERATED XTENDED THRESHOLDC IER - ERROR PARAMETER. IER - 0 IMPLIES NO ERROR.C TERMINAL ERROR a 12t*%C N x I INDICATES O&F OF IN. IS, IL. OR ITC IS TOO SMALL. OR THAT ONE OF N MisC OR L IS NOT A POSITIVE INTEGER.
C N 2 2 INDICATES "ATPIX INVERSION FAILED.C PRECISION - SIN6LEC RfQ'D IMSL ROUTINES - LEGT1F.LUJDA1F.LUFLf*F,UFR1ST,VUULFF,VMULFP
C LAN16UAGE - FORYRANC-----------------------------------C LATEST REVISION -MAY 10, 19~79C PODELC X(K.1) H (K).NAK).6(IC)'U(K)C Y(K.l) aSCK*I)'E(K+1)+Vtg,1)
CSUBROUTINE' FTKALI (KvXviq,6,Y9sQoRvP,INtIS9IL,
NMiLT1,T2, ITo, a, .1IERIPS)C
DIMENSION (,(N,)(I,)YC)SS,)QI1)
DiMNENSION XE(20), XP(2O).&X(20) ,TX( li) .T420) ,,4NL(?C),*EAM (20) .FGCIT .1)DIMENSION T5(?(0) T6(O910)DATA 101019111~20120/IF (IN96EN *AND& IS*6E.M1 *AND. IL,6EoL
* AND* (IT.GEaN *OR. ITsGE*M1) oAIWDe N.GT.0)9 AND. N1s6T.O sANDe L.GT*O) 60 TO 5
left 12960 TO 9000
S lEA a 0C CALCULATE P IF K 3ZERO
C P(O) a 6(O)*Q(O)*6T(0)IF (K *NE* 0) 60 TO 10CALL V14ULFF (6,0,Q No Lo L#IN#IL#7TZ.ItIR)CALL VMULFP (T2, 69 No L, N#IT,1N9 PoINIER)IF(IPS9EQo0) 60 TO 166WAITE(6,160) (P(1,J)9Jx19lN)o(TX(l) .Im1l%)DO 16S 1=29INWR1TE(69161) (P(I ,J),J21lN)
165 CONTINUE166 CONTINUE160 FORMAT( P(0) z',1O(E1O.4))161 FORPAT(8K ,1u(E10.*.))10 CONTINUE
C SAVE INITIAL XDO 12 I1 oX K (3I )= X ( I
12 CONTINUEC CALCULATE X-PRINE AT STEP s.41C X1(K*1) 2 H(K)'E-HAT(K)
CALL VPULFF (he X, No ,1INTTIR00 15 1 19N
x (1 ) axP ( I)15 CONTINUE
IF(IPS*EQ.1) WRITE(6,170) (XP(i) .Illo)170 FORMAT( X-PRIME aM.! z',10(F1O.2))
C CALCULATE P-PRIME AT STEP K#1C P~(K+1)a ()PKOTKG(eQ)*T)
CALL VMULFF (Ho Po N, N, ,NI.T.TI
CALL VMULFP (Tit No No N, N,IT,1N, P,IN,IER)CALL VMALFF. ( 6,0.o N, L, LIN91LVT2,JTIER)CALL VMULFP (T2, 6, N, Le NITJN,T19ITIER)Do 25 1I 1 lR4
00 20 J X lNP(IJ) a P(19J) + TlCI,j)
20 CONTINUE25 CONTINUE
IF(IPS*EQ.0) G0 TO 186WRITE £6,180) (P0i J),Ja1 IN)00 185 IaZ2lt.WRITE(6,18I) (PCI ,J) ,ji, IN)
155 CONTINUE186 CONT7INUE180 FORMATVu P-PRIME z',j0(E1O.4))181 FORMATIX , 1O(F1O.4))
C CALCULATE 1MATRIX K AT STEP K~lC £ C£*1) z P'C£.1)'STCX.1)*(SC£.1)ePC£.1I)'STC£41).RC£.l))**-I
CALL V14ULFP ( S, PoR1, No NISq1NTZqlT,IEU)IFCIPS9EQ.0) 60 TO 28DO 27 Islo"1WRITE (6,187) 1 ,(T2CI,JJ) ,JJ=I ,P)
27 CONTINUE28 CONTINUE
CALL VMULFP CT2, 5,M1, k9M1,ITISrT1,ITIEft)DO 35 1 1,4
DO 30 a a9"TIC1,J) a Tl(I,J) * R(J,I)
30 CONTINUE
35 CONTINUE187 FORMAT(- S.P.ST*ftC-,I1,-,J) 2 T1 su,1O(F1O.2))
CALL LEQT1F(T1, t,MlvIZT2,1OT3,IER)If (JER *EQ* 0) 60 TO 40IER a 13060 TO 9000
40 DO 50 1 a 1'"I0O 45 j aI,
T1091x) - TZ(!,J)F6(Jvl)=TI(J,1)
45 CONTINUEso CONTINUE
IF(IPS.EQ.O) GO- TO 194WRITE (6,190) (Ti (1,1) ,11 ,N1)DO 193 I22.JIN
193 CONTINUE194 COTINUE190 FORMAT(' K x Ti *a,1OCF1O.2))191 FOOMAMCOX ,10( F1O.?))
C CALCULATE X-HA7 AT STEP K*1C KALMAN ESTIMATEC X-14ATfK41) auCc~~c1.3K1*'K-C*
KL2000 53 I1.IN
L 9L
F x I(I)=(?.F x M( 1 1F 9
53 CON7INUE54 CONTINUE
DO 542 IZ1,INT4(I)=XP( J)-XCJ)
542? CONTIINUEC 01S-. ERR, Z S(X'-)()-Y..sNL T5
55 CALL VMULFF ( So X.Ml, Noll ,ISINqT3vISqIER)CALL VMULFF ( S974,141, ,N,I1,IS*INT5,IS,!ER)CALL VM4ULFF( So XtM1, NX1,ISINvhLI209IER)DO 60 1 1 WMI
73(l) T3(I) - Y(I)60 CONTINUE
IF(IPS*EQ.1) wRZTE(6,195)(31)Iz.) XI)Il)'195 FORMAT( OS ERR - S.X-Y a'912CF9*2))
C Ti a GAIN KCALL VMULFF (T1,73, N11JT.JoSTZ.ITIE6R)DO 65 1 a I1,N
xC!) a XP(I) - T2(191)T6(1,1)=T2(I.1)
65 CONTINUEIF(IPS.EQ.1) WRITE(69200)((I.= ),ZC1)I,)
200 FORMAT( X-HAT ='*V10(FIO.2))69 CONTINUE
C CALCULATE P AT STEP K41C P(K.1) a PK1-(*)SKI*'KI
CALL VMULFF (Ti, So Nq~1, %,IT,1ST?,ITqIER)CALL VMULFF (729 Ps N, No NoIToINTiIT,RER)DO 75 1 aiN72(I.1)zT6(I,i)
DO 70 J a IsNPCjI,) a P(Ij) - TI(1.j)
70 CONTINUE75 CONTINUE
IF(IPS*EQ*D) 60 TO 216WRITE(6,210) (P(i ,J),Jul ,IN)DO 215 1329IN
W1 CNINE6~i PIJ9=9216 CONTINUE
26C0 10 9005900 COINU590CALLINUEATT(EHTA"
CA05 REUR S IR6HTA105 RETRA~N 1,XFi1O I2)130 FORMAT(1XI3,12(F6*O.3)) l2)130 FORMIAT(X,13,12CFIO.3))10 FOR1*AT(' P '3.1(F0*2))211 FOR0AT( P 2x.0(10(FI)))21EOMA(ND0(l*)
(00
BIBLIOGRAPHY
1. By: Infared Countermeasures Branch, Infared Signatures: Theory,Techniques, and Equipment, NWC Tech. Publ. 4902, 1971.
2. S. A. Dudani, An Experimental Study of Moment Methods for AutomaticIdentification of Three Dimensional Objects from TV Images, Ohio St.Univ., PhD. Diss., 1973.
3. R. L. Plo, "Euler Angle Transforms", IEEE Trans. on Auto. Control,Vol. AC-ll, no. 4, pp. 707-715, Oct. 1966.
4. E. H. Ehling, Range Instrumentation, Prentice-Hall Inc., EnglewoodCliffs, N. J., 1967.
5. A. V. Oppenheim and R. W. Schafer, Digital Signal Processing,Prentice-Hall, Inc., Englewood Cliffs, N. J., 1975.
6. W. C. Effinger, Fortran Subroutine to Calculate Projected SurfaceAreas of Rotated Three Dimensional Objects, NWC Memo Reg. 4055-34-73,1973.
7. A. Rosenfeld, "A Non-Linear Edge Detection Technique", Proc. of theIEEE, pp. 814-816, May 1970.
8. R. L. Fox, Optimization Methods for Engineering Design, Addison-Wesley, Reading, Mass., 1971.
9. E. 0. Brigham, The Fast Fourier Transform, Prentice-Hall, EnglewoodCliffs, N. J., 1974.
10. J. D. Kendrick, and others, "Estimation of Aircraft Target MotionUsing Pattern Recognition Orientation Measurements," presented atthe IEEE Decision and Control Conference, San Diego, CA., December1978.
11. M. D. Levine, "Feature Extraction: A Survey", Proceedings of theIEEE, Vol. 57, pp. 1391-1407, August 1969.
12. I. E. Abdou and W. K. Pratt, "Quantative Design and Evaluation ofEnhancement/Thresholding Edge Detectors", Proceedings of the IEEE,Vol. 67, pp. 753-763, May 1979.
13. R. L. T. Hampton, A Guided Tour of Kalman Filtering, NWC TechnicalNote 4070-58-75, 1975.
14. By: International Mathematical and Statistical Libraries, Inc.,Houston, Texas, IMSL Library 2, Reference Manual, pp. FTKALM-l -
FTKALM-4, Edition 6, 1977.
101
INITIAL DISTRIBUTION LIST
No. Copies
1. Defense Documentation Center 2Cameron StationAlexandria, Virginia 22314
2. Library, Code 0142 2Naval Postgraduate SchoolMonterey, California 93940
3. Department Chairman, Code 62Department of Electrical EngineeringNaval Postgraduate SchoolMonterey, California 93940
4. Professor H. Titus, Code 62TsDepartment of Electrical EngineeringNaval Postgraduate SchoolMonterey, California 93940
5. Robert H. Williams 5Naval Weapons CenterCode 62201China Lake, California 93555
102
i m l in-L, . . . . . l .li l l-!
