Add files via upload

ucnl · Feb 24, 2021 · ab45d89 · ab45d89
1 parent c5f5817
commit ab45d89
Show file tree

Hide file tree

Showing 3 changed files with 119 additions and 1 deletion.
diff --git a/CSharp/UCNLNav/PCore2D.cs b/CSharp/UCNLNav/PCore2D.cs
@@ -4,6 +4,8 @@
 
 namespace UCNLNav
 {
+    #region Custom EventArgs
+
     public class TargetLocationUpdatedEventArgs : EventArgs
     {
         public DateTime TimeStamp { get; private set; }
@@ -82,6 +84,8 @@ public BaseQualityUpdatedEventArgs(TBAQuality tbaState, double gdop, double pdop
         #endregion
     }
 
+    #endregion
+
     public class PCore2D<T> where T : GeoPoint3D
     {
         #region Properties
@@ -247,7 +251,9 @@ public void ProcessBasePoints(IEnumerable<T> basePoints, double depth, DateTime
 
                 DOPState dopState = DOPState.Invalid;
                 double gdop = double.NaN, pdop = double.NaN, hdop = double.NaN, vdop = double.NaN, tdop = double.NaN;
-                if (Navigation.GetDOPs(basePoints, targetLocation, Algorithms.WGS84Ellipsoid, out gdop, out pdop, out hdop, out vdop, out tdop))
+
+                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);
                 }

diff --git a/CSharp/UCNLNav/TrackFilter.cs b/CSharp/UCNLNav/TrackFilter.cs
@@ -0,0 +1,111 @@
+using System;
+using System.Collections.Generic;
+
+namespace UCNLNav
+{
+    public class TrackFilter
+    {
+        #region Properties
+
+        public int FilterSize { get; private set; }
+
+        List<MPoint> points;
+
+        double anchor_lat_rad = double.NaN;
+        double anchor_lon_rad = double.NaN;
+
+        double max_dist_m_to_reset = 3000;
+
+        double prev_lat_rad = double.NaN;
+        double prev_lon_rad = double.NaN;
+
+        #endregion
+
+        #region Constructor
+
+        public TrackFilter(int filterSize)
+            : this(filterSize, 3000)
+        {
+        }
+
+        public TrackFilter(int filterSize, double reset_delta_m)
+        {
+            if (reset_delta_m <= 0)
+                throw new ArgumentOutOfRangeException("reset_delta_m should be greater than zero");
+
+            if (filterSize <= 0)
+                throw new ArgumentOutOfRangeException("filterSize should be greater than zero");
+
+            max_dist_m_to_reset = reset_delta_m;
+            FilterSize = filterSize;
+            points = new List<MPoint>(filterSize);
+        }
+
+        #endregion
+
+        #region Methods
+
+        public void Reset()
+        {
+            anchor_lat_rad = double.NaN;
+            anchor_lon_rad = double.NaN;
+            points.Clear();
+
+        }
+
+        public GeoPoint Filter(double lat_deg, double lon_deg)
+        {
+            if (points.Count == 0)
+            {
+                anchor_lat_rad = Algorithms.Deg2Rad(lat_deg);
+                anchor_lon_rad = Algorithms.Deg2Rad(lon_deg);
+                points.Add(new MPoint(0, 0));
+                prev_lat_rad = anchor_lat_rad;
+                prev_lon_rad = anchor_lon_rad;
+                return new GeoPoint(lat_deg, lon_deg);
+            }
+            else
+            {                
+                double lat_rad = Algorithms.Deg2Rad(lat_deg);
+                double lon_rad = Algorithms.Deg2Rad(lon_deg);
+
+                if (Algorithms.HaversineInverse(prev_lat_rad, prev_lon_rad, lat_rad, lon_rad, Algorithms.WGS84Ellipsoid.MajorSemiAxis_m) >= max_dist_m_to_reset)
+                {
+                    Reset();
+                    return new GeoPoint(lat_deg, lon_deg);
+                }
+
+                if (points.Count >= FilterSize)
+                    points.RemoveAt(0);
+
+                double deltay = 0, deltax = 0;
+                Algorithms.GetDeltasByGeopoints_WGS84(anchor_lat_rad, anchor_lon_rad,
+                    lat_rad, lon_rad,
+                    out deltay, out deltax);
+
+                points.Add(new MPoint(deltax, deltay));
+
+                double meanx = 0.0, meany = 0.0;
+                for (int i = 0; i < points.Count; i++)
+                {
+                    meanx += points[i].X * (i + 1);
+                    meany += points[i].Y * (i + 1);
+                }
+
+                double fWeight = (points.Count + points.Count * points.Count) / 2.0;
+                meanx /= fWeight;
+                meany /= fWeight;
+
+
+                Algorithms.GeopointOffsetByDeltas_WGS84(anchor_lat_rad, anchor_lon_rad, meany, meanx, out lat_rad, out lon_rad);
+
+                prev_lat_rad = lat_rad;
+                prev_lon_rad = lon_rad;
+
+                return new GeoPoint(Algorithms.Rad2Deg(lat_rad), Algorithms.Rad2Deg(lon_rad));
+            }
+        }
+
+        #endregion
+    }   
+}
diff --git a/CSharp/UCNLNav/UCNLNav.csproj b/CSharp/UCNLNav/UCNLNav.csproj
@@ -44,6 +44,7 @@
     <Compile Include="PCore2D.cs" />
     <Compile Include="PositioningCore2D.cs" />
     <Compile Include="Properties\AssemblyInfo.cs" />
+    <Compile Include="TrackFilter.cs" />
     <Compile Include="Utilities.cs" />
     <Compile Include="VLBL\IVLBLMeasurement.cs" />
     <Compile Include="VLBL\VLBLCore.cs" />