home *** CD-ROM | disk | FTP | other *** search
/ Power-Programmierung / CD1.mdf / fortran / library / whets / tcorl.c < prev    next >
Encoding:
C/C++ Source or Header  |  1994-07-05  |  6.1 KB  |  180 lines
  1. /*  VERSION OF STP SUBROUTINE TCORL: SINGLE PRECISION      */
  2. /*  PDL VERSION                                            */
  3.  
  4. main()
  5. {
  6. int   i,j;
  7. int   coptr,zlptr;   
  8. int   iii; 
  9. float zcm,tstat,deltim;   
  10. float ztc8,ztc4;      
  11. float coordx,coordy,coordz;   
  12. float coorvx,coorvy,coorvz;      
  13. float thee35,thee45;
  14. float cov11,cov13,cov22,cov24;
  15. float cov33,cov35,cov44,cov45;
  16. float sigdz1,sigdz2,sigdz3;
  17. float sigvz1,sigvz2,sigvz3;
  18. float deltdx,deltdy,deltdz;
  19. float deltvx,deltvy,deltvz;
  20. float deltdz1,deltdz2,deltdz3;
  21. float deltvz1,deltvz2,deltvz3;
  22. float sigcz1,sigcz2,sigcz3;
  23. float sigtz1,sigtz2,sigtz3;
  24. float zstm[1][100];   
  25. float zsfr[6][100];   
  26. float ztacc[3][100];
  27. float zrvo[3][100];
  28. float zdrgx[3][100];
  29. float zrvi[15][100];   
  30. float ztmat[9][100];   
  31. int phase;
  32.  
  33. printf("enter phase prior to halting (1 = initialization only) \n");
  34. scanf ("%6d", &phase);
  35.  
  36.  
  37.    for (i = 0; i < 100; i++)
  38.       zstm[0][i] = 3.0;
  39.  
  40.    for (i = 0; i < 6; i++)
  41.       for (j = 0; j < 100; j++)
  42.          zsfr[i][j] = 3.0;
  43.  
  44.    for (i = 0; i < 3; i++)
  45.       for (j = 0; j < 100; j++){
  46.          ztacc[i][j] = 3.0;
  47.          zrvo[i][j] = 3.0;
  48.          zdrgx[i][j] = 3.0;
  49.       }
  50.  
  51.    for (i = 0; i < 15; i++)
  52.       for (j = 0; j < 100; j++)
  53.          zrvi[i][j] = 3.0;
  54.  
  55.    for (i = 0; i < 9; i++)
  56.       for (j = 0; j < 100; j++)
  57.          ztmat[i][j] = 3.0;
  58.  
  59. /*  READ OR INITIALIZE ALL VARIABLES  */
  60.  
  61.    zlptr = 100;
  62.    zstm[0][0] = 5.0;
  63.  
  64.    for (iii = 0; iii < 1; iii++){
  65. /*  PASS LABEL TO ASSEMBLY LANGUAGE   */
  66.  
  67. /*      asm("timstr:"); */
  68.       for (i = 0; i < 1; i++){
  69. /*  START BENCHMARK 1  ********************************************** */
  70.  
  71. if (1 == phase) 
  72.   exit(1);
  73.  
  74. /*  INITIALIZE THE CANDIDATE OBJECT POINTER  */
  75.          coptr = 0;
  76.  
  77. /*  INCREMENT TRTT'S TIMER  */
  78.          zcm = zcm + ztc8;
  79.  
  80. /*  SET THE CANDIDATE OBJECT POINTER TO THE NEXT OBJECT  */
  81.          do {
  82.             coptr = coptr + 1;
  83.  
  84. /*  PROPAGATE CANDIDATE OBJECT'S RADAR FACE CARTESIAN COORDINATE
  85.    (RFCC) STATE VECTOR TO THE TIME OF THE TEST OBJECT'S STATE VECTOR */
  86.  
  87.             deltim = zstm[0][0] - zstm[0][coptr];
  88.             coordx = zsfr[0][coptr] +
  89.                ( zsfr[3][coptr] + 0.5 * ztacc[0][coptr] * deltim) * deltim;
  90.             coordy = zsfr[1][coptr] +
  91.                ( zsfr[4][coptr] + 0.5 * ztacc[1][coptr] * deltim) * deltim;
  92.             coordz = zsfr[2][coptr] +
  93.                ( zsfr[5][coptr] + 0.5 * ztacc[2][coptr] * deltim) * deltim;
  94.             coorvx = zsfr[3][coptr] + ztacc[0][coptr] * deltim;
  95.             coorvy = zsfr[4][coptr] + ztacc[1][coptr] * deltim;
  96.             coorvz = zsfr[5][coptr] + ztacc[2][coptr] * deltim;
  97.  
  98. /*  PROPAGATE THE RVCC STATE ERROR COVARIANCE ELEMENTS OF THE 
  99.                   CANDIDATE OBJECT  */
  100.  
  101.             thee35 = deltim *
  102.                (zdrgx[0][coptr] * ztmat[0][coptr]
  103.                +zdrgx[1][coptr] * ztmat[3][coptr]
  104.                +zdrgx[2][coptr] * ztmat[6][coptr] );
  105.  
  106.             thee45 = deltim *
  107.                (zdrgx[0][coptr] * ztmat[0][coptr]
  108.                +zdrgx[1][coptr] * ztmat[4][coptr]
  109.                +zdrgx[2][coptr] * ztmat[7][coptr] );
  110.  
  111.             cov11 = zrvi[0][coptr] + zrvi[2][coptr] * deltim;
  112.             cov13 = zrvi[2][coptr] + zrvi[9][coptr] * deltim;
  113.             cov22 = zrvi[5][coptr] + zrvi[7][coptr] * deltim;
  114.             cov24 = zrvi[7][coptr] + zrvi[12][coptr] * deltim;
  115.             cov33 = zrvi[9][coptr] + zrvi[11][coptr] * thee35;
  116.             cov35 = zrvi[11][coptr] + zrvi[14][coptr] * thee35;
  117.             cov44 = zrvi[12][coptr] + zrvi[13][coptr] * thee45;
  118.             cov45 = zrvi[13][coptr] + zrvi[14][coptr] * thee45;
  119.  
  120.             sigdz1 = cov11 + cov13 * deltim;
  121.             sigdz2 = cov22 + cov24 * deltim;
  122.             sigvz1 = cov33 + cov35 * thee35;
  123.             sigvz2 = cov44 + cov45 * thee45;
  124.             sigdz3 = zrvo[1][coptr] +
  125.                (2.0 * zrvo[1][coptr] + zrvo[2][coptr] * deltim) * deltim;
  126.             sigvz3 = zrvo[2][coptr];
  127.  
  128. /*  TAKE THE DIFFERENCE BETWEEN TEST AND CANDIDATE OBJECT STATES  */
  129.             deltdx = zsfr[0][0] - coordx;
  130.             deltdy = zsfr[1][0] - coordy;
  131.             deltdz = zsfr[2][0] - coordz;
  132.             deltvx = zsfr[3][0] - coorvx;
  133.             deltvy = zsfr[4][0] - coorvy;
  134.             deltvz = zsfr[5][0] - coorvz;
  135.  
  136. /*  TRANSFORM THE DIFFERENCE VECTOR FROM RFCC TO RVCC  */
  137.             deltdz1 = ztmat[0][0] * deltdx
  138.                + ztmat[3][0] * deltdy
  139.                + ztmat[6][0] * deltdz;
  140.             deltdz2 = ztmat[1][0] * deltdx
  141.                + ztmat[4][0] * deltdy
  142.                + ztmat[7][0] * deltdz;
  143.            deltdz3 = ztmat[2][0] * deltdx
  144.                + ztmat[5][0] * deltdy
  145.                + ztmat[8][0] * deltdz;
  146.            deltvz1 = ztmat[0][0] * deltvx
  147.                + ztmat[3][0] * deltvy
  148.                + ztmat[6][0] * deltvz;
  149.            deltvz2 = ztmat[1][0] * deltvx
  150.                + ztmat[4][0] * deltvy
  151.                + ztmat[7][0] * deltvz;
  152.            deltvz3 = ztmat[2][0] * deltvx
  153.                + ztmat[5][0] * deltvy
  154.                + ztmat[8][0] * deltvz;
  155.  
  156. /*  COMPUTE THE VARIANCE OF THE STATE VECTOR DIFFERENCES  */
  157.            sigcz1 = sigdz1 + zrvi[0][0];
  158.            sigcz2 = sigdz2 + zrvi[5][0];
  159.            sigtz1 = sigvz1 + zrvi[9][0];
  160.            sigtz2 = sigvz2 + zrvi[12][0];
  161.            sigcz3 = sigdz3 + zrvo[0][0];
  162.            sigtz3 = sigvz3 + zrvo[2][0];
  163.  
  164. /*  COMPUTE THE 6 D.O.F. RVCC TEST STATISTICS  */
  165.            tstat = (deltdz1*deltdz1)/sigcz1 * (deltdz2*deltdz2)/sigcz2
  166.               + (deltdz3*deltdz3)/sigcz3 * (deltvz1*deltvz1)/sigtz1
  167.               + (deltvz2*deltvz2)/sigtz2 * (deltvz3*deltvz3)/sigtz3;
  168.  
  169. /*  INCREMENT TRTT'S TIMER  */
  170.             zcm = zcm + ztc4;
  171.  
  172. /*  CONTINUE LOOP.  EXIT ROUTINE AFTER LAST CANDIDATE OBJECT  */
  173.  
  174.          } while (coptr < zlptr);  
  175. /*  EXIT BENCHMARK 1  *********************************************  */
  176.       }     
  177. /*      asm("timend:"); */
  178.    }
  179. }
  180.