dotMorten/NmeaParser
1
0
Fork 0
mirror of https://github.com/dotMorten/NmeaParser.git synced 2026-01-20 15:40:16 +01:00
Code Issues Actions Packages Projects Releases Wiki Activity

Improve plotview with weighted averages and quality filters

Browse source
This commit is contained in:
Morten Nielsen 2020-09-24 21:45:44 -07:00
parent cd0eda6a02
commit 830b5c25f3
4 changed files with 92 additions and 12 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

9
src/NmeaParser/Gnss/GnssMonitor.cs
View file

@ -29,6 +29,7 @@ namespace NmeaParser.Gnss
private bool m_supportGGaMessages; //If device support GGA, ignore RMC for location
private Dictionary<string, NmeaMessage> m_allMessages { get; } = new Dictionary<string, NmeaMessage>();
private object m_lock = new object();
private bool m_isLearning = true; // Indicates that we still haven't seen a full round of location messages yet
/// <summary>
/// Initializes a new instance of the <see cref="GnssMonitor"/> class.
@ -73,6 +74,10 @@ namespace NmeaParser.Gnss
double lat = 0;
double lon = 0;
List<string> properties = new List<string>();
if (m_isLearning && message is IGeographicLocation && m_allMessages.ContainsKey(message.MessageType))
{
m_isLearning = false; // We've received a full round of messages. Now start report locations
}
lock (m_lock)
{
string msgid = message.MessageType;
@ -89,7 +94,6 @@ namespace NmeaParser.Gnss
if (m_allMessages.ContainsKey("GN" + message.MessageType.Substring(2)))
return;
}
if (message is NmeaParser.Messages.Garmin.Pgrme rme)
{
if (rme.HorizontalError != HorizontalError)
@ -244,7 +248,8 @@ namespace NmeaParser.Gnss
properties.Add(nameof(FixQuality));
IsFixValid = true;
}
LocationChanged?.Invoke(this, EventArgs.Empty);
if (!m_isLearning)
LocationChanged?.Invoke(this, EventArgs.Empty);
}
if (properties.Count > 0)
OnPropertyChanged(properties);

2
src/SampleApp.WinDesktop/MainWindow.xaml.cs
View file

@ -133,7 +133,7 @@ namespace SampleApp.WinDesktop
private void Monitor_LocationChanged(object sender, EventArgs e)
{
var mon = sender as GnssMonitor;
mapplot.AddLocation(mon.Latitude, mon.Longitude, mon.Altitude, mon.FixQuality);
mapplot.AddLocation(mon.Latitude, mon.Longitude, mon.Altitude, mon.FixQuality, mon.Gst?.SigmaLatitudeError ?? mon.HorizontalError, mon.Gst?.SigmaLongitudeError ?? mon.HorizontalError, mon.Gst?.SigmaHeightError ?? mon.VerticalError);
}
private void device_MessageReceived(object sender, NmeaParser.NmeaMessageReceivedEventArgs args)

18
src/SampleApp.WinDesktop/PointPlotView.xaml
View file

@ -62,5 +62,23 @@
<StackPanel HorizontalAlignment="Right" VerticalAlignment="Top" Margin="20">
<TextBlock x:Name="Status" Foreground="White" FontSize="12" />
</StackPanel>
<Border HorizontalAlignment="Left" VerticalAlignment="Top" Margin="20"
BorderBrush="White" BorderThickness="1" CornerRadius="5"
Background="#00ffffff" Padding="10">
<StackPanel >
<TextBlock Text="Settings" Foreground="White" FontSize="12" Margin="0,-20" HorizontalAlignment="Left" Background="Black" />
<TextBlock Text="Filter:" Foreground="White" FontSize="12" />
<ComboBox x:Name="filterSelector" SelectedIndex="0" SelectionChanged="filterSelector_SelectionChanged">
<ComboBoxItem Content="None" />
<ComboBoxItem Content="&gt;= DGPS" />
<ComboBoxItem Content="&gt;= Float RTK" />
<ComboBoxItem Content="= Fixed RTK" />
</ComboBox>
<CheckBox Margin="0,10,0,0" Foreground="White" x:Name="weightedAverage"
IsChecked="True" Checked="WeightedAverageChecked" Unchecked="WeightedAverageChecked"
>Weighted average</CheckBox>
</StackPanel>
</Border>
</Grid>
</UserControl>

75
src/SampleApp.WinDesktop/PointPlotView.xaml.cs
View file

@ -1,4 +1,5 @@
using Esri.ArcGISRuntime.Location;
using Esri.ArcGISRuntime.Tasks.Offline;
using System;
using System.Collections.Generic;
using System.Data.SqlTypes;
@ -61,22 +62,31 @@ namespace SampleApp.WinDesktop
public double Longitude { get ;set; }
public double Z { get ;set; }
public NmeaParser.Messages.Gga.FixQuality Quality { get; set; }
public double LatError { get; set; }
public double LonError { get; set; }
public double ZError { get; set; }
}
public void AddLocation(double latitude, double longitude, double altitude, NmeaParser.Messages.Gga.FixQuality quality)
{
if(quality == NmeaParser.Messages.Gga.FixQuality.Invalid)
public void AddLocation(double latitude, double longitude, double altitude, NmeaParser.Messages.Gga.FixQuality quality,
double latError, double lonError, double zError)
{
if(quality == NmeaParser.Messages.Gga.FixQuality.Invalid)
return;
if (locations.Count == 1 && double.IsNaN(locations[0].LatError) && !double.IsNaN(latError) && locations[0].Latitude == latitude)
locations.RemoveAt(0); //First measurement probably came from RMC
locations.Add(new Point {
Latitude = latitude,
Longitude = longitude,
Z = altitude,
Quality = quality
Quality = quality,
LatError = latError,
LonError = lonError,
ZError = zError
});
UpdatePlot();
}
int filterIndex = 0;
private void UpdatePlot()
{
if (size.Width == 0 || size.Height == 0 || !IsVisible)
@ -91,9 +101,29 @@ namespace SampleApp.WinDesktop
return;
}
var measurements = locations.ToArray(); // Grab copy to avoid threading issues
if(filterIndex > 0)
{
measurements = measurements.Where(m =>
filterIndex == 1 && (m.Quality == NmeaParser.Messages.Gga.FixQuality.DgpsFix || m.Quality == NmeaParser.Messages.Gga.FixQuality.FloatRtk || m.Quality == NmeaParser.Messages.Gga.FixQuality.Rtk) ||
filterIndex == 2 && (m.Quality == NmeaParser.Messages.Gga.FixQuality.FloatRtk || m.Quality == NmeaParser.Messages.Gga.FixQuality.Rtk) ||
filterIndex == 3 && m.Quality == NmeaParser.Messages.Gga.FixQuality.Rtk).ToArray();
}
if(measurements.Length == 0)
{
Dispatcher.Invoke(() =>
{
Status.Text = "";
plot.Source = null;
});
return;
}
var latAvr = measurements.Select(l => l.Latitude).Average();
if (useWeightedAverage && !measurements.Any(m => double.IsNaN(m.LatError)))
latAvr = WeightedAverage(measurements, z => z.Latitude, z => 1 / z.LatError);
var lonAvr = measurements.Select(l => l.Longitude).Average();
if (useWeightedAverage && !measurements.Any(m => double.IsNaN(m.LonError)))
lonAvr = WeightedAverage(measurements, z => z.Longitude, z => 1 / z.LonError);
//List<double> lonDistances = new List<double>(locations.Count);
//List<double> latDistances = new List<double>(locations.Count);
List<double> distances = new List<double>(measurements.Length);
@ -104,18 +134,20 @@ namespace SampleApp.WinDesktop
var dLat = Vincenty.GetDistanceVincenty(latAvr, lonAvr, l.Latitude, lonAvr);
var dLon = Vincenty.GetDistanceVincenty(latAvr, lonAvr, latAvr, l.Longitude);
distances.Add(d);
//latDistances.Add(dLat);
//lonDistances.Add(dLon);
if (latAvr > l.Latitude) dLat = -dLat;
if (lonAvr > l.Longitude) dLon = -dLon;
locations2.Add(new Point() { Latitude = dLat, Longitude= dLon, Quality = l.Quality });
locations2.Add(new Point() { Latitude = dLat, Longitude= dLon, Quality = l.Quality, LatError = l.LatError, LonError = l.LonError, ZError = l.ZError });
}
var latMin = locations2.Select(l => l.Latitude).Min();
var lonMin = locations2.Select(l => l.Longitude).Min();
var latMax = locations2.Select(l => l.Latitude).Max();
var lonMax = locations2.Select(l => l.Longitude).Max();
var latAvr2 = locations2.Select(l => l.Latitude).Average();
if (useWeightedAverage && !measurements.Any(m => double.IsNaN(m.LatError)))
latAvr2 = WeightedAverage(locations2, z => z.Latitude, z => 1/z.LatError);
var lonAvr2 = locations2.Select(l => l.Longitude).Average();
if (useWeightedAverage && !measurements.Any(m => double.IsNaN(m.LonError)))
lonAvr2 = WeightedAverage(locations2, z => z.Longitude, z => 1/z.LonError);
var maxDifLat = Math.Max(latAvr2 - latMin, latMax - latAvr2);
var maxDifLon = Math.Max(lonAvr2 - lonMin, lonMax - lonAvr2);
//var maxDif = Math.Max(maxDifLat, maxDifLon);
@ -183,7 +215,10 @@ namespace SampleApp.WinDesktop
}
var stdDevLat = Math.Sqrt(locations2.Sum(d => (d.Latitude - latAvr2) * (d.Latitude - latAvr2)) / locations2.Count);
var stdDevLon = Math.Sqrt(locations2.Sum(d => (d.Longitude - lonAvr2) * (d.Longitude - lonAvr2)) / locations2.Count);
var zs = measurements.Where(l => !double.IsNaN(l.Z));
var zAvr = measurements.Select(l => l.Z).Where(l => !double.IsNaN(l)).Average();
if (useWeightedAverage && !measurements.Any(m => double.IsNaN(m.ZError)))
zAvr = WeightedAverage(zs, z => z.Z, z => 1 / z.ZError);
var stdDevZ = Math.Sqrt(measurements.Select(l => l.Z).Where(l => !double.IsNaN(l)).Sum(d => (d - zAvr) * (d - zAvr)) / measurements.Select(l => l.Z).Where(l => !double.IsNaN(l)).Count());
var meanH = distances.Average();
var stdDevH = Math.Sqrt(distances.Sum(d => d * d) / distances.Count);
@ -201,7 +236,16 @@ namespace SampleApp.WinDesktop
Status.Text = $"Measurements: {measurements.Length}\nAverage:\n - Latitude: {latAvr.ToString("0.0000000")}\n - Longitude: {lonAvr.ToString("0.0000000")}\n - Elevation: {zAvr.ToString("0.000")}m\nStandard Deviation:\n - Latitude: {stdDevLat.ToString("0.###")}m\n - Longitude: {stdDevLon.ToString("0.###")}m\n - Horizontal: {stdDevH.ToString("0.###")}m\n95% confidence: {marginOfErrorH.ToString("0.###")}m\n - Elevation: {stdDevZ.ToString("0.###")}m";
});
}
public static double WeightedAverage<T>(IEnumerable<T> records, Func<T, double> value, Func<T, double> weight)
{
double weightedValueSum = records.Sum(x => value(x) * weight(x));
double weightSum = records.Sum(x => weight(x));
if (weightSum != 0)
return weightedValueSum / weightSum;
else
throw new DivideByZeroException("Your message here");
}
internal static class Vincenty
{
private const double D2R = 0.01745329251994329576923690768489; //Degrees to radians
@ -306,5 +350,18 @@ namespace SampleApp.WinDesktop
UpdatePlot();
}
}
private void filterSelector_SelectionChanged(object sender, SelectionChangedEventArgs e)
{
filterIndex = filterSelector.SelectedIndex;
UpdatePlot();
}
bool useWeightedAverage = true;
private void WeightedAverageChecked(object sender, RoutedEventArgs e)
{
useWeightedAverage = ((CheckBox)sender).IsChecked.Value;
UpdatePlot();
}
}
}