home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Power-Programmierung
/
CD1.mdf
/
fortran
/
library
/
whets
/
tcorl.c
< prev
next >
Wrap
C/C++ Source or Header
|
1994-07-05
|
6KB
|
180 lines
/* VERSION OF STP SUBROUTINE TCORL: SINGLE PRECISION */
/* PDL VERSION */
main()
{
int i,j;
int coptr,zlptr;
int iii;
float zcm,tstat,deltim;
float ztc8,ztc4;
float coordx,coordy,coordz;
float coorvx,coorvy,coorvz;
float thee35,thee45;
float cov11,cov13,cov22,cov24;
float cov33,cov35,cov44,cov45;
float sigdz1,sigdz2,sigdz3;
float sigvz1,sigvz2,sigvz3;
float deltdx,deltdy,deltdz;
float deltvx,deltvy,deltvz;
float deltdz1,deltdz2,deltdz3;
float deltvz1,deltvz2,deltvz3;
float sigcz1,sigcz2,sigcz3;
float sigtz1,sigtz2,sigtz3;
float zstm[1][100];
float zsfr[6][100];
float ztacc[3][100];
float zrvo[3][100];
float zdrgx[3][100];
float zrvi[15][100];
float ztmat[9][100];
int phase;
printf("enter phase prior to halting (1 = initialization only) \n");
scanf ("%6d", &phase);
for (i = 0; i < 100; i++)
zstm[0][i] = 3.0;
for (i = 0; i < 6; i++)
for (j = 0; j < 100; j++)
zsfr[i][j] = 3.0;
for (i = 0; i < 3; i++)
for (j = 0; j < 100; j++){
ztacc[i][j] = 3.0;
zrvo[i][j] = 3.0;
zdrgx[i][j] = 3.0;
}
for (i = 0; i < 15; i++)
for (j = 0; j < 100; j++)
zrvi[i][j] = 3.0;
for (i = 0; i < 9; i++)
for (j = 0; j < 100; j++)
ztmat[i][j] = 3.0;
/* READ OR INITIALIZE ALL VARIABLES */
zlptr = 100;
zstm[0][0] = 5.0;
for (iii = 0; iii < 1; iii++){
/* PASS LABEL TO ASSEMBLY LANGUAGE */
/* asm("timstr:"); */
for (i = 0; i < 1; i++){
/* START BENCHMARK 1 ********************************************** */
if (1 == phase)
exit(1);
/* INITIALIZE THE CANDIDATE OBJECT POINTER */
coptr = 0;
/* INCREMENT TRTT'S TIMER */
zcm = zcm + ztc8;
/* SET THE CANDIDATE OBJECT POINTER TO THE NEXT OBJECT */
do {
coptr = coptr + 1;
/* PROPAGATE CANDIDATE OBJECT'S RADAR FACE CARTESIAN COORDINATE
(RFCC) STATE VECTOR TO THE TIME OF THE TEST OBJECT'S STATE VECTOR */
deltim = zstm[0][0] - zstm[0][coptr];
coordx = zsfr[0][coptr] +
( zsfr[3][coptr] + 0.5 * ztacc[0][coptr] * deltim) * deltim;
coordy = zsfr[1][coptr] +
( zsfr[4][coptr] + 0.5 * ztacc[1][coptr] * deltim) * deltim;
coordz = zsfr[2][coptr] +
( zsfr[5][coptr] + 0.5 * ztacc[2][coptr] * deltim) * deltim;
coorvx = zsfr[3][coptr] + ztacc[0][coptr] * deltim;
coorvy = zsfr[4][coptr] + ztacc[1][coptr] * deltim;
coorvz = zsfr[5][coptr] + ztacc[2][coptr] * deltim;
/* PROPAGATE THE RVCC STATE ERROR COVARIANCE ELEMENTS OF THE
CANDIDATE OBJECT */
thee35 = deltim *
(zdrgx[0][coptr] * ztmat[0][coptr]
+zdrgx[1][coptr] * ztmat[3][coptr]
+zdrgx[2][coptr] * ztmat[6][coptr] );
thee45 = deltim *
(zdrgx[0][coptr] * ztmat[0][coptr]
+zdrgx[1][coptr] * ztmat[4][coptr]
+zdrgx[2][coptr] * ztmat[7][coptr] );
cov11 = zrvi[0][coptr] + zrvi[2][coptr] * deltim;
cov13 = zrvi[2][coptr] + zrvi[9][coptr] * deltim;
cov22 = zrvi[5][coptr] + zrvi[7][coptr] * deltim;
cov24 = zrvi[7][coptr] + zrvi[12][coptr] * deltim;
cov33 = zrvi[9][coptr] + zrvi[11][coptr] * thee35;
cov35 = zrvi[11][coptr] + zrvi[14][coptr] * thee35;
cov44 = zrvi[12][coptr] + zrvi[13][coptr] * thee45;
cov45 = zrvi[13][coptr] + zrvi[14][coptr] * thee45;
sigdz1 = cov11 + cov13 * deltim;
sigdz2 = cov22 + cov24 * deltim;
sigvz1 = cov33 + cov35 * thee35;
sigvz2 = cov44 + cov45 * thee45;
sigdz3 = zrvo[1][coptr] +
(2.0 * zrvo[1][coptr] + zrvo[2][coptr] * deltim) * deltim;
sigvz3 = zrvo[2][coptr];
/* TAKE THE DIFFERENCE BETWEEN TEST AND CANDIDATE OBJECT STATES */
deltdx = zsfr[0][0] - coordx;
deltdy = zsfr[1][0] - coordy;
deltdz = zsfr[2][0] - coordz;
deltvx = zsfr[3][0] - coorvx;
deltvy = zsfr[4][0] - coorvy;
deltvz = zsfr[5][0] - coorvz;
/* TRANSFORM THE DIFFERENCE VECTOR FROM RFCC TO RVCC */
deltdz1 = ztmat[0][0] * deltdx
+ ztmat[3][0] * deltdy
+ ztmat[6][0] * deltdz;
deltdz2 = ztmat[1][0] * deltdx
+ ztmat[4][0] * deltdy
+ ztmat[7][0] * deltdz;
deltdz3 = ztmat[2][0] * deltdx
+ ztmat[5][0] * deltdy
+ ztmat[8][0] * deltdz;
deltvz1 = ztmat[0][0] * deltvx
+ ztmat[3][0] * deltvy
+ ztmat[6][0] * deltvz;
deltvz2 = ztmat[1][0] * deltvx
+ ztmat[4][0] * deltvy
+ ztmat[7][0] * deltvz;
deltvz3 = ztmat[2][0] * deltvx
+ ztmat[5][0] * deltvy
+ ztmat[8][0] * deltvz;
/* COMPUTE THE VARIANCE OF THE STATE VECTOR DIFFERENCES */
sigcz1 = sigdz1 + zrvi[0][0];
sigcz2 = sigdz2 + zrvi[5][0];
sigtz1 = sigvz1 + zrvi[9][0];
sigtz2 = sigvz2 + zrvi[12][0];
sigcz3 = sigdz3 + zrvo[0][0];
sigtz3 = sigvz3 + zrvo[2][0];
/* COMPUTE THE 6 D.O.F. RVCC TEST STATISTICS */
tstat = (deltdz1*deltdz1)/sigcz1 * (deltdz2*deltdz2)/sigcz2
+ (deltdz3*deltdz3)/sigcz3 * (deltvz1*deltvz1)/sigtz1
+ (deltvz2*deltvz2)/sigtz2 * (deltvz3*deltvz3)/sigtz3;
/* INCREMENT TRTT'S TIMER */
zcm = zcm + ztc4;
/* CONTINUE LOOP. EXIT ROUTINE AFTER LAST CANDIDATE OBJECT */
} while (coptr < zlptr);
/* EXIT BENCHMARK 1 ********************************************* */
}
/* asm("timend:"); */
}
}