API tools faq
paste
Advertisement
Guest User

heheh

a guest
Nov 17th, 2019
118
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 25.14 KB | None | 0 0
raw download clone embed print report
  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. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!