API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Nov 12th, 2019
142
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 13.54 KB | None | 0 0
raw download clone embed print report
  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. }
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!