Esempio n. 1
0
        public static bool GetDOPs(IEnumerable <GeoPoint3D> bPoints, GeoPoint3D tLoc, Ellipsoid el,
                                   out double GDOP, out double PDOP, out double HDOP, out double VDOP, out double TDOP)
        {
            var d_m = BuildDMatrix(bPoints, tLoc, el);

            GDOP = double.NaN;
            PDOP = double.NaN;
            HDOP = double.NaN;
            VDOP = double.NaN;
            TDOP = double.NaN;
            bool result = false;

            if (d_m != null)
            {
                GDOP = Math.Sqrt(d_m[0, 0] + d_m[1, 1] + d_m[2, 2] + d_m[3, 3]);
                PDOP = Math.Sqrt(d_m[0, 0] + d_m[1, 1] + d_m[2, 2]);
                HDOP = Math.Sqrt(d_m[0, 0] + d_m[1, 1]);
                VDOP = Math.Sqrt(d_m[2, 2]);
                TDOP = Math.Sqrt(d_m[3, 3]);

                result = true;
            }

            return(result);
        }
Esempio n. 2
0
        public static Matrix BuildDMatrix(IEnumerable <GeoPoint3D> bPoints, GeoPoint3D tLoc, Ellipsoid el)
        {
            List <MPoint3D> m_bPoints = new List <MPoint3D>();
            double          cLat = Algorithms.Deg2Rad(tLoc.Latitude);
            double          cLon = Algorithms.Deg2Rad(tLoc.Longitude);
            double          d_lat_m, d_lon_m;

            foreach (var point in bPoints)
            {
                Algorithms.GetDeltasByGeopoints(cLat, cLon,
                                                Algorithms.Deg2Rad(point.Latitude), Algorithms.Deg2Rad(point.Longitude),
                                                el,
                                                out d_lat_m, out d_lon_m);

                m_bPoints.Add(new MPoint3D(d_lon_m, d_lat_m, point.Depth));
            }

            Matrix p_m = new Matrix(m_bPoints.Count, 4);

            for (int i = 0; i < m_bPoints.Count; i++)
            {
                double r = Algorithms.Dist3D(m_bPoints[i].X, m_bPoints[i].Y, m_bPoints[i].Z, 0, 0, tLoc.Depth);
                p_m[i, 0] = -m_bPoints[i].X / r;
                p_m[i, 1] = -m_bPoints[i].Y / r;
                p_m[i, 2] = (tLoc.Depth - m_bPoints[i].Z) / r;
                p_m[i, 3] = 1;
            }

            return(Matrix.Inverse_JG(Matrix.Transpose(p_m) * p_m));
        }
Esempio n. 3
0
 public PositioningCore2D(double rErrThreshold, double simplexSize, Ellipsoid refEllipsoid)
 {
     PreviousLocation     = new GeoPoint3D(double.NaN, double.NaN, 0);
     RadialErrorThreshold = rErrThreshold;
     SimplexSize          = simplexSize;
     TargetDepth          = 0;
     referenceEllipsoid   = refEllipsoid;
 }
Esempio n. 4
0
        public PCore2D(double rErrThreshold, double simplexSize, Ellipsoid refEllipsoid, int maxIntToSpeedEstimationSec)
        {
            targetLocation   = new GeoPoint3D(double.NaN, double.NaN, double.NaN);
            targetLocationTS = DateTime.MinValue;

            RadialErrorThreshold            = rErrThreshold;
            SimplexSize                     = simplexSize;
            referenceEllipsoid              = refEllipsoid;
            MaxIntervalToSpeedEstimationSec = maxIntToSpeedEstimationSec;
        }
Esempio n. 5
0
        public PCore2D(double rErrThreshold, double simplexSize, Ellipsoid refEllipsoid, int courseEstimatorFifoSize)
        {
            targetLocation   = new GeoPoint3D(double.NaN, double.NaN, double.NaN);
            targetLocationTS = DateTime.MinValue;

            crsEstimator = new CourseEstimatorLA2D(courseEstimatorFifoSize);

            RadialErrorThreshold = rErrThreshold;
            SimplexSize          = simplexSize;
            referenceEllipsoid   = refEllipsoid;
        }
Esempio n. 6
0
        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());
            }
        }
Esempio n. 7
0
 public TargetLocationUpdatedEventArgs(GeoPoint3D loc, double rerr, DateTime ts)
     : this(loc.Latitude, loc.Longitude, loc.Depth, rerr, ts)
 {
 }
Esempio n. 8
0
        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());
            }
        }
Esempio n. 9
0
 public static GeoPoint3D ToDeg(GeoPoint3D geoPointRad)
 {
     return(new GeoPoint3D(Algorithms.Rad2Deg(geoPointRad.Latitude),
                           Algorithms.Deg2Rad(geoPointRad.Longitude), geoPointRad.Depth));
 }
Esempio n. 10
0
 public static GeoPoint3D ToRad(GeoPoint3D geoPointDeg)
 {
     return(new GeoPoint3D(Algorithms.Deg2Rad(geoPointDeg.Latitude),
                           Algorithms.Deg2Rad(geoPointDeg.Longitude), geoPointDeg.Depth));
 }