/// <summary> /// Return a filtered Position3D from the specified parameters /// </summary> /// <param name="gpsPosition">The GPS position.</param> /// <param name="deviceError">The device error.</param> /// <param name="horizontalDOP">The horizontal DOP.</param> /// <param name="verticalDOP">The vertical DOP.</param> /// <param name="bearing">The bearing.</param> /// <param name="speed">The speed.</param> /// <returns></returns> public abstract Position3D Filter(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed);
/// <summary>
/// Initializes a new instance of the <see cref="GpgsaSentence"/> class.
/// </summary>
/// <param name="fixMode">The fix mode.</param>
/// <param name="fixMethod">The fix method.</param>
/// <param name="satellites">The satellites.</param>
/// <param name="positionDilutionOfPrecision">The position dilution of precision.</param>
/// <param name="horizontalDilutionOfPrecision">The horizontal dilution of precision.</param>
/// <param name="verticalDilutionOfPrecision">The vertical dilution of precision.</param>
public GpgsaSentence(FixMode fixMode, FixMethod fixMethod, IEnumerable <Satellite> satellites, DilutionOfPrecision positionDilutionOfPrecision, DilutionOfPrecision horizontalDilutionOfPrecision, DilutionOfPrecision verticalDilutionOfPrecision)
{
// Use a string builder to create the sentence text
StringBuilder builder = new(128);
// Append the command word
builder.Append("$GPGSA");
builder.Append(',');
switch (fixMode)
{
case FixMode.Automatic:
builder.Append("A");
break;
default:
builder.Append("M");
break;
}
builder.Append(',');
switch (fixMethod)
{
case FixMethod.Fix2D:
builder.Append("2");
break;
case FixMethod.Fix3D:
builder.Append("3");
break;
default:
builder.Append("1");
break;
}
builder.Append(',');
// A comma-delimited list of satellites involved in a fix. Up to 12 satellites can be serialized.
// This one concerns me, because while the limit is 12, ever more satellites are being launched.
// Should we just serialize everything??
// Get a count of satellites to write, up to 123. We'll scrub the list to ensure only fixed satellites are written
int fixedSatellitesWritten = 0;
foreach (Satellite item in satellites)
{
// Is it fixed?
if (item.IsFixed)
{
// Yes. It cannot have babies
builder.Append(item.PseudorandomNumber.ToString(NmeaCultureInfo));
// Append a comma
builder.Append(",");
// Update the count
fixedSatellitesWritten++;
// If we're at 12, that's the limit. Stop here
if (fixedSatellitesWritten == 12)
{
break;
}
}
}
// If we wrote less than 12 satellites, write commas for the remainder
for (int index = 0; index < 12 - fixedSatellitesWritten; index++)
{
builder.Append(",");
}
// NOTE: Commas have been written at this point
// Position Dilution of Precision
builder.Append(positionDilutionOfPrecision.Value.ToString(NmeaCultureInfo));
builder.Append(",");
// Horizontal Dilution of Precision
builder.Append(horizontalDilutionOfPrecision.Value.ToString(NmeaCultureInfo));
builder.Append(",");
// Vertical Dilution of Precision
builder.Append(verticalDilutionOfPrecision.Value.ToString(NmeaCultureInfo));
// Set this object's sentence
Sentence = builder.ToString();
SetPropertiesFromSentence();
// Finally, append the checksum
AppendChecksum();
}
/// <summary>
/// Called when [sentence changed].
/// </summary>
/// <remarks></remarks>
protected override void OnSentenceChanged()
{
// Process the basic sentence elements
base.OnSentenceChanged();
// Cache the word array
string[] words = Words;
int wordCount = words.Length;
// Do we have enough words to parse the UTC date/time?
if (wordCount >= 2 && words[0].Length != 0 && words[1].Length != 0)
{
#region Parse the UTC time
string utcTimeWord = words[0];
int utcHours = 0;
int utcMinutes = 0;
int utcSeconds = 0;
int utcMilliseconds = 0;
int.TryParse(utcTimeWord.Substring(0, 2), out utcHours);
int.TryParse(utcTimeWord.Substring(2, 2), out utcMinutes);
int.TryParse(utcTimeWord.Substring(4, 2), out utcSeconds);
try
{
utcMilliseconds = Convert.ToInt32(float.Parse(utcTimeWord.Substring(6), NmeaCultureInfo) * 1000, NmeaCultureInfo);
}
catch (Exception) { }
#endregion Parse the UTC time
#region Parse the UTC date
string utcDateWord = words[1];
int utcMonth = 0;
int utcDay = 0;
int utcYear = 0;
int.TryParse(utcDateWord.Substring(0, 2), out utcMonth);
int.TryParse(utcDateWord.Substring(2, 2), out utcDay);
int.TryParse(utcDateWord.Substring(4, 2), out utcYear);
utcYear += 2000;
#endregion Parse the UTC date
#region Build a UTC date/time
_utcDateTime = new DateTime(utcYear, utcMonth, utcDay, utcHours, utcMinutes, utcSeconds, utcMilliseconds, DateTimeKind.Utc);
#endregion Build a UTC date/time
}
else
{
// The UTC date/time is invalid
_utcDateTime = DateTime.MinValue;
}
// Do we have enough data to parse the location?
if (wordCount >= 6 && words[2].Length != 0 && words[3].Length != 0 && words[4].Length != 0 && words[5].Length != 0)
{
#region Parse the latitude
string latitudeWord = words[2];
int latitudeHours = 0;
double latitudeDecimalMinutes = 0.0;
int.TryParse(latitudeWord.Substring(0, 2), out latitudeHours);
double.TryParse(latitudeWord.Substring(2), out latitudeDecimalMinutes);
LatitudeHemisphere latitudeHemisphere =
words[3].Equals("N", StringComparison.OrdinalIgnoreCase) ? LatitudeHemisphere.North : LatitudeHemisphere.South;
#endregion Parse the latitude
#region Parse the longitude
string longitudeWord = words[4];
int longitudeHours = 0;
double longitudeDecimalMinutes = 0.0;
int.TryParse(longitudeWord.Substring(0, 3), out longitudeHours);
double.TryParse(longitudeWord.Substring(3), out longitudeDecimalMinutes);
LongitudeHemisphere longitudeHemisphere =
words[5].Equals("E", StringComparison.OrdinalIgnoreCase) ? LongitudeHemisphere.East : LongitudeHemisphere.West;
#endregion Parse the longitude
#region Build a Position from the latitude and longitude
_position = new Position(
new Latitude(latitudeHours, latitudeDecimalMinutes, latitudeHemisphere),
new Longitude(longitudeHours, longitudeDecimalMinutes, longitudeHemisphere));
#endregion Build a Position from the latitude and longitude
}
// Do we have enough data for the fix quality?
if (wordCount > 7 && words[7].Length != 0)
{
switch (Convert.ToInt32(words[7], NmeaCultureInfo))
{
case 0:
_fixQuality = FixQuality.NoFix;
break;
case 1:
_fixQuality = FixQuality.GpsFix;
break;
case 2:
_fixQuality = FixQuality.DifferentialGpsFix;
break;
case 3:
_fixQuality = FixQuality.PulsePerSecond;
break;
case 4:
_fixQuality = FixQuality.FixedRealTimeKinematic;
break;
case 5:
_fixQuality = FixQuality.FloatRealTimeKinematic;
break;
case 6:
_fixQuality = FixQuality.Estimated;
break;
case 7:
_fixQuality = FixQuality.ManualInput;
break;
case 8:
_fixQuality = FixQuality.Simulated;
break;
default:
_fixQuality = FixQuality.Unknown;
break;
}
}
else
{
// The fix quality is invalid
_fixQuality = FixQuality.Unknown;
}
// Process the fixed satellite count
//_fixedSatelliteCount = wordCount > 8 ? int.Parse(Words[8], NmeaCultureInfo) : 0;
// Process the mean DOP
if (wordCount > 9 && Words[9].Length != 0)
{
try
{
_meanDilutionOfPrecision = new DilutionOfPrecision(float.Parse(Words[9], NmeaCultureInfo));
}
catch (Exception)
{
_meanDilutionOfPrecision = DilutionOfPrecision.Maximum;
}
}
else
{
_meanDilutionOfPrecision = DilutionOfPrecision.Maximum;
}
// Altitude above ellipsoid
if (wordCount > 10 && Words[10].Length != 0)
{
try
{
_altitudeAboveEllipsoid = new Distance(double.Parse(Words[10], NmeaCultureInfo), DistanceUnit.Meters).ToLocalUnitType();
}
catch (Exception)
{
_altitudeAboveEllipsoid = Distance.Empty;
}
}
else
{
_altitudeAboveEllipsoid = Distance.Empty;
}
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which filter is to begin operating.</param>
/// <param name="meanDOP">The mean dilution of precision</param>
public void Initialize(Position3D gpsPosition, DilutionOfPrecision meanDOP)
{
Initialize(gpsPosition, DilutionOfPrecision.CurrentAverageDevicePrecision, meanDOP, meanDOP, Ellipsoid.Default);
}
/// <summary>
/// Called when [sentence changed].
/// </summary>
/// <remarks></remarks>
protected override void OnSentenceChanged()
{
// First, process the basic info for the sentence
base.OnSentenceChanged();
// Cache the sentence words
string[] words = Words;
int wordCount = words.Length;
#region Fix mode
if (wordCount >= 1 && words[0].Length != 0)
{
switch (words[0])
{
case "A":
_fixMode = FixMode.Automatic;
break;
case "M":
_fixMode = FixMode.Manual;
break;
default:
_fixMode = FixMode.Unknown;
break;
}
}
else
{
_fixMode = FixMode.Unknown;
}
#endregion Fix mode
#region Fix method
// Get the fix quality information
if (wordCount >= 2 && words[1].Length != 0)
{
switch (words[1])
{
case "1":
_fixMethod = FixMethod.NoFix;
break;
case "2":
_fixMethod = FixMethod.Fix2D;
break;
case "3":
_fixMethod = FixMethod.Fix3D;
break;
default:
_fixMethod = FixMethod.Unknown;
break;
}
}
else
{
_fixMethod = FixMethod.Unknown;
}
#endregion Fix method
#region Fixed satellites
try
{
if (wordCount >= 3)
{
// The sentence supports up to 12 satellites
_fixedSatellites = new List <Satellite>(12);
// Get each satellite PRN number
int count = wordCount < 14 ? wordCount : 14;
for (int index = 2; index < count; index++)
{
// Is the word empty?
if (words[index].Length != 0)
{
try
{
// No. Add a satellite
_fixedSatellites.Add(
// We'll only have an empty object for now
new Satellite(int.Parse(words[index], NmeaCultureInfo)));
}
catch (Exception) { }
}
}
}
}
catch (System.Exception)
{
}
#endregion Fixed satellites
#region Dilution of Precision
// Set overall dilution of precision
if (wordCount >= 15 && words[14].Length > 1)
{
try
{
_positionDop = new DilutionOfPrecision(float.Parse(words[14], NmeaCultureInfo));
}
catch (Exception)
{
_positionDop = DilutionOfPrecision.Invalid;
}
}
else
{
_positionDop = DilutionOfPrecision.Invalid;
}
// Set horizontal dilution of precision
if (wordCount >= 16 && words[15].Length != 0)
{
try
{
_horizontalDop = new DilutionOfPrecision(float.Parse(words[15], NmeaCultureInfo));
}
catch (Exception)
{
_horizontalDop = DilutionOfPrecision.Invalid;
}
}
else
{
_horizontalDop = DilutionOfPrecision.Invalid;
}
// Set vertical dilution of precision
if (wordCount >= 17 && words[16].Length != 0)
{
try
{
_verticalDop = new DilutionOfPrecision(float.Parse(words[16], NmeaCultureInfo));
}
catch (Exception)
{
_verticalDop = DilutionOfPrecision.Invalid;
}
}
else
{
_verticalDop = DilutionOfPrecision.Invalid;
}
#endregion Dilution of Precision
}
/// <summary>
/// Updates the state.
/// </summary>
/// <param name="currentDOP">The current DOP.</param>
/// <param name="z">The z.</param>
public void UpdateState(DilutionOfPrecision currentDOP, Position3D z)
{
UpdateState(Distance.FromMeters(_deviceError), currentDOP, currentDOP, Azimuth.Empty, Speed.AtRest, z);
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which filter is to begin operating.</param>
/// <param name="deviceError">Distance of the error</param>
/// <param name="meanDOP">The mean dilution of precision</param>
public void Initialize(Position gpsPosition, Distance deviceError, DilutionOfPrecision meanDOP)
{
Initialize(gpsPosition, deviceError, meanDOP, meanDOP, Ellipsoid.Default);
}
/// <summary>
/// Randomize
/// </summary>
/// <param name="minHDOP">The min HDOP.</param>
/// <param name="maxHDOP">The max HDOP.</param>
/// <param name="minVDOP">The min VDOP.</param>
/// <param name="maxVDOP">The max VDOP.</param>
public void Randomize(DilutionOfPrecision minHDOP, DilutionOfPrecision maxHDOP, DilutionOfPrecision minVDOP, DilutionOfPrecision maxVDOP)
{
_minHdop = minHDOP.Value;
_maxHdop = maxHDOP.Value;
_minVdop = minVDOP.Value;
_maxVdop = maxVDOP.Value;
SetRandom(true);
}
/// <summary>
/// Randomize
/// </summary>
/// <param name="seed">The seed.</param>
/// <param name="speedLow">The speed low.</param>
/// <param name="speedHigh">The speed high.</param>
/// <param name="bearingStart">The bearing start.</param>
/// <param name="bearingArc">The bearing arc.</param>
/// <param name="minHDOP">The min HDOP.</param>
/// <param name="maxHDOP">The max HDOP.</param>
/// <param name="minVDOP">The min VDOP.</param>
/// <param name="maxVDOP">The max VDOP.</param>
public void Randomize(Random seed, Speed speedLow, Speed speedHigh, Azimuth bearingStart, Azimuth bearingArc, DilutionOfPrecision minHDOP,
DilutionOfPrecision maxHDOP, DilutionOfPrecision minVDOP, DilutionOfPrecision maxVDOP)
{
Randomize(seed, speedLow, speedHigh, bearingStart, bearingArc);
_minHdop = minHDOP.Value;
_maxHdop = maxHDOP.Value;
_minVdop = minVDOP.Value;
_maxVdop = maxVDOP.Value;
SetRandom(true);
}
/// <summary>
/// Adds a new observation and applies the filter.
/// </summary>
/// <param name="gpsPosition">The new observation to add to the filter.</param>
/// <param name="deviceError">A DeviceError, which does not currently affect position averaging.</param>
/// <param name="horizontalDOP">A horizontal dilution of position, which does not currently affect position averaging.</param>
/// <param name="verticalDOP">A vertical dilution of positoin which does not currently affect position averaging.</param>
/// <param name="bearing">A directional bearing, which does not currently affect position averaging.</param>
/// <param name="speed">A speed, which does not currently affect position averaging.</param>
/// <returns></returns>
/// <remarks>This method updates the FilteredLocation property without consideration for SampleCount.</remarks>
public override Position3D Filter(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed)
{
_samples.Add(gpsPosition);
_sampleTimes.Add(DateTime.Now);
int count = _samples.Count;
int timeCount = _sampleTimes.Count;
int maxCount = 0;
// Only average the number of samples specified in the constructor
while (count > _sampleCount)
{
_samples.RemoveAt(0);
count--;
maxCount++;
}
// Only 2 times are needed, oldest and most recent.
// Try to remove as many as were removed from the sample collection.
while (timeCount > 2 && maxCount > 0)
{
_sampleTimes.RemoveAt(0);
timeCount--;
}
Filter();
return(_filteredPositon);
}
/// <summary>
/// Randomizes the emulation by changing speed and direction
/// </summary>
public override void Randomize()
{
// Randomize the base emulation for speed/bearing
base.Randomize();
_horizontalDOP = new DilutionOfPrecision((float)(Seed.NextDouble() * (_maxHdop - _minHdop) + _minHdop));
_verticalDOP = new DilutionOfPrecision((float)(Seed.NextDouble() * (_maxVdop - _minVdop) + _minVdop));
// Mean is hypotenuse of the (X, Y, Z, n) axes.
_meanDOP = new DilutionOfPrecision((float)Math.Sqrt(Math.Pow(_horizontalDOP.Value, 2) + Math.Pow(_verticalDOP.Value, 2)));
lock (Satellites)
{
if (Satellites.Count == 0)
{
int sats = Seed.Next(4, 12);
//Satellites.Add(new Satellite(32, new Azimuth(225), new Elevation(45), new SignalToNoiseRatio(25)));
Satellites.Add(new Satellite(32, new Azimuth(Seed.Next(360)), new Elevation(Seed.Next(90)), new SignalToNoiseRatio(Seed.Next(50))));
if (sats > 1)
{
Satellites.Add(new Satellite(24, new Azimuth(Seed.Next(360)), new Elevation(Seed.Next(90)), new SignalToNoiseRatio(Seed.Next(50))));
}
if (sats > 2)
{
Satellites.Add(new Satellite(25, new Azimuth(Seed.Next(360)), new Elevation(Seed.Next(90)), new SignalToNoiseRatio(Seed.Next(50))));
}
if (sats > 3)
{
Satellites.Add(new Satellite(26, new Azimuth(Seed.Next(360)), new Elevation(Seed.Next(90)), new SignalToNoiseRatio(Seed.Next(50))));
}
if (sats > 4)
{
Satellites.Add(new Satellite(27, new Azimuth(Seed.Next(360)), new Elevation(Seed.Next(90)), new SignalToNoiseRatio(Seed.Next(50))));
}
if (sats > 5)
{
Satellites.Add(new Satellite(16, new Azimuth(Seed.Next(360)), new Elevation(Seed.Next(90)), new SignalToNoiseRatio(Seed.Next(50))));
}
if (sats > 6)
{
Satellites.Add(new Satellite(14, new Azimuth(Seed.Next(360)), new Elevation(Seed.Next(90)), new SignalToNoiseRatio(Seed.Next(50))));
}
if (sats > 7)
{
Satellites.Add(new Satellite(6, new Azimuth(Seed.Next(360)), new Elevation(Seed.Next(90)), new SignalToNoiseRatio(Seed.Next(50))));
}
if (sats > 8)
{
Satellites.Add(new Satellite(7, new Azimuth(Seed.Next(360)), new Elevation(Seed.Next(90)), new SignalToNoiseRatio(Seed.Next(50))));
}
if (sats > 9)
{
Satellites.Add(new Satellite(4, new Azimuth(Seed.Next(360)), new Elevation(Seed.Next(90)), new SignalToNoiseRatio(Seed.Next(50))));
}
if (sats > 10)
{
Satellites.Add(new Satellite(19, new Azimuth(Seed.Next(360)), new Elevation(Seed.Next(90)), new SignalToNoiseRatio(Seed.Next(50))));
}
if (sats > 11)
{
Satellites.Add(new Satellite(8, new Azimuth(Seed.Next(360)), new Elevation(Seed.Next(90)), new SignalToNoiseRatio(Seed.Next(50))));
}
}
}
SetRandom(true);
}
/// <summary>
/// Adds a new observation and applies the filter.
/// </summary>
/// <param name="gpsPosition">The new observation to add to the filter.</param>
/// <param name="deviceError">A DeviceError, which does not currently affect position averaging.</param>
/// <param name="horizontalDOP">A horizontal dilution of position, which does not currently affect position averaging.</param>
/// <param name="verticalDOP">A vertical dilution of positoin which does not currently affect position averaging.</param>
/// <param name="bearing">A directional bearing, which does not currently affect position averaging.</param>
/// <param name="speed">A speed, which does not currently affect position averaging.</param>
/// <returns></returns>
/// <remarks>This method updates the FilteredLocation property without consideration for SampleCount.</remarks>
public override Position Filter(Position gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed)
{
Position3D pos3D = Filter(new Position3D(gpsPosition), deviceError, horizontalDOP, verticalDOP, bearing, speed);
return(new Position(pos3D.Latitude, pos3D.Longitude));
}
/// <summary>
/// Returns the position
/// </summary>
/// <param name="gpsPosition">The gps Position</param>
/// <param name="currentDOP">The current dilution of precision</param>
/// <returns>A Position structure</returns>
public Position Filter(Position gpsPosition, DilutionOfPrecision currentDOP)
{
return(Filter(gpsPosition, _currentState.DeviceError, currentDOP, currentDOP, Azimuth.Empty, Speed.AtRest));
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which filter is to begin operating.</param>
/// <param name="deviceError">A distance measure of device error</param>
/// <param name="horizontalDOP">The horizontal dilution of precision</param>
/// <param name="verticalDOP">The vertical dilution of precision</param>
/// <param name="ellipsoid">The ellipsoid</param>
public void Initialize(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Ellipsoid ellipsoid)
{
double fail = horizontalDOP.Value * verticalDOP.Value * deviceError.Value;
if (fail == 0 || double.IsNaN(fail) || double.IsInfinity(fail))
{
throw new ArgumentException(
"Parameters deviceError, horizontalDOP and verticalDOP must be greater than zero.");
}
_currentState = new KalmanSystemState(gpsPosition, deviceError, horizontalDOP, verticalDOP, ellipsoid);
}
/// <summary>
/// Returns the 3D position
/// </summary>
/// <param name="gpsPosition">The gps Position</param>
/// <param name="currentDOP">The current dilution of precision</param>
/// <param name="bearing">the directional azimuth</param>
/// <param name="speed">the magnitude of the velocity</param>
/// <returns>A Position3D structure</returns>
public Position3D Filter(Position3D gpsPosition, DilutionOfPrecision currentDOP, Azimuth bearing, Speed speed)
{
return(Filter(gpsPosition, _currentState.DeviceError, currentDOP, currentDOP, bearing, Speed.AtRest));
}
/// <summary>
/// Emulates the error.
/// </summary>
/// <param name="dop">The dop.</param>
/// <returns></returns>
protected virtual Distance EmulateError(DilutionOfPrecision dop)
{
// Calculate the error variance
//return Distance.FromMeters((Seed.NextDouble() * dop.Value) + DilutionOfPrecision.CurrentAverageDevicePrecision.ToMeters().Value); really? isn't that what the estimated precision is for and shouldn't it be +/- the estimated precision range divided by 2 as below
return(Distance.FromMeters(dop.EstimatedPrecision.ToMeters().Value *(Seed.NextDouble() - 0.5)));
}
/// <summary>
/// Return a filtered Position3D from the specified parameters
/// </summary>
/// <param name="gpsPosition">The GPS position.</param>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="bearing">The bearing.</param>
/// <param name="speed">The speed.</param>
/// <returns></returns>
public override Position3D Filter(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed)
{
double fail = horizontalDOP.Value * verticalDOP.Value * deviceError.Value;
if (fail == 0 || double.IsNaN(fail) || double.IsInfinity(fail))
{
throw new ArgumentException(
"Parameters deviceError, horizontalDOP and verticalDOP must be greater than zero.");
}
_currentState.UpdateState(deviceError, horizontalDOP, verticalDOP, bearing, speed, gpsPosition);
return(_currentState.CorrectedLocation());
}
/// <summary>
/// Creates a new instance with the specified DOP measurement.
/// </summary>
/// <param name="dilutionOfPrecision">The dilution of precision.</param>
public DilutionOfPrecisionEventArgs(DilutionOfPrecision dilutionOfPrecision)
{
_dilutionOfPrecision = dilutionOfPrecision;
}
/// <summary>
/// Updates the state.
/// </summary>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="bearing">The bearing.</param>
/// <param name="speed">The speed.</param>
/// <param name="z">The z.</param>
public void UpdateState(Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed, Position3D z)
{
if (_x.IsInvalid)
{
Initialize(z);
return;
}
// More insanity
double fail = horizontalDOP.Value * verticalDOP.Value * deviceError.Value;
if (fail == 0 || double.IsNaN(fail) || double.IsInfinity(fail))
{
throw new ArgumentException(
"Covariance values are invalid. Parameters deviceError, horizontalDOP and verticalDOP must be greater than zero.");
}
_deviceError = deviceError.Value;
_horizontalDOP = horizontalDOP.Value;
_verticalDOP = verticalDOP.Value;
double hCovariance = _deviceError * _horizontalDOP;
double vCovariance = _deviceError * _verticalDOP;
// Setup the observation covariance (measurement error)
_r = new SquareMatrix3D(
hCovariance, 0, 0,
0, hCovariance, 0,
0, 0, vCovariance);
#region Process Noise Estimation
// Get the translation of the last correction
CartesianPoint subX = _x.ToPosition3D(_ellipsoid)
.TranslateTo(bearing, speed.ToDistance(_delay), _ellipsoid)
.ToCartesianPoint();
// Get the vector of the translation and the last observation
// CartesianPoint w = (subX - this.z);
CartesianPoint w =
new CartesianPoint(
Distance.FromMeters(subX.X.Value - _z.X.Value), // Values are in meters
Distance.FromMeters(subX.Y.Value - _z.Y.Value), // Values are in meters
Distance.FromMeters(subX.Z.Value - _z.Z.Value)); // Values are in meters
// Setup the noise covariance (process error)
_q = new SquareMatrix3D(
Math.Abs(w.X.Value), 0, 0,
0, Math.Abs(w.Y.Value), 0,
0, 0, Math.Abs(w.Z.Value));
#endregion Process Noise Estimation
// Update the observation state
_z = z.ToCartesianPoint(_ellipsoid);
#region State vector prediction and covariance
// s.x = s.A*s.x + s.B*s.u;
// this.x = this.A * this.x + this.B * this.u;
CartesianPoint ax = _a.TransformVector(_x);
CartesianPoint bu = _b.TransformVector(_u);
_x =
new CartesianPoint(
Distance.FromMeters(ax.X.Value + bu.X.Value),
Distance.FromMeters(ax.Y.Value + bu.Y.Value),
Distance.FromMeters(ax.Z.Value + bu.Z.Value));
// s.P = s.A * s.P * s.A' + s.Q;
_p = _a * _p * SquareMatrix3D.Transpose(_a) + _q;
#endregion State vector prediction and covariance
#region Kalman gain factor
// K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);
SquareMatrix3D ht = SquareMatrix3D.Transpose(_h);
SquareMatrix3D k = _p * ht * SquareMatrix3D.Invert(_h * _p * ht + _r);
#endregion Kalman gain factor
#region Observational correction
// s.x = s.x + K*(s.z-s.H*s.x);
// this.x = this.x + K * (this.z - this.H * this.x);
CartesianPoint hx = _h.TransformVector(_x);
CartesianPoint zHx = new CartesianPoint(
Distance.FromMeters(_z.X.Value - hx.X.Value),
Distance.FromMeters(_z.Y.Value - hx.Y.Value),
Distance.FromMeters(_z.Z.Value - hx.Z.Value));
CartesianPoint kzHx = k.TransformVector(zHx);
_x =
new CartesianPoint(
Distance.FromMeters(_x.X.Value + kzHx.X.Value),
Distance.FromMeters(_x.Y.Value + kzHx.Y.Value),
Distance.FromMeters(_x.Z.Value + kzHx.Z.Value));
// s.P = s.P - K*s.H*s.P;
_p = _p - k * _h * _p;
#endregion Observational correction
// Bump the state count
_interval++;
// Calculate the average error for the system stste.
_errorState = (_errorState + Math.Sqrt(Math.Pow(hCovariance, 2) + Math.Pow(vCovariance, 2))) * .5f;
// Calculate the interval between samples
DateTime now = DateTime.Now;
_delay = now.Subtract(_lastObservation);
_lastObservation = now;
}
/// <summary>
/// Creates a new sentence
/// </summary>
/// <param name="utcTime">The UTC time.</param>
/// <param name="position">The position.</param>
/// <param name="fixQuality">The fix quality.</param>
/// <param name="trackedSatelliteCount">The tracked satellite count.</param>
/// <param name="horizontalDilutionOfPrecision">The horizontal dilution of precision.</param>
/// <param name="altitude">The altitude.</param>
/// <param name="geoidalSeparation">The geoidal separation.</param>
/// <param name="differentialGpsAge">The differential GPS age.</param>
/// <param name="differentialGpsStationID">The differential GPS station ID.</param>
public GpggaSentence(TimeSpan utcTime, Position position, FixQuality fixQuality, int trackedSatelliteCount,
DilutionOfPrecision horizontalDilutionOfPrecision, Distance altitude, Distance geoidalSeparation,
TimeSpan differentialGpsAge, int differentialGpsStationID)
{
// Use a string builder to create the sentence text
StringBuilder builder = new StringBuilder(128);
#region Append the command word
// Append the command word
builder.Append("$GPGGA");
#endregion Append the command word
// Append a comma
builder.Append(',');
#region Append the UTC time
/* Convert UTC time to a string in the form HHMMSS.SSSS. Any value less than 10 will be
* padded with a zero.
*/
builder.Append(utcTime.Hours.ToString("0#", NmeaCultureInfo));
builder.Append(utcTime.Minutes.ToString("0#", NmeaCultureInfo));
builder.Append(utcTime.Seconds.ToString("0#", NmeaCultureInfo));
builder.Append(".");
builder.Append(utcTime.Milliseconds.ToString("00#", NmeaCultureInfo));
#endregion Append the UTC time
// Append a comma
builder.Append(',');
#region Append the position
// Append latitude in the format HHMM.MMMM.
builder.Append(position.Latitude.ToString(NmeaSentence.LatitudeFormat, NmeaCultureInfo));
// Append Longitude in the format HHHMM.MMMM.
builder.Append(position.Longitude.ToString(NmeaSentence.LongitudeFormat, NmeaCultureInfo));
#endregion Append the position
#region Append fix quality
switch (fixQuality)
{
case FixQuality.NoFix:
builder.Append("0");
break;
case FixQuality.GpsFix:
builder.Append("1");
break;
case FixQuality.DifferentialGpsFix:
builder.Append("2");
break;
case FixQuality.PulsePerSecond:
builder.Append("3");
break;
case FixQuality.FixedRealTimeKinematic:
builder.Append("4");
break;
case FixQuality.FloatRealTimeKinematic:
builder.Append("5");
break;
case FixQuality.Estimated:
builder.Append("6");
break;
case FixQuality.ManualInput:
builder.Append("7");
break;
case FixQuality.Simulated:
builder.Append("8");
break;
}
#endregion Append fix quality
// Append a comma
builder.Append(",");
// Append the tracked (signal strength is > 0) satellite count
builder.Append(trackedSatelliteCount.ToString(NmeaCultureInfo));
// Append a comma
builder.Append(",");
// Append the numerical value of HDOP
builder.Append(horizontalDilutionOfPrecision.Value.ToString(NmeaCultureInfo));
// Append a comma
builder.Append(",");
#region Altitude above sea level
// Append the numerical value in meters
builder.Append(altitude.ToMeters().Value.ToString(NmeaCultureInfo));
// Append a comma, the unit (M = meters), and another comma
builder.Append(", M,");
#endregion Altitude above sea level
#region Geoidal separation
// Append the numerical value in meters
builder.Append(geoidalSeparation.ToMeters().Value.ToString(NmeaCultureInfo));
// Append a comma
builder.Append(", M,");
#endregion Geoidal separation
#region Differential GPS information
// Differnetial signal age in seconds
if (!differentialGpsAge.Equals(TimeSpan.MinValue))
{
builder.Append(differentialGpsAge.TotalSeconds.ToString(NmeaCultureInfo));
}
// Append a comma
builder.Append(",");
// Station ID
if (differentialGpsStationID != -1)
{
builder.Append(differentialGpsStationID.ToString(NmeaCultureInfo));
}
#endregion Differential GPS information
// Set this object's sentence
SetSentence(builder.ToString());
// Finally, append the checksum
AppendChecksum();
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which filter is to begin operating.</param>
/// <param name="deviceError">Distance of the error</param>
/// <param name="horizontalDOP">The horizontal dilution of precision</param>
/// <param name="verticalDOP">The vertical dilution of precision</param>
/// <param name="ellipsoid">The ellipsoid</param>
public void Initialize(Position gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Ellipsoid ellipsoid)
{
_currentState = new KalmanSystemState(new Position3D(gpsPosition), deviceError, horizontalDOP, verticalDOP, ellipsoid);
}
/// <summary>
/// Called when [sentence changed].
/// </summary>
protected override void OnSentenceChanged()
{
// Parse the basic sentence information
base.OnSentenceChanged();
// Cache the words
string[] words = Words;
int wordCount = words.Length;
// Do we have enough data to process the UTC time?
if (wordCount >= 1 && words[0].Length != 0)
{
#region UTC Time
string utcTimeWord = words[0];
int utcHours = int.Parse(utcTimeWord.Substring(0, 2), NmeaCultureInfo);
int utcMinutes = int.Parse(utcTimeWord.Substring(2, 2), NmeaCultureInfo);
int utcSeconds = int.Parse(utcTimeWord.Substring(4, 2), NmeaCultureInfo);
int utcMilliseconds = 0;
if (utcTimeWord.Length > 6)
{
utcMilliseconds = Convert.ToInt32(float.Parse(utcTimeWord.Substring(6), NmeaCultureInfo) * 1000, NmeaCultureInfo);
}
// Build a TimeSpan for this value
_utcTime = new TimeSpan(0, utcHours, utcMinutes, utcSeconds, utcMilliseconds);
#endregion UTC Time
}
else
{
// The UTC time is invalid
_utcTime = TimeSpan.MinValue;
}
// Do we have enough data for locations?
if (wordCount >= 5 && words[1].Length != 0 && words[2].Length != 0 && words[3].Length != 0 && words[4].Length != 0)
{
#region Latitude
string latitudeWord = words[1];
int latitudeHours = int.Parse(latitudeWord.Substring(0, 2), NmeaCultureInfo);
double latitudeDecimalMinutes = double.Parse(latitudeWord.Substring(2), NmeaCultureInfo);
LatitudeHemisphere latitudeHemisphere =
words[2].Equals("N", StringComparison.OrdinalIgnoreCase) ? LatitudeHemisphere.North : LatitudeHemisphere.South;
#endregion Latitude
#region Longitude
string longitudeWord = words[3];
int longitudeHours = int.Parse(longitudeWord.Substring(0, 3), NmeaCultureInfo);
double longitudeDecimalMinutes = double.Parse(longitudeWord.Substring(3), NmeaCultureInfo);
LongitudeHemisphere longitudeHemisphere =
words[4].Equals("E", StringComparison.OrdinalIgnoreCase) ? LongitudeHemisphere.East : LongitudeHemisphere.West;
#endregion Longitude
#region Position
_position = new Position(
new Latitude(latitudeHours, latitudeDecimalMinutes, latitudeHemisphere),
new Longitude(longitudeHours, longitudeDecimalMinutes, longitudeHemisphere));
#endregion Position
}
else
{
_position = Position.Invalid;
}
// Do we have enough data for fix quality?
if (wordCount >= 6 && words[5].Length != 0)
{
#region Fix Quality
switch (int.Parse(words[5], NmeaCultureInfo))
{
case 0:
_fixQuality = FixQuality.NoFix;
break;
case 1:
_fixQuality = FixQuality.GpsFix;
break;
case 2:
_fixQuality = FixQuality.DifferentialGpsFix;
break;
case 3:
_fixQuality = FixQuality.PulsePerSecond;
break;
case 4:
_fixQuality = FixQuality.FixedRealTimeKinematic;
break;
case 5:
_fixQuality = FixQuality.FloatRealTimeKinematic;
break;
case 6:
_fixQuality = FixQuality.Estimated;
break;
case 7:
_fixQuality = FixQuality.ManualInput;
break;
case 8:
_fixQuality = FixQuality.Simulated;
break;
default:
_fixQuality = FixQuality.Unknown;
break;
}
#endregion Fix Quality
}
else
{
// This fix quality is invalid
_fixQuality = FixQuality.Unknown;
}
// Number of satellites in view is skipped. We'll work off of GPGSV data.
if (wordCount >= 7 && words[6].Length != 0)
{
_fixedSatelliteCount = int.Parse(words[6], NmeaCultureInfo);
}
// Is there enough information to process horizontal dilution of precision?
if (wordCount >= 8 && words[7].Length != 0)
{
#region Horizontal Dilution of Precision
try
{
_horizontalDilutionOfPrecision =
new DilutionOfPrecision(float.Parse(words[7], NmeaCultureInfo));
}
catch (ArgumentException)
{
_horizontalDilutionOfPrecision = DilutionOfPrecision.Invalid;
}
#endregion Horizontal Dilution of Precision
}
else
{
// The HDOP is invalid
_horizontalDilutionOfPrecision = DilutionOfPrecision.Invalid;
}
// Is there enough information to process altitude?
if (wordCount >= 9 && words[8].Length != 0)
{
#region Altitude
// Altitude is the 8th NMEA word
_altitude = new Distance(float.Parse(words[8], NmeaCultureInfo), DistanceUnit.Meters);
#endregion Altitude
}
else
{
// The altitude is invalid
_altitude = Distance.Invalid;
}
// Is there enough information to process geoidal separation?
if (wordCount >= 11 && words[10].Length != 0)
{
#region Geoidal Separation
// Parse the geoidal separation
_geoidalSeparation = new Distance(float.Parse(words[10], NmeaCultureInfo), DistanceUnit.Meters);
#endregion Geoidal Separation
}
else
{
// The geoidal separation is invalid
_geoidalSeparation = Distance.Invalid;
}
// Is there enough info to process Differential GPS info?
if (wordCount >= 14 && words[12].Length != 0 && words[13].Length != 0)
{
#region Differential GPS information
_differentialGpsAge = words[12].Length != 0 ? TimeSpan.FromSeconds(float.Parse(words[12], NmeaCultureInfo)) : TimeSpan.MinValue;
if (words[13].Length != 0)
{
_differentialGpsStationID = int.Parse(words[13], NmeaCultureInfo);
}
else
{
_differentialGpsStationID = -1;
}
#endregion Differential GPS information
}
else
{
_differentialGpsStationID = -1;
_differentialGpsAge = TimeSpan.MinValue;
}
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which filter is to begin operating.</param>
/// <param name="deviceError">A distance measure of device error</param>
/// <param name="meanDOP">The mean dilution of precision</param>
/// <param name="ellipsoid">The ellipsoid</param>
public void Initialize(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision meanDOP, Ellipsoid ellipsoid)
{
Initialize(gpsPosition, deviceError, meanDOP, meanDOP, ellipsoid);
}
/// <summary>
/// OnSentanceChanged event handler
/// </summary>
/// <remarks></remarks>
protected override void OnSentenceChanged()
{
base.OnSentenceChanged();
// Cache the sentence words
string[] words = Words;
int wordCount = words.Length;
/*
* Garmin produces several embedded GPS systems. They are easy to setup because Garmin provides a nice utility for uploading configuration data to the GPS. You first load the utility to a PC. Connect the PC to the GPS through one of the serial ports. The utility will check each baud rate until it communicates with the GPS.
*
* The common configuration parameters are output sentences from the GPS unit, the communication baud rate with a host, and the required pulse per second.
*
* Each sentence is preceded with a ‘$’ symbol and ends with a line-feed character. At one sentence per second, the following is out put in four seconds:
*
* $PGRMF, 223, 424798, 041203, 215945, 13, 0000.0000, N, 00000.0000, W, A, 2, 0, 62, 2, 1*3B
* $PGRMF, 223, 424799, 041203, 215946, 13, 00000.0000, N, 00000.0000, W, A, 2, 0, 62, 2, 1*39
* $PGRMF, 223, 424800, 041203, 215947, 13, 00000.0000, N, 00000.0000, W, A, 2, 0, 62, 2, 1*34
* $PGRMF, 223, 424801, 041203, 215948, 13, 00000.0000, N, 00000.0000, W, A, 2, 0, 62, 2, 1*35
*
* The sentence is proprietary to the Garmin GPS Global Positioning System and is translated below.
*
* $PGRMF
* <1>GPS Week Number(0-1023)
* <2>GPS Seconds (0 - 604799)
* <3>UTC Date of position fix, ddmmyy format
* <4>UTC time of position fix, hhmmss format
* <5>GPS leap second count
* <6>Latitude, ddmm.mmmm format (leading zeros transmitted)
* <7>Latitude hemisphere N or S
* <8>Longitude, ddmm.mmmm format (leading zeros transmitted)
* <9>Longitude hemisphere N or S
* <10>Mode M = Manual, A automatic
* <11>Fix type 0 = No Fix, 1 = 2D Fix, 2 = 3D fix
* <12>Speed over ground, 0 to 359 degrees true
* <13>Course over ground, 0 to 9 (rounded to nearest intvalue)
* <14>Time dilution of precision, 0 to 9 (rnded nearest int val)
* <15>Time dilution of precision, 0 to 9 (rnded nearest int val)
* hh <CR><LF>
*/
// TODO: Convert GPS week number/seconds to UTC date/time
// Do we have enough words to parse the fix status?
if (wordCount >= 4 && words[2].Length != 0 && words[3].Length != 0)
{
// Build a UTC date/time object.
_utcDateTime = new DateTime(
int.Parse(words[2].Substring(4, 2), NmeaCultureInfo) + 2000, // Year
int.Parse(words[2].Substring(2, 2), NmeaCultureInfo), // Month
int.Parse(words[2].Substring(0, 2), NmeaCultureInfo), // Day
int.Parse(words[3].Substring(0, 2), NmeaCultureInfo), // Hour
int.Parse(words[3].Substring(2, 2), NmeaCultureInfo), // Minute
int.Parse(words[3].Substring(4, 2), NmeaCultureInfo), // Second
DateTimeKind.Utc);
}
#region Position
// Can we parse the latitude and longitude?
if (wordCount >= 8 && words[5].Length != 0 && words[6].Length != 0 && words[7].Length != 0 && words[8].Length != 0)
{
#region Parse the latitude
string latitudeWord = words[5];
int latitudeHours = int.Parse(latitudeWord.Substring(0, 2), NmeaCultureInfo);
double latitudeDecimalMinutes = double.Parse(latitudeWord.Substring(2), NmeaCultureInfo);
LatitudeHemisphere latitudeHemisphere =
words[6].Equals("N", StringComparison.Ordinal) ? LatitudeHemisphere.North : LatitudeHemisphere.South;
#endregion Parse the latitude
#region Parse the longitude
string longitudeWord = words[7];
int longitudeHours = int.Parse(longitudeWord.Substring(0, 3), NmeaCultureInfo);
double longitudeDecimalMinutes = double.Parse(longitudeWord.Substring(3), NmeaCultureInfo);
LongitudeHemisphere longitudeHemisphere =
words[8].Equals("E", StringComparison.Ordinal) ? LongitudeHemisphere.East : LongitudeHemisphere.West;
#endregion Parse the longitude
#region Build a Position from the latitude and longitude
_position = new Position(
new Latitude(latitudeHours, latitudeDecimalMinutes, latitudeHemisphere),
new Longitude(longitudeHours, longitudeDecimalMinutes, longitudeHemisphere));
#endregion Build a Position from the latitude and longitude
}
else
{
_position = Position.Invalid;
}
#endregion Position
#region Fix Mode
if (wordCount >= 9 && words[9].Length != 0)
{
_fixMode = words[9] == "A" ? FixMode.Automatic : FixMode.Manual;
}
else
{
_fixMode = FixMode.Unknown;
}
#endregion Fix Mode
#region Fix Quality
// Do we have enough data for fix quality?
if (wordCount >= 10 && words[10].Length != 0)
{
switch (int.Parse(words[10], NmeaCultureInfo))
{
case 0:
_fixQuality = FixQuality.NoFix;
break;
case 1:
_fixQuality = FixQuality.GpsFix;
break;
case 2:
_fixQuality = FixQuality.DifferentialGpsFix;
break;
case 3:
_fixQuality = FixQuality.PulsePerSecond;
break;
case 4:
_fixQuality = FixQuality.FixedRealTimeKinematic;
break;
case 5:
_fixQuality = FixQuality.FloatRealTimeKinematic;
break;
case 6:
_fixQuality = FixQuality.Estimated;
break;
case 7:
_fixQuality = FixQuality.ManualInput;
break;
case 8:
_fixQuality = FixQuality.Simulated;
break;
default:
_fixQuality = FixQuality.Unknown;
break;
}
}
else
{
// This fix quality is invalid
_fixQuality = FixQuality.Unknown;
}
#endregion Fix Quality
#region Bearing
// Do we have enough data for fix quality?
if (wordCount >= 13 && words[12].Length != 0)
{
_bearing = new Azimuth(words[12], NmeaCultureInfo);
}
else
{
_bearing = Azimuth.Invalid;
}
#endregion Bearing
#region Speed
// Do we have enough data for fix quality?
if (wordCount >= 12 && words[11].Length != 0)
{
_speed = Speed.FromKilometersPerHour(double.Parse(words[11], NmeaCultureInfo));
}
else
{
_speed = Speed.Invalid;
}
#endregion Speed
#region Position Dilution of Precision
// Do we have enough data for fix quality?
if (wordCount >= 13 && words[13].Length != 0)
{
_positionDop = new DilutionOfPrecision(float.Parse(words[13], NmeaCultureInfo));
}
else
{
_positionDop = DilutionOfPrecision.Invalid;
}
#endregion Position Dilution of Precision
}
