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.  
  3. /*  PDL VERSION                                            */
  4.  
  5.  
  6.  
  7. main()
  8.  
  9. {
  10.  
  11. int   i,j;
  12.  
  13. int   coptr,zlptr;   
  14.  
  15. int   iii; 
  16.  
  17. float zcm,tstat,deltim;   
  18.  
  19. float ztc8,ztc4;      
  20.  
  21. float coordx,coordy,coordz;   
  22.  
  23. float coorvx,coorvy,coorvz;      
  24.  
  25. float thee35,thee45;
  26.  
  27. float cov11,cov13,cov22,cov24;
  28.  
  29. float cov33,cov35,cov44,cov45;
  30.  
  31. float sigdz1,sigdz2,sigdz3;
  32.  
  33. float sigvz1,sigvz2,sigvz3;
  34.  
  35. float deltdx,deltdy,deltdz;
  36.  
  37. float deltvx,deltvy,deltvz;
  38.  
  39. float deltdz1,deltdz2,deltdz3;
  40.  
  41. float deltvz1,deltvz2,deltvz3;
  42.  
  43. float sigcz1,sigcz2,sigcz3;
  44.  
  45. float sigtz1,sigtz2,sigtz3;
  46.  
  47. float zstm[1][100];   
  48.  
  49. float zsfr[6][100];   
  50.  
  51. float ztacc[3][100];
  52.  
  53. float zrvo[3][100];
  54.  
  55. float zdrgx[3][100];
  56.  
  57. float zrvi[15][100];   
  58.  
  59. float ztmat[9][100];   
  60.  
  61. int phase;
  62.  
  63.  
  64.  
  65. printf("enter phase prior to halting (1 = initialization only) \n");
  66.  
  67. scanf ("%6d", &phase);
  68.  
  69.  
  70.  
  71.  
  72.  
  73.    for (i = 0; i < 100; i++)
  74.  
  75.       zstm[0][i] = 3.0;
  76.  
  77.  
  78.  
  79.    for (i = 0; i < 6; i++)
  80.  
  81.       for (j = 0; j < 100; j++)
  82.  
  83.          zsfr[i][j] = 3.0;
  84.  
  85.  
  86.  
  87.    for (i = 0; i < 3; i++)
  88.  
  89.       for (j = 0; j < 100; j++){
  90.  
  91.          ztacc[i][j] = 3.0;
  92.  
  93.          zrvo[i][j] = 3.0;
  94.  
  95.          zdrgx[i][j] = 3.0;
  96.  
  97.       }
  98.  
  99.  
  100.  
  101.    for (i = 0; i < 15; i++)
  102.  
  103.       for (j = 0; j < 100; j++)
  104.  
  105.          zrvi[i][j] = 3.0;
  106.  
  107.  
  108.  
  109.    for (i = 0; i < 9; i++)
  110.  
  111.       for (j = 0; j < 100; j++)
  112.  
  113.          ztmat[i][j] = 3.0;
  114.  
  115.  
  116.  
  117. /*  READ OR INITIALIZE ALL VARIABLES  */
  118.  
  119.  
  120.  
  121.    zlptr = 100;
  122.  
  123.    zstm[0][0] = 5.0;
  124.  
  125.  
  126.  
  127.    for (iii = 0; iii < 1; iii++){
  128.  
  129. /*  PASS LABEL TO ASSEMBLY LANGUAGE   */
  130.  
  131.  
  132.  
  133. /*      asm("timstr:"); */
  134.  
  135.       for (i = 0; i < 1; i++){
  136.  
  137. /*  START BENCHMARK 1  ********************************************** */
  138.  
  139.  
  140.  
  141. if (1 == phase) 
  142.  
  143.   exit(1);
  144.  
  145.  
  146.  
  147. /*  INITIALIZE THE CANDIDATE OBJECT POINTER  */
  148.  
  149.          coptr = 0;
  150.  
  151.  
  152.  
  153. /*  INCREMENT TRTT'S TIMER  */
  154.  
  155.          zcm = zcm + ztc8;
  156.  
  157.  
  158.  
  159. /*  SET THE CANDIDATE OBJECT POINTER TO THE NEXT OBJECT  */
  160.  
  161.          do {
  162.  
  163.             coptr = coptr + 1;
  164.  
  165.  
  166.  
  167. /*  PROPAGATE CANDIDATE OBJECT'S RADAR FACE CARTESIAN COORDINATE
  168.  
  169.    (RFCC) STATE VECTOR TO THE TIME OF THE TEST OBJECT'S STATE VECTOR */
  170.  
  171.  
  172.  
  173.             deltim = zstm[0][0] - zstm[0][coptr];
  174.  
  175.             coordx = zsfr[0][coptr] +
  176.  
  177.                ( zsfr[3][coptr] + 0.5 * ztacc[0][coptr] * deltim) * deltim;
  178.  
  179.             coordy = zsfr[1][coptr] +
  180.  
  181.                ( zsfr[4][coptr] + 0.5 * ztacc[1][coptr] * deltim) * deltim;
  182.  
  183.             coordz = zsfr[2][coptr] +
  184.  
  185.                ( zsfr[5][coptr] + 0.5 * ztacc[2][coptr] * deltim) * deltim;
  186.  
  187.             coorvx = zsfr[3][coptr] + ztacc[0][coptr] * deltim;
  188.  
  189.             coorvy = zsfr[4][coptr] + ztacc[1][coptr] * deltim;
  190.  
  191.             coorvz = zsfr[5][coptr] + ztacc[2][coptr] * deltim;
  192.  
  193.  
  194.  
  195. /*  PROPAGATE THE RVCC STATE ERROR COVARIANCE ELEMENTS OF THE 
  196.  
  197.                   CANDIDATE OBJECT  */
  198.  
  199.  
  200.  
  201.             thee35 = deltim *
  202.  
  203.                (zdrgx[0][coptr] * ztmat[0][coptr]
  204.  
  205.                +zdrgx[1][coptr] * ztmat[3][coptr]
  206.  
  207.                +zdrgx[2][coptr] * ztmat[6][coptr] );
  208.  
  209.  
  210.  
  211.             thee45 = deltim *
  212.  
  213.                (zdrgx[0][coptr] * ztmat[0][coptr]
  214.  
  215.                +zdrgx[1][coptr] * ztmat[4][coptr]
  216.  
  217.                +zdrgx[2][coptr] * ztmat[7][coptr] );
  218.  
  219.  
  220.  
  221.             cov11 = zrvi[0][coptr] + zrvi[2][coptr] * deltim;
  222.  
  223.             cov13 = zrvi[2][coptr] + zrvi[9][coptr] * deltim;
  224.  
  225.             cov22 = zrvi[5][coptr] + zrvi[7][coptr] * deltim;
  226.  
  227.             cov24 = zrvi[7][coptr] + zrvi[12][coptr] * deltim;
  228.  
  229.             cov33 = zrvi[9][coptr] + zrvi[11][coptr] * thee35;
  230.  
  231.             cov35 = zrvi[11][coptr] + zrvi[14][coptr] * thee35;
  232.  
  233.             cov44 = zrvi[12][coptr] + zrvi[13][coptr] * thee45;
  234.  
  235.             cov45 = zrvi[13][coptr] + zrvi[14][coptr] * thee45;
  236.  
  237.  
  238.  
  239.             sigdz1 = cov11 + cov13 * deltim;
  240.  
  241.             sigdz2 = cov22 + cov24 * deltim;
  242.  
  243.             sigvz1 = cov33 + cov35 * thee35;
  244.  
  245.             sigvz2 = cov44 + cov45 * thee45;
  246.  
  247.             sigdz3 = zrvo[1][coptr] +
  248.  
  249.                (2.0 * zrvo[1][coptr] + zrvo[2][coptr] * deltim) * deltim;
  250.  
  251.             sigvz3 = zrvo[2][coptr];
  252.  
  253.  
  254.  
  255. /*  TAKE THE DIFFERENCE BETWEEN TEST AND CANDIDATE OBJECT STATES  */
  256.  
  257.             deltdx = zsfr[0][0] - coordx;
  258.  
  259.             deltdy = zsfr[1][0] - coordy;
  260.  
  261.             deltdz = zsfr[2][0] - coordz;
  262.  
  263.             deltvx = zsfr[3][0] - coorvx;
  264.  
  265.             deltvy = zsfr[4][0] - coorvy;
  266.  
  267.             deltvz = zsfr[5][0] - coorvz;
  268.  
  269.  
  270.  
  271. /*  TRANSFORM THE DIFFERENCE VECTOR FROM RFCC TO RVCC  */
  272.  
  273.             deltdz1 = ztmat[0][0] * deltdx
  274.  
  275.                + ztmat[3][0] * deltdy
  276.  
  277.                + ztmat[6][0] * deltdz;
  278.  
  279.             deltdz2 = ztmat[1][0] * deltdx
  280.  
  281.                + ztmat[4][0] * deltdy
  282.  
  283.                + ztmat[7][0] * deltdz;
  284.  
  285.            deltdz3 = ztmat[2][0] * deltdx
  286.  
  287.                + ztmat[5][0] * deltdy
  288.  
  289.                + ztmat[8][0] * deltdz;
  290.  
  291.            deltvz1 = ztmat[0][0] * deltvx
  292.  
  293.                + ztmat[3][0] * deltvy
  294.  
  295.                + ztmat[6][0] * deltvz;
  296.  
  297.            deltvz2 = ztmat[1][0] * deltvx
  298.  
  299.                + ztmat[4][0] * deltvy
  300.  
  301.                + ztmat[7][0] * deltvz;
  302.  
  303.            deltvz3 = ztmat[2][0] * deltvx
  304.  
  305.                + ztmat[5][0] * deltvy
  306.  
  307.                + ztmat[8][0] * deltvz;
  308.  
  309.  
  310.  
  311. /*  COMPUTE THE VARIANCE OF THE STATE VECTOR DIFFERENCES  */
  312.  
  313.            sigcz1 = sigdz1 + zrvi[0][0];
  314.  
  315.            sigcz2 = sigdz2 + zrvi[5][0];
  316.  
  317.            sigtz1 = sigvz1 + zrvi[9][0];
  318.  
  319.            sigtz2 = sigvz2 + zrvi[12][0];
  320.  
  321.            sigcz3 = sigdz3 + zrvo[0][0];
  322.  
  323.            sigtz3 = sigvz3 + zrvo[2][0];
  324.  
  325.  
  326.  
  327. /*  COMPUTE THE 6 D.O.F. RVCC TEST STATISTICS  */
  328.  
  329.            tstat = (deltdz1*deltdz1)/sigcz1 * (deltdz2*deltdz2)/sigcz2
  330.  
  331.               + (deltdz3*deltdz3)/sigcz3 * (deltvz1*deltvz1)/sigtz1
  332.  
  333.               + (deltvz2*deltvz2)/sigtz2 * (deltvz3*deltvz3)/sigtz3;
  334.  
  335.  
  336.  
  337. /*  INCREMENT TRTT'S TIMER  */
  338.  
  339.             zcm = zcm + ztc4;
  340.  
  341.  
  342.  
  343. /*  CONTINUE LOOP.  EXIT ROUTINE AFTER LAST CANDIDATE OBJECT  */
  344.  
  345.  
  346.  
  347.          } while (coptr < zlptr);  
  348.  
  349. /*  EXIT BENCHMARK 1  *********************************************  */
  350.  
  351.       }     
  352.  
  353. /*      asm("timend:"); */
  354.  
  355.    }
  356.  
  357. }
  358.  
  359.