SHARE
TWEET

Untitled

a guest Nov 12th, 2019 107 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import java.awt.*;
  2. import java.io.*;
  3. import java.util.StringTokenizer;
  4.  
  5. public class Stdalone {
  6.  
  7.     static final double We = 7.292115E-5;            
  8.     static final double c = 299792458.0;            
  9.     static final double pi = 3.1415926535898;        
  10.     static final double Wedot = 7.2921151467E-5;    
  11.     static final double mu = 3.986005E+14;          
  12.     static final double F = -4.442807633E-10;        
  13.     static final double a = 6378137.0;              
  14.     static final double b = 6356752.31;              
  15.     static final double e1sqr = (a * a - b * b) / (a * a);
  16.     static final double e2sqr = (a * a - b * b) / (b * b);
  17.     static final double ITERATIONS =9;
  18.  
  19. public double[] LLA2XYZ(double[] Xi) {
  20.     double N = a / Math.sqrt(1.0 - e1sqr * Math.sin(Xi[0]) * Math.sin(Xi[0]));
  21.     double[] Xo = new double[3];
  22.     Xo[0] = (N + Xi[2]) * Math.cos(Xi[0]) * Math.cos(Xi[1]);
  23.     Xo[1] = (N + Xi[2]) * Math.cos(Xi[0]) * Math.sin(Xi[1]);
  24.     Xo[2] = (N * (1.0 - e1sqr) + Xi[2]) * Math.sin(Xi[0]);
  25.     System.out.println("jd LLA2XYZ = " +Xo[0] +" "+Xo[1]+" "+ Xo[2]+" R = " + Math.sqrt(Xo[0]*Xo[0]+Xo[1]*Xo[1]+Xo[2]*Xo[2]));
  26.     return Xo;
  27. }
  28.  
  29. public double[] XYZ2LLA(double[] Xi) {
  30.     double p = Math.sqrt(Xi[0] * Xi[0] + Xi[1] * Xi[1]);
  31.     double T = Math.atan((Xi[2] * a) / (p * b));
  32.     double sT = Math.sin(T);
  33.     double cT = Math.cos(T);
  34.     double[] Xo = new double[3];
  35.     Xo[0] = Math.atan((Xi[2] + e2sqr * b * sT * sT * sT) / (p - e1sqr * a * cT * cT * cT));
  36.     if ( Xi[0] == 0.0 )
  37.         Xo[1] = pi / 2.0 ;
  38.     else
  39.         Xo[1] = Math.atan(Xi[1] / Xi[0]);
  40.     double N = a / Math.sqrt(1.0 - e1sqr * Math.sin(Xo[0]) * Math.sin(Xo[0]));
  41.     Xo[2] = p / Math.cos(Xo[0]) - N;
  42.     return Xo;
  43. }
  44.  
  45.  
  46. public double[] satpos(double[] eph, double Ttr) {
  47.    
  48.     double Crs = eph[0];  
  49.     double dn  = eph[1] ;
  50.     double M0  = eph[2] ;
  51.     double Cuc = eph[3];  
  52.     double ec  = eph[4];  
  53.     double Cus = eph[5];  
  54.     double A   = eph[6] * eph[6];
  55.     double Toe = eph[7];  
  56.     double Cic = eph[8];  
  57.     double W0  = eph[9] ;
  58.     double Cis = eph[10];
  59.     double i0  = eph[11];
  60.     double Crc = eph[12];
  61.     double w   = eph[13];
  62.     double Wdot= eph[14];
  63.     double idot= eph[15];
  64.     double T;
  65.     T= Ttr - Toe;  
  66.     if ( T >  302400.0 ) T = T - 604800.0;
  67.     if ( T < -302400.0 ) T = T + 604800.0;
  68.  
  69.     double n0 = Math.sqrt(mu / (A * A * A));  
  70.     double n = n0 + dn;  
  71.  
  72.     double M = M0 + n * T;
  73.     double E = M;
  74.     System.out.println("jd  M mu MO T"+ " " +M + " " + mu + " " + M0 + " " + T);
  75.     double Eold;
  76.     do {
  77.         Eold = E;
  78.         E = M + ec * Math.sin(E);  
  79.     } while  ( Math.abs(E - Eold) >= 1.0e-8 );
  80.  
  81.    
  82.  
  83.     double snu = Math.sqrt(1 - ec * ec) * Math.sin(E) / (1 - ec * Math.cos(E));  
  84.     double cnu = (Math.cos(E) - ec) / (1 - ec * Math.cos(E));  
  85.     double nu;  
  86.     nu = Math.atan2(snu, cnu);  
  87.    
  88.     double phi = nu + w;  
  89.    
  90.     double du = Cuc * Math.cos(2 * phi) + Cus * Math.sin(2 * phi);  
  91.     double dr = Crc * Math.cos(2 * phi) + Crs * Math.sin(2 * phi);  
  92.     double di = Cic * Math.cos(2 * phi) + Cis * Math.sin(2 * phi);  
  93.  
  94.     double u = phi + du;  
  95.     double r = A * (1 - ec * Math.cos(E)) + dr;  
  96.     double i = i0 + idot * T + di;  
  97.    
  98.     double Xdash = r * Math.cos(u);
  99.     double Ydash = r * Math.sin(u);
  100.  
  101.     double Wc= W0 + (Wdot - Wedot) * T - Wedot * Toe;  
  102.  
  103.     double[] X = new double[3];    
  104.     X[0] = Xdash * Math.cos(Wc) - Ydash * Math.cos(i) * Math.sin(Wc);  //ECEF x
  105.     X[1] = Xdash * Math.sin(Wc) + Ydash * Math.cos(i) * Math.cos(Wc);  //ECEF y
  106.     X[2] = Ydash * Math.sin(i);  //ECEF z
  107.  
  108.  
  109.  
  110.     double Trel = F * ec * eph[6] * Math.sin(E); //rel
  111.  
  112.     return new double[] {X[0], X[1], X[2], Trel};
  113. }  
  114.  
  115. public double[] calcAzEl(double[] Xs, double[] Xu) {
  116.  
  117.     double x = Xu[0];
  118.     double y = Xu[1];
  119.     double z = Xu[2];
  120.     double p = Math.sqrt(x * x + y * y);
  121.     if ( p == 0.0 )
  122.         return null;
  123.  
  124.     double R = Math.sqrt(x * x + y * y + z * z);
  125.         //System.out.println("jd calcAzEl R = " + R);
  126.     double[][] e = new double[3][3];  
  127.     e[0][0] = - y / p;
  128.     e[0][1] = x / p;
  129.     e[0][2] = 0.0;
  130.     e[1][0] = - x * z / (p * R);
  131.     e[1][1] = - y * z / (p * R);
  132.     e[1][2] = p / R;
  133.     e[2][0] = x / R;
  134.     e[2][1] = y / R;
  135.     e[2][2] = z / R;
  136.  
  137.     double[] d = new double[3];  
  138.     for (int k = 0; k < 3; k++) {
  139.         d[k] = 0.0;
  140.         for (int i = 0; i < 3; i++)
  141.             d[k] += (Xs[i] - Xu[i]) * e[k][i];
  142.     }
  143.  
  144.     double s = d[2] / Math.sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]);
  145.  
  146.     double El;
  147.     if ( s == 1.0 )
  148.         El = 0.5 * pi;
  149.     else
  150.         El = Math.atan(s / Math.sqrt(1.0 - s * s));
  151.  
  152.     double Az;
  153.     if ( (d[1] == 0.0) && (d[0] > 0.0) )
  154.         Az = 0.5 * pi;
  155.     else if ((d[1] == 0.0) && (d[0] < 0.0) )
  156.         Az = 1.5 * pi;
  157.     else {
  158.         Az = Math.atan(d[0] / d[1]);
  159.         if ( d[1] < 0.0 )
  160.             Az += pi;
  161.         else if ( (d[1] > 0.0) && (d[0] < 0.0) )
  162.             Az += 2.0 * pi;
  163.     }
  164.  
  165.     return new double[] {Az, El};
  166. }
  167.  
  168.  
  169. public double ionocorr (double[] ion, double Latu, double Lonu, double Az, double El, double Ttr) {
  170.  
  171.     double a0 = ion[0];
  172.     double a1 = ion[1];
  173.     double a2 = ion[2];
  174.     double a3 = ion[3];
  175.     double b0 = ion[4];
  176.     double b1 = ion[5];
  177.     double b2 = ion[6];
  178.     double b3 = ion[7];
  179.  
  180.  
  181.     Latu = Latu / pi;
  182.     Lonu = Lonu / pi;
  183.     El = El / pi;
  184.  
  185.    
  186.     double phi = 0.0137 / (El + 0.11) - 0.022;
  187.    
  188.     double Lati = Latu + phi * Math.cos (Az);
  189.     if ( Lati > 0.416 )
  190.         Lati = 0.416;
  191.     else if ( Lati < -0.416 )
  192.         Lati = -0.416;
  193.        
  194.     double Loni = Lonu + phi * Math.sin(Az) / Math.cos(Lati * pi);
  195.     double Latm = Lati + 0.064 * Math.cos((Loni - 1.617) * pi);
  196.    
  197.     double T = 4.32E+4 * Loni + Ttr;
  198.     if (T >= 86400.0 )
  199.         T -= 86400.0;
  200.     else if (T < 0 )
  201.         T += 86400.0;
  202.        
  203.     double F = 1.0 + 16.0 * (0.53 - El) * (0.53 - El) * (0.53 - El);
  204.    
  205.     double per = b0 + b1 * Latm + b2 * Latm * Latm + b3 * Latm * Latm * Latm;
  206.     if (per < 72000.0 )
  207.         per = 72000.0;
  208.        
  209.     double x = 2 * pi * (T - 50400.0) / per;
  210.    
  211.     double amp = a0 + a1 * Latm + a2 * Latm * Latm + a3 * Latm * Latm * Latm;
  212.     if (amp < 0.0 )
  213.         amp = 0.0;
  214.        
  215.     double dTiono;
  216.     if (Math.abs(x) >= 1.57 )
  217.         dTiono = F * 5.0E-9;
  218.     else
  219.         dTiono = F * (5.0E-9 + amp * (1.0 - x * x / 2.0 + x * x * x * x / 24.0));
  220.  
  221.     return dTiono;
  222. }
  223.  
  224. public double sub (double[][] A, int r, int c) {
  225.  
  226.     double[][] B = new double[3][3];  
  227.     int i1, j1;
  228.  
  229.     for (int i = 0; i < 3; i++) {
  230.         i1 = i;
  231.         if ( i >= r )
  232.             i1++;
  233.         for (int j = 0; j < 3; j++) {
  234.             j1 = j;
  235.             if ( j >= c )
  236.                 j1++;
  237.             B[i][j] = A[i1][j1];
  238.         }
  239.     }
  240.        
  241.     double subdet = B[0][0] * (B[1][1] * B[2][2] - B[1][2] * B[2][1])
  242.                     - B[1][0] * (B[0][1] * B[2][2] - B[2][1] * B[0][2])
  243.                     + B[2][0] * (B[0][1] * B[1][2] - B[0][2] * B[1][1]);
  244.        
  245.     return subdet;
  246. }
  247.  
  248.  
  249. public double[] solve(double[][] Xs, boolean[] SV,  double[] P, double[] Xr) {
  250.  
  251.     double[] R = new double[33];
  252.     double[] L = new double[33];
  253.     double [][] A = new double[33][4];  
  254.     double[] AL = new double[4];
  255.     double [][] AA = new double[4][4];
  256.     double [][] AAi = new double[4][4];  
  257.     double det;
  258.     double[] D = new double[5];
  259.    
  260.  
  261.     int it = 0;
  262.     System.out.println("solve");   
  263.     do {
  264.         it++;
  265.  
  266.         for (int prn = 1; prn <= 32; prn++)
  267.             if ( SV[prn] ) {
  268.                 System.out.println("jd1 X Y Z P "+ prn +" "+Xs[prn][0]+" "+ Xs[prn][1]+" "+ Xs[prn][2]+" "+P[prn]);
  269.            
  270.                 R[prn] =  Math.sqrt((Xr[0] - Xs[prn][0]) * (Xr[0] - Xs[prn][0])
  271.                                 + (Xr[1] - Xs[prn][1]) * (Xr[1] - Xs[prn][1])
  272.                                 + (Xr[2] - Xs[prn][2]) * (Xr[2] - Xs[prn][2]));
  273.            
  274.                 L[prn] = P[prn] - R[prn];
  275.            
  276.                 for (int k = 0; k < 3; k++)
  277.                     A[prn][k] = (Xr[k] - Xs[prn][k]) / R[prn];
  278.                 A[prn][3] = -1.0;
  279.             }
  280.  
  281.            
  282.             for (int k = 0; k <= 3; k++) {
  283.                 AL[k] = 0.0;
  284.                 for (int prn = 1; prn <= 32; prn++)
  285.                     if ( SV[prn] )
  286.                         AL[k] += A[prn][k] * L[prn];
  287.             }
  288.  
  289.            
  290.             for (int k = 0; k <= 3; k++)
  291.                 for (int i = 0; i <= 3; i++) {
  292.                     AA[k][i] =0.0;
  293.                     for (int prn = 1; prn <= 32; prn++)
  294.                         if ( SV[prn] )
  295.                             AA[k][i] += A[prn][k] * A[prn][i];
  296.                 }
  297.  
  298.             det = AA[0][0] * sub(AA,0,0) - AA[1][0] * sub(AA,1,0)
  299.                     + AA[2][0] * sub(AA,2,0) - AA[3][0] * sub(AA,3,0);
  300.             if ( det == 0.0 )
  301.                 return null;
  302.  
  303.             int j;
  304.             int n;
  305.             for (int k = 0; k <= 3; k++)
  306.                 for (int i = 0; i <= 3; i++) {
  307.                     n = k + i;
  308.                     if ( n % 2 != 0 )
  309.                         j = -1;
  310.                     else
  311.                         j = 1;
  312.                     AAi[k][i] = j * sub(AA,i,k) / det;
  313.                 }
  314.  
  315.             for (int k = 0; k <= 3; k++) {
  316.                 D[k] = 0.0;
  317.                 for (int i = 0; i <= 3; i++)
  318.                     D[k] += AAi[k][i] * AL[i];
  319.             }
  320.  
  321.            
  322.             for (int k = 0; k < 3; k++)
  323.                 Xr[k] += D[k];
  324.  
  325.     } while ( (it < ITERATIONS)
  326.         && ((Math.abs(D[0]) + Math.abs(D[1]) + Math.abs(D[2])) >= 1.0E-2) );  
  327.  
  328.     double Cr = D[3];
  329.  
  330.     if ( it >= ITERATIONS ) {  
  331.         System.out.println("rozw it = " + it);
  332.         return null;
  333.     }
  334.  
  335.     return new double[] {Xr[0], Xr[1], Xr[2], Cr};
  336. }
  337.  
  338.  
  339. public void main() {
  340.  
  341.     double Trc = 0;
  342.     double Cr;
  343.     double[] Xlla = new double[3];
  344.     double[] Xr = new double[3];
  345.     boolean[] SV = new boolean[33];
  346.     double[][] Xs = new double[33][3];
  347.     double[][] eph = new double[33][16];
  348.     double[][] clk = new double[33][5];
  349.     double[] ion = new double[8];
  350.     double[] Praw = new double[33];
  351.     double[] Pcor = new double[33];
  352.  
  353.  
  354.     FileDialog d = new FileDialog(new Frame(), "Wybierz plik", FileDialog.LOAD);
  355.     d.setFile("*");
  356.     d.show();
  357.     String file1 = d.getFile();
  358.     if (file1 != null)
  359.         file1 = d.getDirectory() + d.getFile();  
  360.     else return;
  361.  
  362.     BufferedReader br = null;
  363.     try {
  364.         br = new BufferedReader(new InputStreamReader(new FileInputStream(file1)));  
  365.     }
  366.     catch (IOException e) {
  367.         System.out.println ("IOException = " + e);
  368.         return;
  369.     }
  370.     catch (Throwable e) {
  371.         System.err.println(e);
  372.         return;
  373.     }
  374.  
  375.     try {
  376.         String line;
  377.         if ((line = br.readLine()) != null)
  378.             System.out.println("komenda : " + line);
  379.         if ((line = br.readLine()) != null)
  380.             Trc = Double.valueOf(line.trim()).doubleValue();
  381.  
  382.         if ((line = br.readLine()) != null)
  383.             System.out.println("komenda : " + line);
  384.         for (int i = 0; i < 8; i++)
  385.             if ((line = br.readLine()) != null)
  386.                 ion[i] = Double.valueOf(line.trim()).doubleValue();
  387.  
  388.         if ((line = br.readLine()) != null)
  389.             System.out.println("komenda : " + line);
  390.         StringTokenizer st;
  391.         for (int prn = 1; prn < 32; prn++)
  392.             SV[prn] = false;
  393.         int prn1 = 0;
  394.         do {
  395.             if ((line = br.readLine()) != null) {
  396.                 st = new StringTokenizer(line, " \t");
  397.                 prn1 = Integer.parseInt(st.nextToken());
  398.                
  399.                 if ( prn1 != 0 ) {
  400.                     Praw[prn1] = Double.valueOf(st.nextToken()).doubleValue();
  401.                     SV[prn1] = true;
  402.                 System.out.println("jd prn1 Praw " + prn1 +" "+Praw[prn1]);        
  403.                 }
  404.             }
  405.             else line = br.readLine();
  406.         } while (prn1 != 0);  
  407.  
  408.         if ((line = br.readLine()) != null)
  409.             System.out.println("komenda: " + line);
  410.         while ((line = br.readLine()) != null) {
  411.             prn1 = Integer.parseInt(line.trim());
  412.             for (int i = 0; i < 16; i++) {
  413.                 line = br.readLine();
  414.                 eph[prn1][i] = Double.valueOf(line.trim()).doubleValue();
  415.             System.out.println("jd " + eph[prn1][i]);
  416.             }
  417.             for (int i = 0; i <= 4; i++) {
  418.                 line = br.readLine();
  419.                 clk[prn1][i] = Double.valueOf(line.trim()).doubleValue();
  420.             }
  421.         }
  422.         br.close();
  423.     }
  424.     catch (IOException e) {
  425.         System.out.println ("IOException = " + e);
  426.     }
  427.        
  428.  
  429.     System.out.println("Start ");
  430.  
  431.     System.out.println("       54 18 0");
  432.     Xlla[0] = 54.373810539 * pi / 180.0;  
  433.     Xlla[1] = 18.614489984 * pi / 180.0;
  434.     Xlla[2] = 0;
  435.     Xr = LLA2XYZ(Xlla);
  436.    
  437.     Cr = 0;
  438.     for (int prn = 1; prn <= 32; prn++)
  439.         Pcor[prn] = 0.075 * c;  
  440.  
  441.     for (int pass = 1; pass <= 2; pass++) {
  442.         System.out.println();
  443.         System.out.println("-------------------------- PASS " + pass + " -------------------------");
  444.  
  445.         for (int prn = 1; prn <= 32; prn++)
  446.             if ( SV[prn] ) {
  447.  
  448.  
  449.                 double tau = (Pcor[prn] + Cr) / c;  
  450.                 double Ttr = Trc - tau;
  451.                
  452.  
  453.                 double[] tmp4 = satpos(eph[prn], Ttr);  
  454.                
  455.                 double alpha = tau * We;
  456.                 Xs[prn][0] = tmp4[0] * Math.cos(alpha) + tmp4[1] * Math.sin(alpha);
  457.                 Xs[prn][1] = - tmp4[0] * Math.sin(alpha) + tmp4[1] * Math.cos(alpha);
  458.                 Xs[prn][2] = tmp4[2];
  459.                 double Trel = tmp4[3];
  460.                 System.out.println("jd SV     : " + prn + " "+ Xs[prn][0] + " " + Xs[prn][1] + " " + Xs[prn][2]);  
  461.  
  462.                 double[] tmp3 = new double[3];
  463.                 for (int i = 0; i < 3; i++)
  464.                     tmp3[i] = Xs[prn][i];
  465.                 double[] tmp2 = calcAzEl(tmp3, Xr);
  466.                 double Az, El;
  467.                 if (tmp2 == null) {
  468.                     System.out.println("B;lad in calcAzEl - ");
  469.                     return;
  470.                 }
  471.                 else {
  472.                     Az = tmp2[0];
  473.                     El = tmp2[1];
  474.                 }
  475.                 System.out.println("Az, El : " + prn + " " + (Az * 180.0 / pi) + " " + (El * 180.0 / pi) );
  476.                
  477.                 double dTclck = - clk[prn][0] + clk[prn][2] + clk[prn][3] * (Ttr - clk[prn][1])
  478.                     + clk[prn][4] * (Ttr - clk[prn][1]) * (Ttr - clk[prn][1])
  479.                     + Trel ;
  480.                 System.out.println("dTclck= clk[0]) + clk[2]+ clk[3]*(Ttr - clk[1])+  clk[4] Trel:"
  481.                 + " " +dTclck +"="+ (- clk[prn][0])+ " " +clk[prn][2]+ " " + clk[prn][3]+ " " + (Ttr - clk[prn][1]) + " "
  482.                 + clk[prn][4] + " " +  Trel);              
  483.                
  484.                 double dTiono = ionocorr(ion, Xlla[0], Xlla[1], Az, El, Ttr);
  485.  
  486.                 double dRtrop = 2.312 / Math.sin(Math.sqrt(El * El + 1.904E-3))
  487.                     + 0.084 / Math.sin(Math.sqrt(El * El + 0.6854E-3));
  488.  
  489.                 System.out.println("Corr   : " + prn + " " + Praw[prn]+ " " + (dTclck * c) + " " + (dTiono * c) + " " + dRtrop);  
  490.  
  491.                 Pcor[prn] = Praw[prn] + dTclck * c - dTiono * c - dRtrop + Cr;  
  492.  
  493.             }  
  494.  
  495.         double[] tmp4 = solve(Xs, SV, Pcor, Xr);  
  496.         if (tmp4 == null) {
  497.             System.out.println("Esprawdz");
  498.             return;
  499.         }
  500.         else {
  501.             Xr[0] = tmp4[0];
  502.             Xr[1] = tmp4[1];
  503.             Xr[2] = tmp4[2];
  504.             Cr = tmp4[3];
  505.         }
  506.         System.out.println();
  507.         System.out.println("Pos XYZ: " + Xr[0] + " " + Xr[1] + " " + Xr[2] + " " + Cr);  
  508.         Xlla = XYZ2LLA(Xr);
  509.         System.out.println("Pos LLA: " + (Xlla[0] * 180.0 / pi) + " " + (Xlla[1] * 180.0 / pi) + " " + Xlla[2]);  
  510.     }
  511.  
  512. }
  513.  
  514. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top