• API
• FAQ
• Tools
• Archive
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.

Top