public static double getDistance(XYZCoord a, XYZCoord b) { double d = Math.Pow(a.X - b.X, 2.0) + Math.Pow(a.Y - b.Y, 2.0); d += Math.Pow(a.Z - b.Z, 2.0); return(Math.Sqrt(d)); }
private XYZCoord LinearInterpolate(XYZCoord in1, XYZCoord in2, double percent) { XYZCoord coord; GPSUtilsClass class2 = new GPSUtilsClass(); double[] numArray = new double[] { in1.X, in1.Y, in1.Z }; double[] numArray2 = new double[] { in2.X, in2.Y, in2.Z }; double[] numArray3 = new double[3]; numArray3 = class2.LinearInterpolateXYZ(numArray, numArray2, percent); coord.X = numArray3[0]; coord.Y = numArray3[1]; coord.Z = numArray3[2]; return(coord); }
public bool GetPositionAtTime(double inTime, out XYZCoord result) { result.X = 0.0; result.Y = 0.0; result.Z = 0.0; try { int num = this.m_PositionData.IndexOfKey(inTime); if (num != -1) { result = this.m_PositionData.Values[num]; this.m_current_idx = num; return(true); } int num2 = 0; int num3 = this.m_PositionData.Count - 1; return(this.GetPositionAtTimeBetweenIndexes(inTime, this.m_current_idx, num3, out result) || this.GetPositionAtTimeBetweenIndexes(inTime, num2, num3, out result)); } catch { } return(false); }
public bool GetPositionAtTime(double inTime, out XYZCoord result) { result.X = 0.0; result.Y = 0.0; result.Z = 0.0; try { int num = this.m_PositionData.IndexOfKey(inTime); if (num != -1) { result = this.m_PositionData.Values[num]; this.m_current_idx = num; return true; } int num2 = 0; int num3 = this.m_PositionData.Count - 1; return (this.GetPositionAtTimeBetweenIndexes(inTime, this.m_current_idx, num3, out result) || this.GetPositionAtTimeBetweenIndexes(inTime, num2, num3, out result)); } catch { } return false; }
private bool GetPositionAtTimeBetweenIndexes(double inTime, int low_idx, int high_idx, out XYZCoord result) { try { if ((inTime >= this.m_PositionData.Keys[low_idx]) && (inTime <= this.m_PositionData.Keys[high_idx])) { int num = low_idx; while (num < high_idx) { if (inTime <= this.m_PositionData.Keys[num]) { break; } num++; } num--; this.m_current_idx = num; double percent = (100.0 * (inTime - this.m_PositionData.Keys[num])) / (this.m_PositionData.Keys[num + 1] - this.m_PositionData.Keys[num]); result = this.LinearInterpolate(this.m_PositionData.Values[num], this.m_PositionData.Values[num + 1], percent); return(true); } } catch { } result.X = 0.0; result.Y = 0.0; result.Z = 0.0; return(false); }
public XYZCoord GetRinexSVPos(string filename, int svPRN, double TOW, out double ionoError, out double svClkBias, out XYZCoord svVel) { XYZCoord coord = new XYZCoord(); if (this.m_RinexFile.GetNumEphemerides() == 0) { this.m_RinexFile.Read(@"..\scripts\TunnelsMeasAndNav\brdc2140.07n"); } RinexEph eph = this.m_RinexFile.SearchRinexArrayList((byte) svPRN, (int) TOW); if (eph == null) { ionoError = 0.0; svClkBias = 0.0; svVel.X = svVel.Y = svVel.Z = 0.0; return coord; } tSVD_SVState st = new tSVD_SVState(); tGPSTime gTime = new tGPSTime(); gTime.isTOWValid = true; gTime.isWeekValid = true; gTime.time = TOW; gTime.week = (int) eph.weekNo; new ComputeSVPos().computeSVState(st, (short) svPRN, eph, gTime); coord.X = st.pos[0]; coord.Y = st.pos[1]; coord.Z = st.pos[2]; svVel.X = st.vel[0]; svVel.Y = st.vel[1]; svVel.Z = st.vel[2]; ionoError = st.ionoDelayStd; svClkBias = st.clockBias; return coord; }
public XYZCoord GetIMUTruePos(string filename, double TOW) { XYZCoord result = new XYZCoord(); if (this.m_ImuFile.GetNumSamples() == 0) { this.m_ImuFile.LoadFromFile(filename); } this.m_ImuFile.GetPositionAtTime(TOW, out result); return result; }
public double ComputePRerrorOneSV(int svPRN, double svTOW, double svPseudoRange, double ionoErr, double clkBias, string RinexFileName, string IMUFileName, out string strPRtrue_ClkBiasRange_txTOW) { double num6; double num7; double num = 0.0; strPRtrue_ClkBiasRange_txTOW = "-1.0,-1.0,-1.0"; this.m_SV_PRN = svPRN; this.m_SV_TOW = svTOW; double num2 = svPseudoRange / 299792458.0; this.m_SV_TxTOW = (this.m_SV_TOW - num2) - clkBias; double ionoError = 0.0; double svClkBias = 0.0; XYZCoord svVel = new XYZCoord(); XYZCoord a = this.GetRinexSVPos(RinexFileName, this.m_SV_PRN, this.m_SV_TxTOW, out ionoError, out svClkBias, out svVel); XYZCoord iMUTruePos = this.GetIMUTruePos(IMUFileName, svTOW); XYZCoord coord4 = new XYZCoord(); if ((a == coord4) || (iMUTruePos == coord4)) { return -1.0; } double num5 = XYZCoord.getDistance(a, iMUTruePos); double[] svPos = new double[] { a.X, a.Y, a.Z }; double[] navPos = new double[] { iMUTruePos.X, iMUTruePos.Y, iMUTruePos.Z }; double[] numArray3 = new double[] { svVel.X, svVel.Y, svVel.Z }; computeTropo tropo = new computeTropo(); GPSUtilsClass.ComputeAzEl(navPos, svPos, out num6, out num7); double alt = GPSUtilsClass.ComputeApproxAltitude(navPos); double num9 = tropo.NL_ComputeTropo(alt, num7); double num10 = computeEarthRotation.earthRotationCorrection(svPos, navPos, numArray3); double num11 = num5 - num10; double num12 = (svPseudoRange - (clkBias * 299792458.0)) + (svClkBias * 299792458.0); strPRtrue_ClkBiasRange_txTOW = string.Format("{0}, {1}, {2}", num5, num12, this.m_SV_TxTOW); num = num5 - num12; return (((num - ionoErr) - num9) - num11); }
public static double getDistance(XYZCoord a, XYZCoord b) { double d = Math.Pow(a.X - b.X, 2.0) + Math.Pow(a.Y - b.Y, 2.0); d += Math.Pow(a.Z - b.Z, 2.0); return Math.Sqrt(d); }
private XYZCoord LinearInterpolate(XYZCoord in1, XYZCoord in2, double percent) { XYZCoord coord; GPSUtilsClass class2 = new GPSUtilsClass(); double[] numArray = new double[] { in1.X, in1.Y, in1.Z }; double[] numArray2 = new double[] { in2.X, in2.Y, in2.Z }; double[] numArray3 = new double[3]; numArray3 = class2.LinearInterpolateXYZ(numArray, numArray2, percent); coord.X = numArray3[0]; coord.Y = numArray3[1]; coord.Z = numArray3[2]; return coord; }
private bool GetPositionAtTimeBetweenIndexes(double inTime, int low_idx, int high_idx, out XYZCoord result) { try { if ((inTime >= this.m_PositionData.Keys[low_idx]) && (inTime <= this.m_PositionData.Keys[high_idx])) { int num = low_idx; while (num < high_idx) { if (inTime <= this.m_PositionData.Keys[num]) { break; } num++; } num--; this.m_current_idx = num; double percent = (100.0 * (inTime - this.m_PositionData.Keys[num])) / (this.m_PositionData.Keys[num + 1] - this.m_PositionData.Keys[num]); result = this.LinearInterpolate(this.m_PositionData.Values[num], this.m_PositionData.Values[num + 1], percent); return true; } } catch { } result.X = 0.0; result.Y = 0.0; result.Z = 0.0; return false; }