public void ProcessBasePoints(IEnumerable <T> basePoints, double depth, DateTime timeStamp) { double lat_deg, lon_deg, rErr; int it_Cnt; if (typeof(GeoPoint3DD).IsAssignableFrom(typeof(T))) { UCNLNav.Navigation.TOA_Locate2D(basePoints.Cast <GeoPoint3DD>().ToArray <GeoPoint3DD>(), targetLocation.Latitude, targetLocation.Longitude, depth, Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize, referenceEllipsoid, out lat_deg, out lon_deg, out rErr, out it_Cnt); } else if (typeof(GeoPoint3DT).IsAssignableFrom(typeof(T))) { UCNLNav.Navigation.TDOA_Locate2D(basePoints.Cast <GeoPoint3DT>().ToArray <GeoPoint3DT>(), targetLocation.Latitude, targetLocation.Longitude, depth, Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize, referenceEllipsoid, soundSpeed, out lat_deg, out lon_deg, out rErr, out it_Cnt); } else { if (ExternalSolver != null) { ExternalSolver(basePoints, targetLocation, out lat_deg, out lon_deg, out rErr, out it_Cnt); } else { throw new NullReferenceException("ExternalSolver not defined"); } } if (rErr < radialErrorThreshold) { crsEstimator.AddPoint(new GeoPoint(lat_deg, lon_deg)); if (crsEstimator.IsCourse) { TargetCourseUpdatedHandler.Rise(this, new TargetCourseUpdatedEventArgs(crsEstimator.Course_deg, timeStamp)); } targetLocation.Latitude = lat_deg; targetLocation.Longitude = lon_deg; targetLocationTS = timeStamp; TargetLocationUpdatedHandler.Rise(this, new TargetLocationUpdatedEventArgs(targetLocation, rErr, timeStamp)); TargetLocationUpdatedExHandler.Rise(this, new TargetLocationUpdatedExEventArgs(targetLocation, rErr, crsEstimator.Course_deg, timeStamp)); } else { RadialErrorExeedsThrehsoldEventHandler.Rise(this, new EventArgs()); } }
public void ProcessBasePoints(IEnumerable <T> basePoints, DateTime timeStamp) { double lat_deg, lon_deg, rErr; int it_Cnt; if (typeof(GeoPoint3DD).IsAssignableFrom(typeof(T))) { UCNLNav.Navigation.TOA_Locate2D(basePoints.Cast <GeoPoint3DD>().ToArray <GeoPoint3DD>(), targetLocation.Latitude, targetLocation.Longitude, targetLocation.Depth, Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize, referenceEllipsoid, out lat_deg, out lon_deg, out rErr, out it_Cnt); } else if (typeof(GeoPoint3DT).IsAssignableFrom(typeof(T))) { UCNLNav.Navigation.TDOA_Locate2D(basePoints.Cast <GeoPoint3DT>().ToArray <GeoPoint3DT>(), targetLocation.Latitude, targetLocation.Longitude, targetLocation.Depth, Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize, referenceEllipsoid, soundSpeed, out lat_deg, out lon_deg, out rErr, out it_Cnt); } else { if (ExternalSolver != null) { ExternalSolver(basePoints, targetLocation, out lat_deg, out lon_deg, out rErr, out it_Cnt); } else { throw new NullReferenceException("ExternalSolver not defined"); } } if (rErr < radialErrorThreshold) { if (IsTargetLocation) { var duration = timeStamp.Subtract(targetLocationTS); if (duration.TotalSeconds <= maxIntervalToSpeedEstimationSec) { var prevLocRad = GeoPoint3D.ToRad(targetLocation); double currLocLatRad = Algorithms.Deg2Rad(lat_deg); double currLocLonRad = Algorithms.Deg2Rad(lon_deg); double d_passed_m = 0, course_fwd_rad = 0, course_rev_rad = 0; int its = 0; Algorithms.VincentyInverse(prevLocRad.Latitude, prevLocRad.Longitude, currLocLatRad, currLocLonRad, referenceEllipsoid, Algorithms.VNC_DEF_EPSILON, Algorithms.VNC_DEF_IT_LIMIT, out d_passed_m, out course_fwd_rad, out course_rev_rad, out its); double estTargetCourse = Algorithms.Rad2Deg(course_fwd_rad); double estTargetSpeed = d_passed_m / duration.TotalSeconds; TargetCourseSpeedAndCourseUpdatedHandler.Rise(this, new TargetCourseAndSpeedUpdatedEventArgs(estTargetCourse, estTargetSpeed, timeStamp)); } } targetLocation.Latitude = lat_deg; targetLocation.Longitude = lon_deg; targetLocationTS = timeStamp; TargetLocationUpdatedHandler.Rise(this, new TargetLocationUpdatedEventArgs(targetLocation, rErr, timeStamp)); } else { RadialErrorExeedsThrehsoldEventHandler.Rise(this, new EventArgs()); } }
public void ProcessBasePoints(IEnumerable <T> basePoints, double depth, DateTime timeStamp) { double lat_deg, lon_deg, rErr; int it_Cnt; if (typeof(GeoPoint3DD).IsAssignableFrom(typeof(T))) { UCNLNav.Navigation.TOA_Locate2D(basePoints.Cast <GeoPoint3DD>().ToArray <GeoPoint3DD>(), targetLocation.Latitude, targetLocation.Longitude, depth, Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize, referenceEllipsoid, out lat_deg, out lon_deg, out rErr, out it_Cnt); } else if (typeof(GeoPoint3DT).IsAssignableFrom(typeof(T))) { UCNLNav.Navigation.TDOA_Locate2D(basePoints.Cast <GeoPoint3DT>().ToArray <GeoPoint3DT>(), targetLocation.Latitude, targetLocation.Longitude, depth, Algorithms.NLM_DEF_IT_LIMIT, Algorithms.NLM_DEF_PREC_THRLD, simplexSize, referenceEllipsoid, soundSpeed, out lat_deg, out lon_deg, out rErr, out it_Cnt); } else { if (ExternalSolver != null) { ExternalSolver(basePoints, targetLocation, out lat_deg, out lon_deg, out rErr, out it_Cnt); } else { throw new NullReferenceException("ExternalSolver not defined"); } } if (rErr < radialErrorThreshold) { crsEstimator.AddPoint(new GeoPoint(lat_deg, lon_deg)); if (crsEstimator.IsCourse) { TargetCourseUpdatedHandler.Rise(this, new TargetCourseUpdatedEventArgs(crsEstimator.Course_deg, timeStamp)); } targetLocation.Latitude = lat_deg; targetLocation.Longitude = lon_deg; targetLocationTS = timeStamp; TargetLocationUpdatedHandler.Rise(this, new TargetLocationUpdatedEventArgs(targetLocation, rErr, timeStamp)); TargetLocationUpdatedExHandler.Rise(this, new TargetLocationUpdatedExEventArgs(targetLocation, rErr, crsEstimator.Course_deg, timeStamp)); TBAQuality tbaState = Navigation.GetTBAState(Navigation.GetBasesMaxAngularGapDeg(basePoints, lat_deg, lon_deg)); DOPState dopState = DOPState.Invalid; double gdop = double.NaN, pdop = double.NaN, hdop = double.NaN, vdop = double.NaN, tdop = double.NaN; GeoPoint3D tL = new GeoPoint3D(targetLocation.Latitude, targetLocation.Longitude, depth); if (Navigation.GetDOPs(basePoints, tL, Algorithms.WGS84Ellipsoid, out gdop, out pdop, out hdop, out vdop, out tdop)) { dopState = Navigation.GetDOPState(hdop); } BaseQualityUpdatedHandler.Rise(this, new BaseQualityUpdatedEventArgs(tbaState, gdop, pdop, hdop, vdop, tdop, dopState)); } else { RadialErrorExeedsThrehsoldEventHandler.Rise(this, new EventArgs()); } }