SHARE
TWEET

heheh

a guest Nov 17th, 2019 68 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.text.ParseException;
  4. import java.text.SimpleDateFormat;
  5. import java.util.*;
  6.  
  7.  
  8. /**
  9.  *
  10.  */
  11. public class Stdalone {
  12.  
  13.     static final double We = 7.292115E-5;            //WGS-84 predkosc obrotow Ziemi[rad/sec]
  14.     static final double c = 299792458.0;             //WGS-84 predkosc swiatla
  15.     static final double pi = 3.1415926535898;        //WGS 84 pi;
  16.     static final double Wedot = 7.2921151467E-5;     //WGS 84 predkosc obrotow Ziemi [rad/sec] (dokladna)
  17.     static final double mu = 3.986005E+14;           //WGS 84 stala grawitacji [m^3/sec^2]
  18.     static final double F = -4.442807633E-10;        //korekta relatywistyczna
  19.     static final double a = 6378137.0;               //WGS-84 dluzsza polos
  20.     static final double b = 6356752.31;              //WGS-84 krotsza polos Ziemi
  21.     static final double e1sqr = (a * a - b * b) / (a * a); //mimosrod
  22.     static final double e2sqr = (a * a - b * b) / (b * b); //mimosrod
  23.  
  24.  
  25.     /**
  26.      * ************************************************************************
  27.      */
  28.     public double[] LLA2XYZ(double[] Xi) {
  29.         double N = a / Math.sqrt(1.0 - e1sqr * Math.sin(Xi[0]) * Math.sin(Xi[0]));
  30.         double[] Xo = new double[3];
  31.         Xo[0] = (N + Xi[2]) * Math.cos(Xi[0]) * Math.cos(Xi[1]);
  32.         Xo[1] = (N + Xi[2]) * Math.cos(Xi[0]) * Math.sin(Xi[1]);
  33.         Xo[2] = (N * (1.0 - e1sqr) + Xi[2]) * Math.sin(Xi[0]);
  34.         return Xo;
  35.     }
  36.  
  37.  
  38.     /**
  39.      * ************************************************************************
  40.      */
  41.     public double[] XYZ2LLA(double[] Xi) {
  42.         double p = Math.sqrt(Xi[0] * Xi[0] + Xi[1] * Xi[1]);
  43.         double T = Math.atan((Xi[2] * a) / (p * b));
  44.         double sT = Math.sin(T);
  45.         double cT = Math.cos(T);
  46.         double[] Xo = new double[3];
  47.         Xo[0] = Math.atan((Xi[2] + e2sqr * b * sT * sT * sT) / (p - e1sqr * a * cT * cT * cT));
  48.         if (Xi[0] == 0.0)
  49.             Xo[1] = pi / 2.0;
  50.         else
  51.             Xo[1] = Math.atan(Xi[1] / Xi[0]);
  52.         double N = a / Math.sqrt(1.0 - e1sqr * Math.sin(Xo[0]) * Math.sin(Xo[0]));
  53.         Xo[2] = p / Math.cos(Xo[0]) - N;
  54.         return Xo;
  55.     }
  56.  
  57.  
  58.     /**
  59.      * ************************************************************************
  60.      */
  61.  
  62.     public double[] satpos_(double[] eph, double Ttr) { //dla pliku input.txt orginalnego
  63.  
  64.         double Crs = eph[0];
  65.         double dn = eph[1];
  66.         double M0 = eph[2];
  67.         double Cuc = eph[3];
  68.         double ec = eph[4];
  69.         double Cus = eph[5];
  70.         double A = eph[6] * eph[6];
  71.         double Toe = eph[7];
  72.         double Cic = eph[8];
  73.         double W0 = eph[9];
  74.         double Cis = eph[10];
  75.         double i0 = eph[11];
  76.         double Crc = eph[12];
  77.         double w = eph[13];
  78.         double Wdot = eph[14];
  79.         double idot = eph[15];
  80.         //System.out.println("jd"+" "+ Crs+" "+ dn+" "+ M0+" "+ Cuc+" "+ ec+" "+ Cus+" "+ A +" "+ Toe+" "+  Cic+" "+ W0+" "+ Cis+" "+ i0+" "+  Crc+" "+ w+" "+ Wdot+" "+ idot);
  81.         double T = Ttr - Toe;
  82.  
  83.  
  84.         if (T > 302400.0) T = T - 604800.0;
  85.         if (T < -302400.0) T = T + 604800.0;
  86.  
  87.         double n0 = Math.sqrt(mu / (A * A * A));
  88.         double n = n0 + dn;
  89.  
  90.         double M = M0 + n * T;
  91.         double E = M;
  92.         System.out.println("jd " + " " + M + " " + mu + " " + M0 + " " + T);
  93.         double Eold;
  94.         do {
  95.             Eold = E;
  96.             E = M + ec * Math.sin(E);
  97.         } while (Math.abs(E - Eold) >= 1.0e-8);
  98.  
  99.  
  100.         double snu = Math.sqrt(1 - ec * ec) * Math.sin(E) / (1 - ec * Math.cos(E));
  101.         double cnu = (Math.cos(E) - ec) / (1 - ec * Math.cos(E));
  102.         double nu;
  103.         nu = Math.atan2(snu, cnu);
  104.  
  105.         double phi = nu + w;
  106.  
  107.         double du = Cuc * Math.cos(2 * phi) + Cus * Math.sin(2 * phi);
  108.         double dr = Crc * Math.cos(2 * phi) + Crs * Math.sin(2 * phi);
  109.         double di = Cic * Math.cos(2 * phi) + Cis * Math.sin(2 * phi);
  110.  
  111.         double u = phi + du;
  112.         double r = A * (1 - ec * Math.cos(E)) + dr;
  113.         double i = i0 + idot * T + di;
  114.  
  115.         double Xdash = r * Math.cos(u);
  116.         double Ydash = r * Math.sin(u);
  117.  
  118.         double Wc = W0 + (Wdot - Wedot) * T - Wedot * Toe;
  119.  
  120.         double[] X = new double[3];
  121.         X[0] = Xdash * Math.cos(Wc) - Ydash * Math.cos(i) * Math.sin(Wc);  //ECEF x
  122.         X[1] = Xdash * Math.sin(Wc) + Ydash * Math.cos(i) * Math.cos(Wc);  //ECEF y
  123.         X[2] = Ydash * Math.sin(i);  //ECEF z
  124.  
  125.  
  126.         double Trel = F * ec * eph[6] * Math.sin(E);
  127.  
  128.         return new double[]{X[0], X[1], X[2], Trel};
  129.     }
  130.  
  131.     public double[] satpos(double[] eph, double Ttr) { //dla plikow obs i nav
  132.  
  133.         //Prawa keplera
  134.         double Crs = eph[0];
  135.         double dn = eph[1];
  136.         double M0 = eph[2];
  137.         double Cuc = eph[3];
  138.         double ec = eph[4];
  139.         double Cus = eph[5];
  140.         double A = eph[6] * eph[6];
  141.         double Toe = eph[7];
  142.         double Cic = eph[8];
  143.         double W0 = eph[9];
  144.         double Cis = eph[10];
  145.         double i0 = eph[11];
  146.         double Crc = eph[12];
  147.         double w = eph[13];
  148.         double Wdot = eph[14];
  149.         double idot = eph[15];
  150.  
  151.         double T;
  152.  
  153.         T = Ttr - Toe;
  154.         if (T > 302400.0) T = T - 604800.0; //koniec tygodnia
  155.         if (T < -302400.0) T = T + 604800.0; //poczatek tygodnia
  156.  
  157.         double n0 = Math.sqrt(mu / (A * A * A));  //ruch sredni
  158.         double n = n0 + dn;  //korekta
  159.  
  160.         double M = M0 + n * T;  //srednia anomalia
  161.         double E = M; //E
  162.         double Eold;
  163.         do {
  164.             Eold = E;
  165.             E = M + ec * Math.sin(E);  //
  166.         } while (Math.abs(E - Eold) >= 1.0e-8);
  167.  
  168.         double snu = Math.sqrt(1 - ec * ec) * Math.sin(E) / (1 - ec * Math.cos(E));   //
  169.         double cnu = (Math.cos(E) - ec) / (1 - ec * Math.cos(E));   //
  170.         double nu;  //
  171.         nu = Math.atan2(snu, cnu);   //
  172.  
  173.         double phi = nu + w;  //
  174.  
  175.         double du = Cuc * Math.cos(2 * phi) + Cus * Math.sin(2 * phi);  //
  176.         double dr = Crc * Math.cos(2 * phi) + Crs * Math.sin(2 * phi);  //
  177.         double di = Cic * Math.cos(2 * phi) + Cis * Math.sin(2 * phi);  //
  178.  
  179.         double u = phi + du;  //
  180.         double r = A * (1 - ec * Math.cos(E)) + dr;  //
  181.         double i = i0 + idot * T + di;  //inklinacja
  182.         double Xdash = r * Math.cos(u);  //
  183.         double Ydash = r * Math.sin(u);  //
  184.  
  185.         double Wc = W0 + (Wdot - Wedot) * T - Wedot * Toe;  //wezel wspeptujacy
  186.  
  187.         double[] X = new double[3];
  188.         X[0] = Xdash * Math.cos(Wc) - Ydash * Math.cos(i) * Math.sin(Wc);  //ECEF x
  189.         X[1] = Xdash * Math.sin(Wc) + Ydash * Math.cos(i) * Math.cos(Wc);  //ECEF y
  190.         X[2] = Ydash * Math.sin(i);  //ECEF z
  191.  
  192.         double Trel = F * ec * eph[6] * Math.sin(E); //korekta relatiwystyczna
  193.  
  194.         return new double[]{X[0], X[1], X[2], Trel};
  195.     }
  196.  
  197.  
  198.     /**
  199.      * ************************************************************************
  200.      */
  201.     public double[] calcAzEl(double[] Xs, double[] Xu) {
  202.  
  203.         double x = Xu[0];
  204.         double y = Xu[1];
  205.         double z = Xu[2];
  206.         double p = Math.sqrt(x * x + y * y);
  207.         if (p == 0.0)
  208.             return null;
  209.  
  210.         double R = Math.sqrt(x * x + y * y + z * z);
  211.  
  212.         double[][] e = new double[3][3];
  213.         e[0][0] = -y / p;
  214.         e[0][1] = x / p;
  215.         e[0][2] = 0.0;
  216.         e[1][0] = -x * z / (p * R);
  217.         e[1][1] = -y * z / (p * R);
  218.         e[1][2] = p / R;
  219.         e[2][0] = x / R;
  220.         e[2][1] = y / R;
  221.         e[2][2] = z / R;
  222.  
  223.         double[] d = new double[3];
  224.         for (int k = 0; k < 3; k++) {
  225.             d[k] = 0.0;
  226.             for (int i = 0; i < 3; i++)
  227.                 d[k] += (Xs[i] - Xu[i]) * e[k][i];
  228.         }
  229.  
  230.         double s = d[2] / Math.sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]);
  231.  
  232.         double El;
  233.         if (s == 1.0)
  234.             El = 0.5 * pi;
  235.         else
  236.             El = Math.atan(s / Math.sqrt(1.0 - s * s));
  237.  
  238.         double Az;
  239.         if ((d[1] == 0.0) && (d[0] > 0.0))
  240.             Az = 0.5 * pi;
  241.         else if ((d[1] == 0.0) && (d[0] < 0.0))
  242.             Az = 1.5 * pi;
  243.         else {
  244.             Az = Math.atan(d[0] / d[1]);
  245.             if (d[1] < 0.0)
  246.                 Az += pi;
  247.             else if ((d[1] > 0.0) && (d[0] < 0.0))
  248.                 Az += 2.0 * pi;
  249.         }
  250.  
  251.         return new double[]{Az, El};
  252.     }
  253.  
  254.  
  255.     /**
  256.      * ************************************************************************
  257.      * korekty jonosferyczne  *
  258.      */
  259.     public double ionocorr(double[] ion, double Latu, double Lonu, double Az, double El, double Ttr) {
  260.  
  261.         double a0 = ion[0];
  262.         double a1 = ion[1];
  263.         double a2 = ion[2];
  264.         double a3 = ion[3];
  265.         double b0 = ion[4];
  266.         double b1 = ion[5];
  267.         double b2 = ion[6];
  268.         double b3 = ion[7];
  269.  
  270.         Latu = Latu / pi;
  271.         Lonu = Lonu / pi;
  272.         El = El / pi;
  273.  
  274.  
  275.         double phi = 0.0137 / (El + 0.11) - 0.022;
  276.  
  277.         double Lati = Latu + phi * Math.cos(Az);
  278.         if (Lati > 0.416)
  279.             Lati = 0.416;
  280.         else if (Lati < -0.416)
  281.             Lati = -0.416;
  282.  
  283.         double Loni = Lonu + phi * Math.sin(Az) / Math.cos(Lati * pi);
  284.         double Latm = Lati + 0.064 * Math.cos((Loni - 1.617) * pi);
  285.  
  286.         double T = 4.32E+4 * Loni + Ttr;
  287.         if (T >= 86400.0)
  288.             T -= 86400.0;
  289.         else if (T < 0)
  290.             T += 86400.0;
  291.  
  292.         double F = 1.0 + 16.0 * (0.53 - El) * (0.53 - El) * (0.53 - El);
  293.  
  294.         double per = b0 + b1 * Latm + b2 * Latm * Latm + b3 * Latm * Latm * Latm;
  295.         if (per < 72000.0)
  296.             per = 72000.0;
  297.  
  298.         double x = 2 * pi * (T - 50400.0) / per;
  299.  
  300.         double amp = a0 + a1 * Latm + a2 * Latm * Latm + a3 * Latm * Latm * Latm;
  301.         if (amp < 0.0)
  302.             amp = 0.0;
  303.  
  304.         double dTiono;
  305.         if (Math.abs(x) >= 1.57)
  306.             dTiono = F * 5.0E-9;
  307.         else
  308.             dTiono = F * (5.0E-9 + amp * (1.0 - x * x / 2.0 + x * x * x * x / 24.0));
  309.  
  310.         return dTiono;
  311.     }
  312.  
  313.  
  314.     /**
  315.      * ************************************************************************
  316.      * wyznacznik macierzy
  317.      */
  318.     public double sub(double[][] A, int r, int c) {
  319.  
  320.         double[][] B = new double[3][3];
  321.         int i1, j1;
  322.  
  323.         for (int i = 0; i < 3; i++) {
  324.             i1 = i;
  325.             if (i >= r)
  326.                 i1++;
  327.             for (int j = 0; j < 3; j++) {
  328.                 j1 = j;
  329.                 if (j >= c)
  330.                     j1++;
  331.                 B[i][j] = A[i1][j1];
  332.             }
  333.         }
  334.  
  335.         double subdet = B[0][0] * (B[1][1] * B[2][2] - B[1][2] * B[2][1])
  336.                 - B[1][0] * (B[0][1] * B[2][2] - B[2][1] * B[0][2])
  337.                 + B[2][0] * (B[0][1] * B[1][2] - B[0][2] * B[1][1]);
  338.  
  339.         return subdet;
  340.     }
  341.  
  342.  
  343.     /**
  344.      * ************************************************************************
  345.      * szukaj rozwiazania iteracyjnie
  346.      */
  347.     public double[] solve(double[][] Xs, boolean[] SV, double[] P, double[] Xr) {
  348.  
  349.         double[] R = new double[33];
  350.         double[] L = new double[33];
  351.         double[][] A = new double[33][4];
  352.         double[] AL = new double[4];
  353.         double[][] AA = new double[4][4];
  354.         double[][] AAi = new double[4][4];
  355.         double det;
  356.         double[] D = new double[5];
  357.  
  358.         int it = 0; //
  359.         do {
  360.             it++;
  361.  
  362.             for (int prn = 1; prn <= 32; prn++)
  363.                 if (SV[prn]) {
  364.                     //odleglosc odbiornika i satelity
  365.                     R[prn] = Math.sqrt((Xr[0] - Xs[prn][0]) * (Xr[0] - Xs[prn][0])
  366.                             + (Xr[1] - Xs[prn][1]) * (Xr[1] - Xs[prn][1])
  367.                             + (Xr[2] - Xs[prn][2]) * (Xr[2] - Xs[prn][2]));
  368.                     //residuum
  369.                     L[prn] = P[prn] - R[prn];
  370.                     //
  371.                     for (int k = 0; k < 3; k++)
  372.                         A[prn][k] = (Xr[k] - Xs[prn][k]) / R[prn];
  373.                     A[prn][3] = -1.0;
  374.                 }
  375.  
  376.             //praca na macierzach
  377.             for (int k = 0; k <= 3; k++) {
  378.                 AL[k] = 0.0;
  379.                 for (int prn = 1; prn <= 32; prn++)
  380.                     if (SV[prn])
  381.                         AL[k] += A[prn][k] * L[prn];
  382.             }
  383.  
  384.             //
  385.             for (int k = 0; k <= 3; k++)
  386.                 for (int i = 0; i <= 3; i++) {
  387.                     AA[k][i] = 0.0;
  388.                     for (int prn = 1; prn <= 32; prn++)
  389.                         if (SV[prn])
  390.                             AA[k][i] += A[prn][k] * A[prn][i];
  391.                 }
  392.  
  393.             //dowrotnosc macierzy
  394.             //
  395.             det = AA[0][0] * sub(AA, 0, 0) - AA[1][0] * sub(AA, 1, 0)
  396.                     + AA[2][0] * sub(AA, 2, 0) - AA[3][0] * sub(AA, 3, 0);
  397.             if (det == 0.0)
  398.                 return null;
  399.  
  400.             int j;
  401.             int n;
  402.             for (int k = 0; k <= 3; k++)
  403.                 for (int i = 0; i <= 3; i++) {
  404.                     n = k + i;
  405.                     if (n % 2 != 0)   //
  406.                         j = -1;
  407.                     else
  408.                         j = 1;
  409.                     AAi[k][i] = j * sub(AA, i, k) / det;
  410.                 }
  411.  
  412.             //mnozenie z odwrtonoscia macierzy
  413.             for (int k = 0; k <= 3; k++) {
  414.                 D[k] = 0.0;
  415.                 for (int i = 0; i <= 3; i++)
  416.                     D[k] += AAi[k][i] * AL[i];
  417.             }
  418.  
  419.             //uaktualnij pozycje
  420.             for (int k = 0; k < 3; k++)
  421.                 Xr[k] += D[k];
  422.  
  423.         } while ((it < 9)    //maksimu 6 iteracji w innym wypadku jest cos nie tak
  424.                 && ((Math.abs(D[0]) + Math.abs(D[1]) + Math.abs(D[2])) >= 1.0E-2));  //kryterium
  425.  
  426.         double Cr = D[3]; //blad zegara
  427.  
  428.         if (it >= 9) {
  429.             System.out.println("rozwiazanie,  przekroczony limit.  it = " + it);
  430.             return null;
  431.         }
  432.  
  433.         return new double[]{Xr[0], Xr[1], Xr[2], Cr};
  434.     }
  435.  
  436.  
  437.     /**
  438.      * ************************************************************************
  439.      */
  440.     public void main() {
  441.  
  442.         double Trc = 0;  //GPS czas odbioru
  443.         double Cr; //korekta zegara
  444.         double[] Xlla = new double[3];  //[rad,rad,m]
  445.         double[] Xr = new double[3]; //pozycja odbiornika, ECEF
  446.         boolean[] SV = new boolean[33]; //ktore satelity sa wykorzystywane
  447.         double[][] Xs = new double[33][3];  //pozycja satelitow
  448.         double[][] eph = new double[33][16];   //efemerydy
  449.         double[][] clk = new double[33][5];  //dane dot. zegara
  450.         double[] ion = new double[8]; //poprawiki jonosferyczne
  451.         double[] Praw = new double[33]; //pseudoodleglosci
  452.         double[] Pcor = new double[33]; //skorygowane pseudoodleglosci
  453.  
  454.  
  455.         FileDialog d = new FileDialog(new Frame(), "Wybierz plik N", FileDialog.LOAD);
  456.         d.setFile("*");
  457.         d.show();
  458.         String file1 = d.getFile();
  459.         if (file1 != null)
  460.             file1 = d.getDirectory() + d.getFile();
  461.         else return;
  462.  
  463.         BufferedReader br = null;
  464.         try {
  465.             br = new BufferedReader(new InputStreamReader(new FileInputStream(file1)));
  466.         } catch (IOException e) {
  467.             System.out.println("IOException = " + e);
  468.             return;
  469.         } catch (Throwable e) {
  470.             System.err.println(e);
  471.             return;
  472.         }
  473.  
  474.         try {
  475.             String line;
  476.             for (int i = 0; i < 3; i++) {
  477.                 if ((line = br.readLine()) != null) ;
  478.             }
  479.  
  480.             //iono
  481.             if ((line = br.readLine()) != null) {
  482.                 for (int i = 0; i < 4; i++) {
  483.                     ion[i] = Double.parseDouble(line.substring(2 + (i * 12), 2 + ((i + 1) * 12)));
  484.                 }
  485.             }
  486.             if ((line = br.readLine()) != null) {
  487.                 for (int i = 0; i < 4; i++) {
  488.                     ion[i + 4] = Double.parseDouble(line.substring(2 + (i * 12), 2 + ((i + 1) * 12)));
  489.                 }
  490.             }
  491.  
  492.             do {
  493.                 if ((line = br.readLine()) != null) ;
  494.             } while (line != null && !line.contains("END OF HEADER"));
  495.  
  496.             //efemerydy
  497.             StringTokenizer st;
  498.             for (int prn = 1; prn < 32; prn++)
  499.                 SV[prn] = false;
  500.             int prn1 = 0;
  501.             do {
  502.                 if ((line = br.readLine()) != null) {
  503.                     st = new StringTokenizer(line);
  504.                     prn1 = Integer.parseInt(st.nextToken());
  505.                     for (int i = 0; i < 6; i++)
  506.                         st.nextToken();
  507.                     clk[prn1][0] = 4.65661287307739E-0010;
  508.                     clk[prn1][2] = Double.parseDouble(st.nextToken());
  509.                     clk[prn1][3] = Double.parseDouble(st.nextToken());
  510.                     clk[prn1][4] = Double.parseDouble(st.nextToken());
  511.                     //Praw[prn1] = Double.valueOf(st.nextToken()).doubleValue();
  512.                     SV[prn1] = true;
  513.                     int el = 0;
  514.                     for (int i = 0; i < 7; i++) {
  515.                         if ((line = br.readLine()) != null) {
  516.                             st = new StringTokenizer(line);
  517.                             if (i == 0) {
  518.                                 st.nextToken();
  519.                                 for (int j = 0; j < 3; j++) {
  520.                                     eph[prn1][el] = Double.parseDouble(st.nextToken());
  521.                                     el++;
  522.                                 }
  523.                             } else if (i == 4) {
  524.                                 eph[prn1][el] = Double.parseDouble(st.nextToken());
  525.                             } else if (i < 4) {
  526.                                 for (int j = 0; j < 4; j++) {
  527.                                     eph[prn1][el] = Double.parseDouble(st.nextToken());
  528.                                     if (el == 7)
  529.                                         clk[prn1][1] = eph[prn1][el];
  530.                                     el++;
  531.                                 }
  532.                             }
  533.                         }
  534.                     }
  535.                 }
  536.             } while (line != null);  //
  537.             br.close();
  538.  
  539.             d = new FileDialog(new Frame(), "Wybierz plik O", FileDialog.LOAD);
  540.             d.setFile("*");
  541.             d.show();
  542.             String file2 = d.getFile();
  543.             if (file2 != null)
  544.                 file2 = d.getDirectory() + d.getFile();
  545.             else return;
  546.  
  547.             try {
  548.                 br = new BufferedReader(new InputStreamReader(new FileInputStream(file2)));
  549.             } catch (IOException e) {
  550.                 System.out.println("IOException = " + e);
  551.                 return;
  552.             }
  553.  
  554.             StringBuilder lineSearch = new StringBuilder();
  555.             int num = 0;
  556.             int used = 0;
  557.             for (boolean flag : SV) {
  558.                 if (flag) {
  559.                     lineSearch.append(Integer.toString(num)).append("G");
  560.                     used++;
  561.                 }
  562.                 num++;
  563.             }
  564.             lineSearch.setLength(lineSearch.length() - 1);
  565.             lineSearch.append("R");
  566.  
  567.             do {
  568.                 if ((line = br.readLine()) != null) ;
  569.             } while (line != null && !line.contains(lineSearch.toString()));
  570.             st = new StringTokenizer(line);
  571.             StringBuilder sDate = new StringBuilder(Integer.toString(2000 + Integer.parseInt(st.nextToken())));
  572.             sDate.insert(0, Integer.toString(Integer.parseInt(st.nextToken())) + "/");
  573.             sDate.insert(0, Integer.toString(Integer.parseInt(st.nextToken())) + "/");
  574.             SimpleDateFormat format = new SimpleDateFormat("dd/MM/yyyy");
  575.             Date date = format.parse(sDate.toString());
  576.             Calendar c = Calendar.getInstance();
  577.             c.setTime(date);
  578.             int weekDay = c.get(Calendar.DAY_OF_WEEK);
  579.             weekDay -= 1;
  580.             Trc = (((weekDay * 24) + Integer.parseInt(st.nextToken())) * 3600) + (Integer.parseInt(st.nextToken()) * 60) + (int) Double.parseDouble(st.nextToken());
  581.  
  582.             String[] s = line.split(" +");
  583.             s = s[8].split("G");
  584.             int found = Integer.parseInt(s[0]);
  585.             prn1 = 0;
  586.             for (boolean flag : SV) {
  587.                 if (flag) {
  588.                     if ((line = br.readLine()) != null) ;
  589.                     st = new StringTokenizer(line);
  590.                     Praw[prn1] = Float.parseFloat(st.nextToken());
  591.                     if ((line = br.readLine()) != null) ;
  592.                 }
  593.                 prn1++;
  594.             }
  595.             System.out.println(Double.toString(Trc));
  596.             for (Double i : ion) {
  597.                 System.out.println(Double.toString(i));
  598.             }
  599.             int i = 0;
  600.             for (boolean flag : SV) {
  601.                 if (flag)
  602.                     System.out.println(Integer.toString(i) + " " + Double.toString(Praw[i]));
  603.                 i++;
  604.             }
  605.             i = 0;
  606.             for (boolean flag : SV) {
  607.                 if (flag) {
  608.                     System.out.println(Integer.toString(i));
  609.                     for (Double db : eph[i]) {
  610.                         System.out.println(Double.toString(db));
  611.                     }
  612.                     for (Double db : clk[i]) {
  613.                         System.out.println(Double.toString(db));
  614.                     }
  615.                 }
  616.                 i++;
  617.             }
  618.             //
  619.  
  620.         } catch (IOException | ParseException e) {
  621.             System.out.println("IOException = " + e);
  622.         }
  623.  
  624.         System.out.println("SPozycja poczatkowa Lat [deg.dec] +  Lon [deg.dec] +  Alt [m] : ");
  625.         System.out.println("       52 5 0");
  626.         Xlla[0] = 54.373810539 * pi / 180.0;
  627.         Xlla[1] = 18.614489984 * pi / 180.0;
  628.         Xlla[2] = 0;
  629.  
  630.         Xr = LLA2XYZ(Xlla);
  631.  
  632.  
  633.         Cr = 0;
  634.         for (int prn = 1; prn <= 32; prn++)
  635.             Pcor[prn] = 0.075 * c;  //
  636.  
  637.         for (int pass = 1; pass <= 2; pass++) {
  638.             System.out.println();
  639.             System.out.println("-------------------------- Iteracja " + pass + " -------------------------");
  640.  
  641.             for (int prn = 1; prn <= 32; prn++) //
  642.                 if (SV[prn]) {
  643.  
  644.                     //wylicz czas
  645.                     double tau = (Pcor[prn] + Cr) / c;
  646.                     double Ttr = Trc - tau;
  647.  
  648.                     //
  649.                     double[] tmp4 = satpos(eph[prn], Ttr);
  650.  
  651.                     //
  652.                     double alpha = tau * We;
  653.                     Xs[prn][0] = tmp4[0] * Math.cos(alpha) + tmp4[1] * Math.sin(alpha);
  654.                     Xs[prn][1] = -tmp4[0] * Math.sin(alpha) + tmp4[1] * Math.cos(alpha);
  655.                     Xs[prn][2] = tmp4[2];
  656.                     double Trel = tmp4[3];
  657.                     System.out.println("SV     : " + prn + " " + Xs[prn][0] + " " + Xs[prn][1] + " " + Xs[prn][2]);
  658.                     //
  659.  
  660.                     //wylicza azymut i elewacje
  661.                     double[] tmp3 = new double[3];
  662.                     for (int i = 0; i < 3; i++)
  663.                         tmp3[i] = Xs[prn][i];
  664.                     double[] tmp2 = calcAzEl(tmp3, Xr);
  665.                     double Az, El;
  666.                     if (tmp2 == null) {
  667.                         System.out.println("Blad - sprawdz dane wejsciowe");
  668.                         return;
  669.                     } else {
  670.                         Az = tmp2[0];
  671.                         El = tmp2[1];
  672.                     }
  673.                     System.out.println("Az, El : " + prn + " " + (Az * 180.0 / pi) + " " + (El * 180.0 / pi));
  674.  
  675.                     //oblicz pseudoodleglosci
  676.  
  677.                     //poprawki zegara
  678.                     double dTclck = -clk[prn][0] + clk[prn][2] + clk[prn][3] * (Ttr - clk[prn][1])
  679.                             + clk[prn][4] * (Ttr - clk[prn][1]) * (Ttr - clk[prn][1])
  680.                             + Trel;
  681.  
  682.                     //poprawki iono
  683.                     double dTiono = ionocorr(ion, Xlla[0], Xlla[1], Az, El, Ttr);
  684.  
  685.                     //poprawki tropo
  686.                     double dRtrop = 2.312 / Math.sin(Math.sqrt(El * El + 1.904E-3))
  687.                             + 0.084 / Math.sin(Math.sqrt(El * El + 0.6854E-3));
  688.  
  689.                     System.out.println("Corr   : " + prn + " " + (dTclck * c) + " " + (dTiono * c) + " " + dRtrop);
  690.                     //pseudoodleglosci poprawione
  691.                     Pcor[prn] = Praw[prn] + dTclck * c - dTiono * c - dRtrop + Cr;
  692.  
  693.                 }  //for
  694.  
  695.             //oblicz pozycje odbiornika
  696.             double[] tmp4 = solve(Xs, SV, Pcor, Xr);
  697.             if (tmp4 == null) {
  698.                 System.out.println("Blad - sprawdz dane wejsciowe");
  699.                 return;
  700.             } else {
  701.                 Xr[0] = tmp4[0];
  702.                 Xr[1] = tmp4[1];
  703.                 Xr[2] = tmp4[2];
  704.                 Cr = tmp4[3];
  705.             }
  706.             System.out.println();
  707.             System.out.println("Pos XYZ: " + Xr[0] + " " + Xr[1] + " " + Xr[2] + " " + Cr);
  708.             Xlla = XYZ2LLA(Xr);
  709.             System.out.println("Pos LLA: " + (Xlla[0] * 180.0 / pi) + " " + (Xlla[1] * 180.0 / pi) + " " + Xlla[2]);
  710.         }
  711.  
  712.     }
  713.  
  714. }
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