public void FrobeniusTest() { double[,] a = Matrix.Magic(5); double expected = 74.330343736592520; double actual = Norm.Frobenius(a); Assert.AreEqual(expected, actual, 1e-12); }
/*class Sat * { * public entry Observation; * public EntryNavigation Navigation; * }*/ static void Main(string[] args) { //Constante double GM = 3.986005 * Math.Pow(10, 14); double omegaE = 7.2921151467 * Math.Pow(10, -5); var pi = Math.PI; Thread.CurrentThread.CurrentCulture = CultureInfo.InvariantCulture; //Code Rinex fisier = new Rinex(); fisier.ReadFIle(@"D:\GNSS\250221.sbL"); fisier.ReadFIle(@"D:\GNSS\250221.sbO"); getPosition(fisier.ObservationFile, fisier.NavigationFile); // Conversie din Epoch in JulDate // Meeus, Jean (1991) Astronomical Algorithms, Willmann-Bell, Richmond, Virginia, p. 59--62 double julday(DateTime date) { double y = date.Year; double m = date.Month; double d = date.Day; double h = date.Hour + (double)date.Minute / 60 + (double)date.Second / 3600; //Jan and Feb are considered to be the 13th and 14th month of the preceding year if (m <= 2) { y -= 1; m += 12; } //Julian day number 0 assigned to the day starting at noon on Monday, January 1, 4713 BC return(Math.Floor(365.25 * (y + 4716)) + Math.Floor(30.6001 * (m + 1)) + d + h / 24 - 1537.5); } //Conversie in GPS Time ( return double[2] = { week, sec_of_week } ) double[] gps_time(double julday) { double a = Math.Floor(julday + .5); double b = a + 1537; double c = Math.Floor((b - 122.1) / 365.25); double e = Math.Floor(365.25 * c); double f = Math.Floor((b - e) / 30.6001); double d = b - e - Math.Floor(30.6001 * f) + (julday + .5) % 1; double day_of_week = (Math.Floor(julday + .5)) % 7; //GPS standard epoch, JD = 2,444,244.5 (January 6th, 1980, 00:00 UTC) double week = Math.Floor((julday - 2444244.5) / 7); // We add + 1 as the GPS week starts at Saturday midnight double sec_of_week = Math.Round(((d % 1) + day_of_week + 1) * 86400); return(new double[2] { week, sec_of_week }); } double check_t(double t) { //halfWeek = 302400; if (t > 302400) { t -= 604800; } if (t < -302400) { t += 604800; } return(t); } //Computation of satellite coordinates (X,Y,Z) at time t //Strang G, Borre K (1997) Linear algebra, geodesy, and GPS - pag 482-487 double[] satPos(double t, EntryNavigation entry) { double A = entry.Group2.sqrtA * entry.Group2.sqrtA; double tk = check_t(t - entry.Group3.Toe); double n0 = Math.Sqrt(GM / Math.Pow(A, 3)); double n = n0 + entry.Group1.deltaN; double M = entry.Group1.M0 + n * tk; M = (M + 2 * pi) % (2 * pi); double E = M; for (int j = 0; j < 10; j++) { double E_old = E; E = M + entry.Group2.e * Math.Sin(E); double dE = (E - E_old) % (2 * pi); if (Math.Abs(dE) < 1.2E-12) { break; } } E = (E + 2 * pi) % (2 * pi); double v = Math.Atan2(Math.Sqrt(1 - Math.Pow(entry.Group2.e, 2)) * Math.Sin(E), Math.Cos(E) - entry.Group2.e); double phi = v + entry.Group4.omega; phi = phi % (2 * pi); double u = phi + entry.Group2.Cuc * Math.Cos(2 * phi) + entry.Group2.Cus * Math.Sin(2 * phi); double r = A * (1 - entry.Group2.e * Math.Cos(E)) + entry.Group4.Crc * Math.Cos(2 * phi) + entry.Group1.Crs * Math.Sin(2 * phi); double i = entry.Group4.i0 + entry.Group5.IDOT * tk + entry.Group3.Cic * Math.Cos(2 * phi) + entry.Group3.Cis * Math.Sin(2 * phi); double Omega = entry.Group3.OMEGA + (entry.Group4.OMEGADOT - omegaE) * tk - omegaE * entry.Group3.Toe; Omega = (Omega + 2 * pi) % (2 * pi); double x1 = Math.Cos(u) * r; double y1 = Math.Sin(u) * r; double x = x1 * Math.Cos(Omega) - y1 * Math.Cos(i) * Math.Sin(Omega); double y = x1 * Math.Sin(Omega) + y1 * Math.Cos(i) * Math.Cos(Omega); double z = y1 * Math.Sin(i); return(new double[3] { x, y, z }); } //Returns rotated satellite ECEF coordinates due to Earth rotation during signal travel time //Strang G, Borre K (1997) Linear algebra, geodesy, and GPS. - pag 492 double[] e_r_corr(double travelTime, double[] Xsat) { double omegaTau = omegaE * travelTime; double[,] R3 = new double[3, 3] { { Math.Cos(omegaTau), Math.Sin(omegaTau), 0 }, { -Math.Sin(omegaTau), Math.Cos(omegaTau), 0 }, { 0, 0, 1 } }; double[] XsatRot = { 0, 0, 0 }; for (int i = 0; i < 3; i++) { XsatRot[0] += R3[0, i] * Xsat[i]; XsatRot[1] += R3[1, i] * Xsat[i]; XsatRot[2] += R3[2, i] * Xsat[i]; } return(new double[3] { XsatRot[0], XsatRot[1], XsatRot[2] }); } //calculate geodetic coordinates double[] toGeod(double a, double finv, double x, double y, double z) { double h = 0; double tolsq = 1E-10; double maxIt = 10; double rtd = 180 / pi; double esq; if (finv < 1E-20) { esq = 0; } else { esq = (2 - 1 / finv) / finv; } double oneesq = 1 - esq; double P = Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)); double dlambda; if (P > 1e-20) { dlambda = Math.Atan2(y, x) * rtd; } else { dlambda = 0; } if (dlambda < 0) { dlambda += 360; } double r = Math.Sqrt(Math.Pow(P, 2) + Math.Pow(z, 2)); double sinPhi; if (r > 1e-20) { sinPhi = z / r; } else { sinPhi = 0; } double dPhi = Math.Asin(sinPhi); if (r < 1e-20) { h = 0; return(new double[3] { dPhi, dlambda, h }); } h = r - a * (1 - sinPhi * sinPhi / finv); for (int i = 0; i < maxIt; i++) { sinPhi = Math.Sin(dPhi); double cosPhi = Math.Cos(dPhi); double nPhi = a / Math.Sqrt(1 - esq * sinPhi * sinPhi); double dP = P - (nPhi + h) * cosPhi; double dZ = z - (nPhi * oneesq + h) * sinPhi; h += sinPhi * dZ + cosPhi * dP; dPhi += (cosPhi * dZ - sinPhi * dP) / (nPhi + h); if (dP * dP + dZ * dZ < tolsq) { break; } } dPhi *= rtd; return(new double[3] { dPhi, dlambda, h }); } //Transformation of vector dx into topocentric coordinate system with origin at X. //D vector length in units like the input //Az azimuth from north positive clockwise, degrees //El elevation angle, degrees double[] topocent(double[] x, double[] dx) { double dtr = pi / 180; double[] data = toGeod(6378137, 298.257223563, x[0], x[1], x[2]); double phi = data[0]; double lambda = data[1]; double c1 = Math.Cos(lambda * dtr); double s1 = Math.Sin(lambda * dtr); double cb = Math.Cos(phi * dtr); double sb = Math.Sin(phi * dtr); double[,] F = new double[3, 3]; F[0, 0] = -s1; F[0, 1] = -sb * c1; F[0, 2] = cb * c1; F[1, 0] = c1; F[1, 1] = -sb * s1; F[1, 2] = cb * s1; F[2, 0] = 0; F[2, 1] = cb; F[2, 2] = sb; double[,] Ftrans = F.Transpose(); double[] vect = { 0, 0, 0 }; for (int i = 0; i < 3; i++) { vect[0] += Ftrans[0, i] * dx[i]; vect[1] += Ftrans[1, i] * dx[i]; vect[2] += Ftrans[2, i] * dx[i]; } double E = vect[0]; double N = vect[1]; double U = vect[2]; double horDis = Math.Sqrt(Math.Pow(E, 2) + Math.Pow(N, 2)); double Az; double El; if (horDis < 1e-20) { Az = 0; El = 90; } else { Az = Math.Atan2(E, N) / dtr; El = Math.Atan2(U, horDis) / dtr; } if (Az < 0) { Az += 360; } double D = Math.Sqrt(Math.Pow(dx[0], 2) + Math.Pow(dx[1], 2) + Math.Pow(dx[2], 2)); return(new double[3] { Az, El, D }); } // Calculation of tropospheric correction. // sinel sin of elevation angle of satellite // hsta height of station in km // p atmospheric pressure in mb at height hp // tkel surface temperature in degrees Kelvin at height htkel // hum humidity in % at height hhum // hp height of pressure measurement in km // htkel height of temperature measurement in km // hhum height of humidity measurement in km double tropo(double sinel, double hsta, double p, double tkel, double hum, double hp, double htkel, double hhum) { double ae = 6378137; double b0 = 7.839257e-5; double tlapse = -6.5; double tkhum = tkel + tlapse * (hhum - htkel); double atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15); double e0 = 0.0611 * hum * Math.Pow(10, atkel); double tksea = tkel - tlapse * htkel; double em = -978.77 / (2.8704e6 * tlapse * 1e-5); double tkelh = tksea + tlapse * hhum; double e0sea = e0 * Math.Pow((tksea / tkelh), 4 * em); double tkelp = tksea + tlapse * hp; double psea = p * Math.Pow((tksea / tkelp), em); if (sinel < 0) { sinel = 0; } double tropo = 0; bool done = false; double refsea = 77.624e-6 / tksea; double htop = 1.1385e-5 / refsea; refsea *= psea; double Ref = refsea * Math.Pow((htop - hsta) / htop, 4); for (; ;) { double rtop = Math.Pow((ae + htop), 2) - Math.Pow(ae + hsta, 2) * (1 - Math.Pow(sinel, 2)); if (rtop < 0) { rtop = 0; } rtop = Math.Sqrt(rtop) - (ae + hsta) * sinel; double a = -sinel / (htop - hsta); double b = -b0 * (1 - Math.Pow(sinel, 2)) / (htop - hsta); double[] rn = new double[10]; for (int i = 0; i < 8; i++) { rn[i] = Math.Pow(rtop, i + 2); } double[] alpha = { 2 * a, 2 * Math.Pow(a, 2) + 4 * b / 3, a *(Math.Pow(a, 2) + 3 * b), Math.Pow(a, 4) / 5 + 2.4 * Math.Pow(a, 2) * b + 1.2 * Math.Pow(b, 2), 2 * a * b * (Math.Pow(a, 2) + 3 * b) / 3, Math.Pow(b, 2) * (Math.Pow(a, 2) * 6 + 4 * b) * 1.428571e-1, 0, 0 }; if (Math.Pow(b, 2) > 1e-35) { alpha[6] = a * Math.Pow(b, 3) / 2; alpha[7] = Math.Pow(b, 4) / 9; } double dr = rtop; for (int i = 0; i < 8; i++) { dr += alpha[i] * rn[i]; } tropo += dr * Ref * 1000; if (done == true) { return(tropo); } done = true; refsea = (371900e-6 / tksea - 12.92e-6) / tksea; htop = 1.1385e-5 * (1255 / tksea + 0.05) / refsea; Ref = refsea * e0sea * Math.Pow((htop - hsta) / htop, 4); } } //Computation of receiver position from pseudoranges using ordinary least-squares principle double[,] recpo_ls(record obsData, double time, RinexNavigation eph) { //Console.WriteLine("+++++++++++++++++ | {0}", obsData.Epoch.TimeOfDay); //obs[] primul pseudorange de la toti satelitii dintr o epoca double[] pos = { 0, 0, 0, 0 }; //double GDOP; double v_light = 299792458; double dtr = pi / 180; int n = 0; int[] sat = new int[obsData.Satellites.Count]; for (int i = 0; i < obsData.Satellites.Count; i++) { for (int j = 0; j < eph.Entries.Count; j++) { if (gps_time(julday(eph.Entries[j].Toc.AddHours(2)))[1] >= time && gps_time(julday(eph.Entries[j].Toc))[1] <= time && obsData.Satellites[i].Name == eph.Entries[j].Name) { sat.SetValue(i, n); n++; break; } } } if (n < 4) { return new double[1, 1] { { 0 } } } ; double[] El = new double[n]; double[,] A = new double[n, 4]; double[,] omc = new double[n, 1]; double[,] satPosition = new double[n, 10]; for (int iter = 0; iter < 6; iter++) { for (int i = 0; i < n; i++) { //cautare eph corespunzatoare sat din obs file double travelTime; double rho2; for (int j = 0; j < eph.Entries.Count; j++) { if (gps_time(julday(eph.Entries[j].Toc.AddHours(2)))[1] >= time && gps_time(julday(eph.Entries[j].Toc))[1] <= time && obsData.Satellites[i].Name == eph.Entries[j].Name) { double txRaw = time - obsData.Satellites[sat[i]].Data[0] / v_light; double dt = check_t(txRaw - eph.Entries[j].Group3.Toe); double tcorr = (eph.Entries[j].Group0.a2 * dt + eph.Entries[j].Group0.a1 * dt + eph.Entries[j].Group0.a0); double txGAL = txRaw - tcorr; dt = check_t(txGAL - eph.Entries[j].Group3.Toe); tcorr = (eph.Entries[j].Group0.a2 * dt + eph.Entries[j].Group0.a1 * dt + eph.Entries[j].Group0.a0); txGAL = txRaw - tcorr; double[] X = satPos(txGAL, eph.Entries[j]); satPosition[i, 0] = X[0]; satPosition[i, 1] = X[1]; satPosition[i, 2] = X[2]; double[] rotX; double trop; if (iter == 0) { travelTime = 0.072; rotX = X; trop = 0; } else { rho2 = Math.Pow(X[0] - pos[0], 2) + Math.Pow(X[1] - pos[1], 2) + Math.Pow(X[2] - pos[2], 2); travelTime = Math.Sqrt(rho2) / v_light; rotX = e_r_corr(travelTime, X); satPosition[i, 3] = rotX[0]; satPosition[i, 4] = rotX[1]; satPosition[i, 5] = rotX[2]; rho2 = Math.Pow(rotX[0] - pos[0], 2) + Math.Pow(rotX[1] - pos[1], 2) + Math.Pow(rotX[2] - pos[2], 2); double el = topocent(new double[] { pos[0], pos[1], pos[2] }, new double[] { rotX[0] - pos[0], rotX[1] - pos[1], rotX[2] - pos[2] })[1]; if (iter == 5) { El[i] = el; } trop = tropo(Math.Sin(el * dtr), 0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); satPosition[i, 9] = trop; } omc.SetValue(obsData.Satellites[sat[i]].Data[0] - Norm.Frobenius(new double[, ] { { rotX[0] - pos[0], rotX[1] - pos[1], rotX[2] - pos[2] }, { 0, 0, 0 } }) - pos[3] + v_light * tcorr, i, 0); //double val = Math.Sqrt(Math.Pow(rotX[0] - pos[0], 2) + Math.Pow(rotX[1] - pos[1], 2) + Math.Pow(rotX[2] - pos[2], 2)) - pos[4] + v_light * tcorr - trop; //omc.SetValue(val, i, 0); A.SetValue((-(rotX[0] - pos[0])) / obsData.Satellites[sat[i]].Data[0], i, 0); A.SetValue((-(rotX[1] - pos[1])) / obsData.Satellites[sat[i]].Data[0], i, 1); A.SetValue((-(rotX[2] - pos[2])) / obsData.Satellites[sat[i]].Data[0], i, 2); A.SetValue(1, i, 3); // Console.WriteLine("{0} | {1} | {2} | {3}",A.GetValue(i,0),A.GetValue(i,1),A.GetValue(i,2),A.GetValue(i,3)); } else { continue; } break; } } double[] x = { 0, 0, 0, 0 }; double[,] Ainv = A.PseudoInverse(); for (int i = 0; i < omc.Length; i++) { x[0] += Ainv[0, i] * omc[i, 0]; x[1] += Ainv[1, i] * omc[i, 0]; x[2] += Ainv[2, i] * omc[i, 0]; x[3] += Ainv[3, i] * omc[i, 0]; } pos[0] += x[0]; pos[1] += x[1]; pos[2] += x[2]; pos[3] += x[3]; } for (int i = 0; i < n; i++) { satPosition[i, 6] = pos[0]; satPosition[i, 7] = pos[1]; satPosition[i, 8] = pos[2]; } return(satPosition); } //conversion of degrees to degrees, mins, secs double[] deg2dms(double deg) { bool negArg = false; if (deg < 0) { negArg = true; deg = -deg; } double intDeg = Math.Floor(deg); double Decimal = deg - intDeg; double minPart = Decimal * 60; double min = Math.Floor(minPart); double secPart = minPart - min; double sec = secPart * 60; if (sec == 60) { min++; sec = 0; } if (min == 60) { intDeg++; min = 0; } if (negArg == true) { intDeg = -intDeg; } return(new double[3] { intDeg, min, sec }); } void cart2geo(double x, double y, double z) { double a = 6378137; double f = 1 / 298.257223563; double lambda = Math.Atan2(y, x); double ex2 = (2 - f) * f / Math.Pow(1 - f, 2); double c = a * Math.Sqrt(1 + ex2); double phi = Math.Atan(z / ((Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) * (1 - (2 - f)) * f))); double h = 0.1; double oldH = 0; double N; while (Math.Abs(h - oldH) > 1e-13) { oldH = h; N = c / Math.Sqrt(1 + ex2 * Math.Pow(Math.Cos(phi), 2)); phi = Math.Atan(z / ((Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) * (1 - (2 - f) * f * N / (N + h))))); h = Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) / Math.Cos(phi) - N; } double[] phiRes = deg2dms(phi * 180 / pi); double[] lambdaRes = deg2dms(lambda * 180 / pi); Console.WriteLine("lat : {0}° {1}' {2}\"", phiRes[0], phiRes[1], phiRes[2]); Console.WriteLine("long : {0}° {1}' {2}\"", lambdaRes[0], lambdaRes[1], lambdaRes[2]); } void getPosition(RinexObservation Obsfile, RinexNavigation eph) { int noOfEpochs = Obsfile.Entries.Count; double[,] Pos = new double[4, noOfEpochs]; double[,] pos; foreach (record ObsData in Obsfile.Entries) { double timeOfWeek = gps_time(julday(ObsData.Epoch))[1]; pos = recpo_ls(ObsData, timeOfWeek, eph); if (pos[0, 0] != 0) { for (int i = 0; i < pos.GetLength(0); i++) { using (StreamWriter sw = File.AppendText(@"D:\GNSS\test.txt")) { sw.Write("{0} | {1} | {2} | {3} | {4} | {5} | {6} | {7} | {8} | {9} \n", pos[i, 0].ToString(), pos[i, 1].ToString(), pos[i, 2].ToString(), pos[i, 3].ToString(), pos[i, 4].ToString(), pos[i, 5].ToString(), pos[i, 6].ToString(), pos[i, 7].ToString(), pos[i, 8].ToString(), pos[i, 9].ToString()); } } // 0 , 1 , 2 - satellite position // 3 , 4 , 5 - rotated satellite ECEF coordinates due to Earth rotation during signal travel time // 6 , 7 , 8 - receiver position (corrected) // 9 - tropo correction } } } #region codeVechi /*// 6 Jan 1980 00:00:00 * //Constante * double GM = 3.986005 * Math.Pow(10, 14); * double omegaE = 7.292115 * Math.Pow(10, -5); * //var pi = Math.PI; * double[] CalculateSatPosition(EntryNavigation entry) * { * var totalDaysFrom1980 = (entry.Toc - reper).TotalDays; * * var dayOfWeek = totalDaysFrom1980 % 7; * * var secondsInTheWeek = dayOfWeek * 86400; * * * // Time from current ephemeris epoch * double tk = secondsInTheWeek - entry.Group3.Toe; * if (tk > 302400) * tk -= 604800; * else if (tk < -302400) * tk += 604800; * //Console.WriteLine(tk); * //Console.WriteLine(Convert.ToDecimal(tk)); * * * * * * //EccentricAnomally * // Computed mean motion * double a = Math.Pow(entry.Group2.sqrtA, 2); * double n0 = Math.Sqrt(GM / Math.Pow(a, 3)); * * // Corrected mean motion * double n = n0 + entry.Group1.deltaN; * * // Mean anomally * double Mk = entry.Group1.M0 + n * tk; * * // Solve Kepler's Equation for Eccentric Anomally * double Ek = Mk; * for (; ; ) * { * double temp = Ek; * Ek = Mk + entry.Group2.e * Math.Sin(Ek); * if (Math.Abs(Ek - temp) < 1e-10) break; * } * // return Ek * * * * //True anomally * double vk = Math.Atan2(Math.Sqrt(1 - entry.Group2.e * entry.Group2.e) * Math.Sin(Ek), Math.Cos(Ek) - entry.Group2.e); * * // Argument of lattitude * double AOL = vk * entry.Group4.omega; * * // Second Harmonic Perturbation * double duk = entry.Group2.Cus * Math.Sin(2 * AOL) + entry.Group2.Cuc * Math.Cos(2 * AOL); // Argument of lattitude correction * double drk = entry.Group1.Crs * Math.Sin(2 * AOL) + entry.Group4.Crc * Math.Cos(2 * AOL); // Radius Correction * double dik = entry.Group3.Cis * Math.Sin(2 * AOL) + entry.Group3.Cic * Math.Cos(2 * AOL); // Inclination Correction * * // Corrected Argument of lattitude , Radius & Inclination * double uk = AOL + duk; * double rk = a * (1 - entry.Group2.e * Math.Cos(Ek)) + drk; * double ik = entry.Group4.i0 + dik + entry.Group5.IDOT * tk; * * // Position in orbital plane * double x_kp = rk * Math.Cos(uk); * double y_kp = rk * Math.Sin(uk); * * // Corrected longitude of ascending mode * double OmegaK = entry.Group3.OMEGA + (entry.Group4.OMEGADOT - omegaE) * tk - omegaE * entry.Group3.Toe; * * // Earth fixed coordinates * double x = x_kp * Math.Cos(OmegaK) - y_kp * Math.Cos(ik) * Math.Sin(OmegaK); * double y = x_kp * Math.Sin(OmegaK) - y_kp * Math.Cos(ik) * Math.Cos(OmegaK); * double z = y_kp * Math.Sin(ik); * * return new double[3] * { * x, y, z * }; * } * * static double CalculateTi(double[] sat, double pseudorange) * { * return (Math.Pow(sat[0], 2) + Math.Pow(sat[1], 2) + Math.Pow(sat[2], 2)) - Math.Pow(pseudorange, 2); * } * * * var sat1 = CalculateSatPosition(fisier.NavigationFile.Entries[84]); var pseudo1 = fisier.ObservationFile.Entries[60].Satellites[0].Data[0]; * var sat2 = CalculateSatPosition(fisier.NavigationFile.Entries[83]); var pseudo2 = fisier.ObservationFile.Entries[60].Satellites[1].Data[0]; * var sat3 = CalculateSatPosition(fisier.NavigationFile.Entries[82]); var pseudo3 = fisier.ObservationFile.Entries[60].Satellites[2].Data[0]; * var sat4 = CalculateSatPosition(fisier.NavigationFile.Entries[81]); var pseudo4 = fisier.ObservationFile.Entries[60].Satellites[3].Data[0]; * var sat5 = CalculateSatPosition(fisier.NavigationFile.Entries[90]); var pseudo5 = fisier.ObservationFile.Entries[60].Satellites[5].Data[0]; * * * Matrix<double> B = DenseMatrix.OfArray(new double[,] * { * { CalculateTi(sat2, pseudo2) - CalculateTi(sat1, pseudo1) }, * { CalculateTi(sat3, pseudo3) - CalculateTi(sat1, pseudo1) }, * { CalculateTi(sat4, pseudo4) - CalculateTi(sat1, pseudo1) }, * { CalculateTi(sat5, pseudo5) - CalculateTi(sat1, pseudo1) } * }); * * Matrix<double> A = DenseMatrix.OfArray(new double[,] * { * { sat2[0] - sat1[0], sat2[1] - sat1[1], sat2[2] - sat1[2], pseudo1 - pseudo2 }, * { sat3[0] - sat1[0], sat3[1] - sat1[1], sat3[2] - sat1[2], pseudo1 - pseudo3 }, * { sat4[0] - sat1[0], sat4[1] - sat1[1], sat4[2] - sat1[2], pseudo1 - pseudo4 }, * { sat5[0] - sat1[0], sat5[1] - sat1[1], sat5[2] - sat1[2], pseudo1 - pseudo5 } * }); * Console.WriteLine(Convert.ToDecimal(sat2[0] - sat1[0])); * var X = B.Transpose().Multiply(0.5).Multiply(A.Inverse()); * Console.WriteLine(A.Inverse()); * Console.WriteLine(B.Transpose()); * Console.WriteLine(B.Transpose().Multiply(0.5)); * Console.WriteLine(X); * Console.WriteLine(Convert.ToDecimal(X[0, 0])); * Console.WriteLine(Convert.ToDecimal(X[0, 1])); * Console.WriteLine(Convert.ToDecimal(X[0, 2])); * Console.WriteLine(Convert.ToDecimal(X[0, 3])); // 59316429.1916321 * * Console.WriteLine(); * Console.WriteLine(ecef2lla(X[0, 0], X[0, 1], X[0, 2])[0]); * Console.WriteLine(ecef2lla(X[0, 0], X[0, 1], X[0, 2])[1]); * Console.WriteLine(ecef2lla(X[0, 0], X[0, 1], X[0, 2])[2]); * } * * // Exemplu din documentatie * * /*double[] sat1 = new double[3] { 28573624.909, 176258.719, 475886.493 }; double pseudo1 = 25573786.094; * double[] sat2 = new double[3] { 20534972.474, 3620869.695, 20821515.054 }; double pseudo2 = 23269991.712; * double[] sat3 = new double[3] { 13834909.426, 9331764.237, 24705373.313 }; double pseudo3 = 23527045.278; * double[] sat4 = new double[3] { -18325015.195, 12831313.778, 20831862.073 }; double pseudo4 = 29205487.559; * double[] sat5 = new double[3] { -11441576.697, 19817392.158, 15998439.113 }; double pseudo5 = 26129807.790; * * Console.WriteLine(CalculateTi(sat1, pseudo1, 0)); * Console.WriteLine("159654526737584.50");*/ #endregion } }