/// <summary>
/// Copies the settings of the NMEA Emulator.
/// </summary>
/// <returns> A new NMEA Emulator with the same settings. </returns>
//public override Emulator Clone()
//{
// // Make a new emulator
// NmeaEmulator emulator = (NmeaEmulator)Clone(new NmeaEmulator());
// emulator._HorizontalDOP = _HorizontalDOP;
// emulator._VerticalDOP = _VerticalDOP;
// emulator._MeanDOP = _MeanDOP;
// emulator._FixMode = _FixMode;
// emulator._FixStatus = _FixStatus;
// emulator._FixMethod = _FixMethod;
// emulator._FixQuality = _FixQuality;
// emulator._MagneticVariation = _MagneticVariation;
// emulator._minHDOP = _minHDOP;
// emulator._maxHDOP = _maxHDOP;
// emulator._minVDOP = _minVDOP;
// emulator._maxVDOP = _maxVDOP;
// return emulator;
//}
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);
}
public void Randomize(
Random seed,
Speed speedLow,
Speed speedHigh,
Azimuth bearingStart,
Azimuth bearingArc,
DilutionOfPrecision minHDOP,
DilutionOfPrecision maxHDOP,
DilutionOfPrecision minVDOP,
DilutionOfPrecision maxVDOP)
{
base.Randomize(seed, speedLow, speedHigh, bearingStart, bearingArc);
_minHDOP = minHDOP.Value;
_maxHDOP = maxHDOP.Value;
_minVDOP = minVDOP.Value;
_maxVDOP = maxVDOP.Value;
SetRandom(true);
}
/// <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>
/// Adds a new observation and applies the filter.
/// </summary>
/// <param name="gpsPosition"> The new observation to add to the filter. </param>
/// <param name="deviceError"> Does not currently affect position averaging. </param>
/// <param name="horizontalDOP"> Does not currently affect position averaging. </param>
/// <param name="verticalDOP"> Does not currently affect position averaging. </param>
/// <param name="bearing"> Does not currently affect position averaging. </param>
/// <param name="speed"> Does not currently affect position averaging. </param>
/// <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)
{
this._samples.Add(gpsPosition);
this._sampleTimes.Add(DateTime.Now);
int count = this._samples.Count;
int timeCount = this._sampleTimes.Count;
int maxCount = 0;
// Only average the number of samples specified in the constructor
while (count > _sampleCount)
{
this._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)
{
this._sampleTimes.RemoveAt(0);
timeCount--;
}
Filter();
return _filteredPositon;
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which tfilter is to begin opperating.</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>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which tfilter is to begin opperating.</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>
/// Initializes a new instance of the <see cref="KalmanSystemState"/> struct.
/// </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="ellipsoid">The ellipsoid.</param>
/// <param name="u">The u.</param>
/// <param name="x">The x.</param>
/// <param name="z">The z.</param>
/// <param name="a">A.</param>
/// <param name="b">The b.</param>
/// <param name="h">The h.</param>
/// <param name="p">The p.</param>
/// <param name="q">The q.</param>
/// <param name="r">The r.</param>
internal KalmanSystemState(
Position3D gpsPosition, Distance deviceError,
DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP,
Ellipsoid ellipsoid,
CartesianPoint u, CartesianPoint x, CartesianPoint z,
SquareMatrix3D a, SquareMatrix3D b, SquareMatrix3D h,
SquareMatrix3D p, SquareMatrix3D q, SquareMatrix3D r)
{
_deviceError = deviceError.IsEmpty ? DilutionOfPrecision.CurrentAverageDevicePrecision.Value : deviceError.Value;
_horizontalDOP = horizontalDOP.IsEmpty ? DilutionOfPrecision.Good.Value : horizontalDOP.Value;
_verticalDOP = verticalDOP.IsEmpty ? DilutionOfPrecision.Good.Value : verticalDOP.Value;
_ellipsoid = ellipsoid ?? Ellipsoid.Default;
double hCovariance = _deviceError * _horizontalDOP;
double vCovariance = _deviceError * _verticalDOP;
_u = u.IsInvalid ? CartesianPoint.Empty : u;
_x = x;
_z = z.IsInvalid ? CartesianPoint.Empty : z;
_a = a ?? new SquareMatrix3D();
_b = b ?? new SquareMatrix3D();
_h = h ?? new SquareMatrix3D();
_p = p ?? new SquareMatrix3D();
_q = q ?? SquareMatrix3D.Default(0);
_r = r ?? new SquareMatrix3D(
hCovariance, 0, 0,
0, hCovariance, 0,
0, 0, vCovariance);
_interval = 0;
_errorState = Math.Sqrt(Math.Pow(hCovariance, 2) + Math.Pow(vCovariance, 2));
_delay = TimeSpan.MaxValue;
_lastObservation = DateTime.MinValue;
if (!gpsPosition.IsEmpty)
Initialize(gpsPosition);
}
/// <summary>
/// Called when [sentence changed].
/// </summary>
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 = 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 = Convert.ToInt32(float.Parse(utcTimeWord.Substring(6), NmeaCultureInfo) * 1000, NmeaCultureInfo);
#endregion Parse the UTC time
#region Parse the UTC date
string utcDateWord = words[1];
int utcMonth = int.Parse(utcDateWord.Substring(0, 2), NmeaCultureInfo);
int utcDay = int.Parse(utcDateWord.Substring(2, 2), NmeaCultureInfo);
int utcYear = int.Parse(utcDateWord.Substring(4, 2), NmeaCultureInfo) + 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 = int.Parse(latitudeWord.Substring(0, 2), NmeaCultureInfo);
double latitudeDecimalMinutes = double.Parse(latitudeWord.Substring(2), NmeaCultureInfo);
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 = int.Parse(longitudeWord.Substring(0, 3), NmeaCultureInfo);
double longitudeDecimalMinutes = double.Parse(longitudeWord.Substring(3), NmeaCultureInfo);
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)
_meanDilutionOfPrecision = new DilutionOfPrecision(float.Parse(Words[9], NmeaCultureInfo));
else
_meanDilutionOfPrecision = DilutionOfPrecision.Maximum;
// Altitude above ellipsoid
if (wordCount > 10 && Words[10].Length != 0)
_altitudeAboveEllipsoid = new Distance(double.Parse(Words[10], NmeaCultureInfo), DistanceUnit.Meters).ToLocalUnitType();
else
_altitudeAboveEllipsoid = Distance.Empty;
}
public void Initialize(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision meanDOP, Ellipsoid ellipsoid)
{
Initialize(gpsPosition, deviceError, meanDOP, meanDOP, ellipsoid);
}
public void Initialize(Position3D gpsPosition, DilutionOfPrecision meanDOP)
{
Initialize(gpsPosition, DilutionOfPrecision.CurrentAverageDevicePrecision, meanDOP, meanDOP, Ellipsoid.Default);
}
public void Initialize(Position gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Ellipsoid ellipsoid)
{
_currentState = new KalmanSystemState(new Position3D(gpsPosition), deviceError, horizontalDOP, verticalDOP, ellipsoid);
}
public void Initialize(Position gpsPosition, Distance deviceError, DilutionOfPrecision meanDOP)
{
Initialize(gpsPosition, deviceError, meanDOP, meanDOP, Ellipsoid.Default);
}
public void UpdateState(Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed, Position3D z)
{
if (this.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.");
}
this._deviceError = deviceError.Value;
this._horizontalDOP = horizontalDOP.Value;
this._verticalDOP = verticalDOP.Value;
double hCovariance = this._deviceError * this._horizontalDOP;
double vCovariance = this._deviceError * this._verticalDOP;
// Setup the observation covariance (measurement error)
this.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 = this.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 - this.z.X.Value), // Values are in meters
Distance.FromMeters(subX.Y.Value - this.z.Y.Value), // Values are in meters
Distance.FromMeters(subX.Z.Value - this.z.Z.Value)); // Values are in meters
// Setup the noise covariance (process error)
this.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
// Update the observation state
this.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 = this.A.TransformVector(x);
CartesianPoint Bu = this.B.TransformVector(u);
this.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;
this.P = this.A * this.P * SquareMatrix3D.Transpose(this.A) + this.Q;
#endregion
#region Kalman gain factor
//K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);
SquareMatrix3D Ht = SquareMatrix3D.Transpose(this.H);
SquareMatrix3D K = this.P * Ht * SquareMatrix3D.Invert(this.H * this.P * Ht + this.R);
#endregion
#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 = this.H.TransformVector(x);
CartesianPoint zHx = new CartesianPoint(
Distance.FromMeters(this.z.X.Value - Hx.X.Value),
Distance.FromMeters(this.z.Y.Value - Hx.Y.Value),
Distance.FromMeters(this.z.Z.Value - Hx.Z.Value));
CartesianPoint kzHx = K.TransformVector(zHx);
this.x =
new CartesianPoint(
Distance.FromMeters(this.x.X.Value + kzHx.X.Value),
Distance.FromMeters(this.x.Y.Value + kzHx.Y.Value),
Distance.FromMeters(this.x.Z.Value + kzHx.Z.Value));
//s.P = s.P - K*s.H*s.P;
this.P = this.P - K * this.H * this.P;
#endregion
// Bump the state count
this._interval++;
// Calculate the average error for the system stste.
this._errorState = (this._errorState + Math.Sqrt(Math.Pow(hCovariance, 2) + Math.Pow(vCovariance, 2))) * .5f;
// Calculate the interval between samples
DateTime now = DateTime.Now;
this._delay = now.Subtract(_lastObservation);
this._lastObservation = now;
}
public void UpdateState(DilutionOfPrecision currentDOP, Position3D z)
{
UpdateState(Distance.FromMeters(_deviceError), currentDOP, currentDOP, Azimuth.Empty, Speed.AtRest, z);
}
protected override void OnSentenceChanged()
{
// First, process the basic info for the sentence
base.OnSentenceChanged();
// Cache the sentence words
string[] words = base.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
#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
#region Fixed satellites
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)
// No. Add a satellite
_FixedSatellites.Add(
// We'll only have an empty object for now
new Satellite(int.Parse(words[index], NmeaCultureInfo)));
}
#endregion
#region Dilution of Precision
// Set overall dilution of precision
if (wordCount >= 15 && words[14].Length != 0)
_PositionDop = new DilutionOfPrecision(float.Parse(words[14], NmeaCultureInfo));
else
_PositionDop = DilutionOfPrecision.Invalid;
// Set horizontal dilution of precision
if (wordCount >= 16 && words[15].Length != 0)
_HorizontalDop = new DilutionOfPrecision(float.Parse(words[15], NmeaCultureInfo));
else
_HorizontalDop = DilutionOfPrecision.Invalid;
// Set vertical dilution of precision
if (wordCount >= 17 && words[16].Length != 0)
_VerticalDop = new DilutionOfPrecision(float.Parse(words[16], NmeaCultureInfo));
else
_VerticalDop = DilutionOfPrecision.Invalid;
#endregion
}
protected override void OnSentenceChanged()
{
// Parse the basic sentence information
base.OnSentenceChanged();
// Cache the words
string[] words = base.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
}
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
#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
#region Position
_Position = new Position(
new Latitude(latitudeHours, latitudeDecimalMinutes, latitudeHemisphere),
new Longitude(longitudeHours, longitudeDecimalMinutes, longitudeHemisphere));
#endregion
}
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
}
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
_HorizontalDilutionOfPrecision =
new DilutionOfPrecision(float.Parse(words[7], NmeaCultureInfo));
#endregion
}
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
}
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
}
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
if (words[12].Length != 0)
_DifferentialGpsAge = TimeSpan.FromSeconds(float.Parse(words[12], NmeaCultureInfo));
else
_DifferentialGpsAge = TimeSpan.MinValue;
if (words[13].Length != 0)
_DifferentialGpsStationID = int.Parse(words[13], NmeaCultureInfo);
else
_DifferentialGpsStationID = -1;
#endregion
}
else
{
_DifferentialGpsStationID = -1;
_DifferentialGpsAge = TimeSpan.MinValue;
}
}
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>
/// 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();
}
public Position Filter(Position gpsPosition, DilutionOfPrecision currentDOP)
{
return(Filter(gpsPosition, _currentState.DeviceError, currentDOP, currentDOP, Azimuth.Empty, Speed.AtRest));
}
/// <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;
}
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>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which tfilter is to begin opperating.</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);
}
public Position3D Filter(Position3D gpsPosition, DilutionOfPrecision currentDOP, Azimuth bearing, Speed speed)
{
return(Filter(gpsPosition, _currentState.DeviceError, currentDOP, currentDOP, bearing, Speed.AtRest));
}
/// <summary>
/// Returns the 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 Position sturcture</returns>
public Position Filter(Position gpsPosition, DilutionOfPrecision currentDOP, Azimuth bearing, Speed speed)
{
return Filter(gpsPosition, _currentState.DeviceError, currentDOP, currentDOP, bearing, speed);
}
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>
/// Adds a new observation and applies the filter.
/// </summary>
/// <param name="gpsPosition"> The new observation to add to the filter. </param>
/// <param name="deviceError"> Does not currently affect position averaging. </param>
/// <param name="horizontalDOP"> Does not currently affect position averaging. </param>
/// <param name="verticalDOP"> Does not currently affect position averaging. </param>
/// <param name="bearing"> Does not currently affect position averaging. </param>
/// <param name="speed"> Does not currently affect position averaging. </param>
/// <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)
{
this._samples.Add(gpsPosition);
this._sampleTimes.Add(DateTime.Now);
int count = this._samples.Count;
int timeCount = this._sampleTimes.Count;
int maxCount = 0;
// Only average the number of samples specified in the constructor
while (count > _sampleCount)
{
this._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)
{
this._sampleTimes.RemoveAt(0);
timeCount--;
}
Filter();
return(_filteredPositon);
}
protected override void OnSentenceChanged()
{
// First, process the basic info for the sentence
base.OnSentenceChanged();
// Cache the sentence words
string[] words = base.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
#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
#region Fixed satellites
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)
{
// No. Add a satellite
_FixedSatellites.Add(
// We'll only have an empty object for now
new Satellite(int.Parse(words[index], NmeaCultureInfo)));
}
}
}
#endregion
#region Dilution of Precision
// Set overall dilution of precision
if (wordCount >= 15 && words[14].Length != 0)
{
_PositionDop = new DilutionOfPrecision(float.Parse(words[14], NmeaCultureInfo));
}
else
{
_PositionDop = DilutionOfPrecision.Invalid;
}
// Set horizontal dilution of precision
if (wordCount >= 16 && words[15].Length != 0)
{
_HorizontalDop = new DilutionOfPrecision(float.Parse(words[15], NmeaCultureInfo));
}
else
{
_HorizontalDop = DilutionOfPrecision.Invalid;
}
// Set vertical dilution of precision
if (wordCount >= 17 && words[16].Length != 0)
{
_VerticalDop = new DilutionOfPrecision(float.Parse(words[16], NmeaCultureInfo));
}
else
{
_VerticalDop = DilutionOfPrecision.Invalid;
}
#endregion
}
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);
}
public GpgsaSentence(FixMode fixMode, FixMethod fixMethod, IList <Satellite> satellites,
DilutionOfPrecision positionDilutionOfPrecision,
DilutionOfPrecision horizontalDilutionOfPrecision,
DilutionOfPrecision verticalDilutionOfPrecision)
{
// Use a string builder to create the sentence text
StringBuilder builder = new StringBuilder(128);
// Append the command word
builder.Append("$GPGSA");
// Append a comma
builder.Append(',');
#region Append the fix method
switch (_FixMode)
{
case FixMode.Automatic:
builder.Append("A");
break;
default:
builder.Append("M");
break;
}
#endregion
// Append a comma
builder.Append(',');
#region Append the fix method
switch (_FixMethod)
{
case FixMethod.Fix2D:
builder.Append("2");
break;
case FixMethod.Fix3D:
builder.Append("3");
break;
default:
builder.Append("1");
break;
}
#endregion
// Append a comma
builder.Append(',');
#region Fixed satellites
/* 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;
for (int index = 0; index < satellites.Count; index++)
{
// Get the satellite
Satellite item = satellites[index];
// 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(",");
}
#endregion
// NOTE: Commas have been written at this point
// Position Dilution of Precision
builder.Append(positionDilutionOfPrecision.Value.ToString(NmeaCultureInfo));
// Append a comma
builder.Append(",");
// Horizontal Dilution of Precision
builder.Append(horizontalDilutionOfPrecision.Value.ToString(NmeaCultureInfo));
// Append a comma
builder.Append(",");
// Vertical Dilution of Precision
builder.Append(verticalDilutionOfPrecision.Value.ToString(NmeaCultureInfo));
// Set this object's sentence
SetSentence(builder.ToString());
// Finally, append the checksum
AppendChecksum();
}
protected override void OnSentenceChanged()
{
// Parse the basic sentence information
base.OnSentenceChanged();
// Cache the words
string[] words = base.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
}
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
#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
#region Position
_Position = new Position(
new Latitude(latitudeHours, latitudeDecimalMinutes, latitudeHemisphere),
new Longitude(longitudeHours, longitudeDecimalMinutes, longitudeHemisphere));
#endregion
}
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
}
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
_HorizontalDilutionOfPrecision =
new DilutionOfPrecision(float.Parse(words[7], NmeaCultureInfo));
#endregion
}
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
}
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
}
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
if (words[12].Length != 0)
{
_DifferentialGpsAge = TimeSpan.FromSeconds(float.Parse(words[12], NmeaCultureInfo));
}
else
{
_DifferentialGpsAge = TimeSpan.MinValue;
}
if (words[13].Length != 0)
{
_DifferentialGpsStationID = int.Parse(words[13], NmeaCultureInfo);
}
else
{
_DifferentialGpsStationID = -1;
}
#endregion
}
else
{
_DifferentialGpsStationID = -1;
_DifferentialGpsAge = TimeSpan.MinValue;
}
}
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 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 a comma
builder.Append(',');
#region Append the position
// Append latitude in the format HHMM.MMMM.
builder.Append(position.Latitude.ToString("HHMM.MMMM,I,", NmeaCultureInfo));
// Append Longitude in the format HHHMM.MMMM.
builder.Append(position.Longitude.ToString("HHHMM.MMMM,I,", NmeaCultureInfo));
#endregion
#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 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
#region Geoidal separation
// Append the numerical value in meters
builder.Append(geoidalSeparation.ToMeters().Value.ToString(NmeaCultureInfo));
// Append a comma
builder.Append(",M,");
#endregion
#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
// Set this object's sentence
SetSentence(builder.ToString());
// Finally, append the checksum
AppendChecksum();
}
public GpgsaSentence(FixMode fixMode, FixMethod fixMethod, IList<Satellite> satellites,
DilutionOfPrecision positionDilutionOfPrecision,
DilutionOfPrecision horizontalDilutionOfPrecision,
DilutionOfPrecision verticalDilutionOfPrecision)
{
// Use a string builder to create the sentence text
StringBuilder builder = new StringBuilder(128);
// Append the command word
builder.Append("$GPGSA");
// Append a comma
builder.Append(',');
#region Append the fix method
switch (_FixMode)
{
case FixMode.Automatic:
builder.Append("A");
break;
default:
builder.Append("M");
break;
}
#endregion
// Append a comma
builder.Append(',');
#region Append the fix method
switch (_FixMethod)
{
case FixMethod.Fix2D:
builder.Append("2");
break;
case FixMethod.Fix3D:
builder.Append("3");
break;
default:
builder.Append("1");
break;
}
#endregion
// Append a comma
builder.Append(',');
#region Fixed satellites
/* 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;
for(int index = 0; index < satellites.Count; index++)
{
// Get the satellite
Satellite item = satellites[index];
// 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(",");
#endregion
// NOTE: Commas have been written at this point
// Position Dilution of Precision
builder.Append(positionDilutionOfPrecision.Value.ToString(NmeaCultureInfo));
// Append a comma
builder.Append(",");
// Horizontal Dilution of Precision
builder.Append(horizontalDilutionOfPrecision.Value.ToString(NmeaCultureInfo));
// Append a comma
builder.Append(",");
// Vertical Dilution of Precision
builder.Append(verticalDilutionOfPrecision.Value.ToString(NmeaCultureInfo));
// Set this object's sentence
SetSentence(builder.ToString());
// Finally, append the checksum
AppendChecksum();
}
public abstract Position3D Filter(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed);
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 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 a comma
builder.Append(',');
#region Append the position
// Append latitude in the format HHMM.MMMM.
builder.Append(position.Latitude.ToString("HHMM.MMMM,I,", NmeaCultureInfo));
// Append Longitude in the format HHHMM.MMMM.
builder.Append(position.Longitude.ToString("HHHMM.MMMM,I,", NmeaCultureInfo));
#endregion
#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 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
#region Geoidal separation
// Append the numerical value in meters
builder.Append(geoidalSeparation.ToMeters().Value.ToString(NmeaCultureInfo));
// Append a comma
builder.Append(",M,");
#endregion
#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
// Set this object's sentence
SetSentence(builder.ToString());
// Finally, append the checksum
AppendChecksum();
}
protected override void OnSentenceChanged()
{
// Process the basic sentence elements
base.OnSentenceChanged();
// Cache the word array
string[] words = base.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 = 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 = Convert.ToInt32(float.Parse(utcTimeWord.Substring(6), NmeaCultureInfo) * 1000, NmeaCultureInfo);
#endregion
#region Parse the UTC date
string utcDateWord = words[1];
int utcMonth = int.Parse(utcDateWord.Substring(0, 2), NmeaCultureInfo);
int utcDay = int.Parse(utcDateWord.Substring(2, 2), NmeaCultureInfo);
int utcYear = int.Parse(utcDateWord.Substring(4, 2), NmeaCultureInfo) + 2000;
#endregion
#region Build a UTC date/time
_UtcDateTime = new DateTime(utcYear, utcMonth, utcDay, utcHours, utcMinutes, utcSeconds, utcMilliseconds, DateTimeKind.Utc);
#endregion
}
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 = int.Parse(latitudeWord.Substring(0, 2), NmeaCultureInfo);
double latitudeDecimalMinutes = double.Parse(latitudeWord.Substring(2), NmeaCultureInfo);
LatitudeHemisphere latitudeHemisphere =
words[3].Equals("N", StringComparison.OrdinalIgnoreCase) ? LatitudeHemisphere.North : LatitudeHemisphere.South;
#endregion
#region Parse the longitude
string longitudeWord = words[4];
int longitudeHours = int.Parse(longitudeWord.Substring(0, 3), NmeaCultureInfo);
double longitudeDecimalMinutes = double.Parse(longitudeWord.Substring(3), NmeaCultureInfo);
LongitudeHemisphere longitudeHemisphere =
words[5].Equals("E", StringComparison.OrdinalIgnoreCase) ? LongitudeHemisphere.East : LongitudeHemisphere.West;
#endregion
#region Build a Position from the latitude and longitude
_Position = new Position(
new Latitude(latitudeHours, latitudeDecimalMinutes, latitudeHemisphere),
new Longitude(longitudeHours, longitudeDecimalMinutes, longitudeHemisphere));
#endregion
}
// 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
if (wordCount > 8)
{
_FixedSatelliteCount = int.Parse(Words[8], NmeaCultureInfo);
}
else
{
_FixedSatelliteCount = 0;
}
// Process the mean DOP
if (wordCount > 9 && Words[9].Length != 0)
{
_MeanDilutionOfPrecision = new DilutionOfPrecision(float.Parse(Words[9], NmeaCultureInfo));
}
else
{
_MeanDilutionOfPrecision = DilutionOfPrecision.Maximum;
}
// Altitude above ellipsoid
if (wordCount > 10 && Words[10].Length != 0)
{
_AltitudeAboveEllipsoid = new Distance(double.Parse(Words[10], NmeaCultureInfo), DistanceUnit.Meters).ToLocalUnitType();
}
else
{
_AltitudeAboveEllipsoid = Distance.Empty;
}
}
/// <summary>
/// OnSentanceChanged event handler
/// </summary>
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
}
/// <summary>
/// Method to calculate all the data required for the graphs.
/// </summary>
private void OnAddReceiverClick(object sender, EventArgs e)
{
#region CreateAndConfigureReceiver
// Let's create the GPSReceiver. The receiver stores many properties and has a defined location. This location
// is the point of reference for visibility calculations.
receiver = new GpsReceiver();
// add receiver to the tree
TreeNode newNode = new TreeNode(Localization.Receiver);
rootNode.Nodes.Add(newNode);
// Easy reference to Earth Central body used to initialize the ElevationAngleAccessConstraint and
// to calculate the Az/El/Range Data.
EarthCentralBody earth = CentralBodiesFacet.GetFromContext().Earth;
// set the receiver properties based on user selections
// The receiver has a receiver FrontEnd that contains the visibility and tracking constraints
// Be sure to convert your angles to Radians!
double minimumAngle = Trig.DegreesToRadians(Double.Parse(MaskAngle.Text));
receiver.ReceiverConstraints.Clear();
receiver.ReceiverConstraints.Add(new ElevationAngleConstraint(earth, minimumAngle));
receiver.NumberOfChannels = (int)NumberOfChannels.Value;
receiver.NoiseModel = new ConstantGpsReceiverNoiseModel(0.8);
// The receiver's methods of reducing the number of visible satellites to the limit imposed by the number of channels
if (BestNSolType.Checked)
{
receiver.ReceiverSolutionType = GpsReceiverSolutionType.BestN;
}
else
{
receiver.ReceiverSolutionType = GpsReceiverSolutionType.AllInView;
}
// create a new location for the receiver by using the Cartographic type from AGI.Foundation.Coordinates
// again, remember to convert from degrees to Radians! (except the height of course)
Cartographic position = new Cartographic(Trig.DegreesToRadians(double.Parse(Longitude.Text)),
Trig.DegreesToRadians(double.Parse(Latitude.Text)),
double.Parse(ReceiverHeight.Text));
// Now create an antenna for the GPS receiver. We specify the location of the antenna by assigning a
// PointCartographic instance to the LocationPoint property. We specify that the antenna should be oriented
// according to the typically-used East-North-Up axes by assigning an instance of AxesEastNorthUp to
// the OrientationAxes property. While the orientation of the antenna won't affect which satellites are visible
// or tracked in this case, it will affect the DOP values. For example, the EDOP value can be found in
// DilutionOfPrecision.X, but only if we've configured the antenna to use East-North-Up axes.
PointCartographic antennaLocationPoint = new PointCartographic(earth, position);
Platform antenna = new Platform
{
LocationPoint = antennaLocationPoint,
OrientationAxes = new AxesEastNorthUp(earth, antennaLocationPoint)
};
receiver.Antenna = antenna;
#endregion
// update the tree to reflect the correct receiver info
newNode.Nodes.Add(new TreeNode(string.Format(Localization.FixedMaskAngle + "= {0:0.00} " + Localization.degrees, MaskAngle.Text)));
newNode.Nodes.Add(new TreeNode(string.Format(Localization.NumberOfChannels + "= {0}", NumberOfChannels.Value)));
newNode.Nodes.Add(new TreeNode(string.Format(Localization.SolutionType + "= {0}", receiver.ReceiverSolutionType == GpsReceiverSolutionType.AllInView ? Localization.AllInView : Localization.BestN)));
newNode.Nodes.Add(new TreeNode(string.Format(Localization.Latitude + "= {0:0.000000} " + Localization.degrees, Latitude.Text)));
newNode.Nodes.Add(new TreeNode(string.Format(Localization.Longitude + "= {0:0.000000} " + Localization.degrees, Longitude.Text)));
newNode.Nodes.Add(new TreeNode(string.Format(Localization.Height + "= {0:0.000} " + Localization.meters, ReceiverHeight.Text)));
rootNode.Expand();
newNode.Expand();
#region CalculateDataForGraphs
// Now, we'll open the almanac
SemAlmanac almanac;
using (Stream stream = openAlmanacDialog.OpenFile())
using (StreamReader reader = new StreamReader(stream))
{
// Read the SEM almanac from an almanac stream reader.
almanac = SemAlmanac.ReadFrom(reader, 1);
}
// Now create a PlatformCollection to hold GpsSatellite object instances. The SemAlmanac method CreateSatellitesFromRecords returns
// just such a collection. We'll use this set of satellites as the set from which we'll try and track. There is a
// GpsSatellite object for each satellite specified in the almanac.
PlatformCollection gpsSatellites = almanac.CreateSatelliteCollection();
// We provide the receiver with the complete set of gpsSatellites to consider for visibility calculations.
// This is usually all SVs defined in the almanac - however you may want to remove SVs that aren't healthy. This can
// be done by creating the gpsSatellites collection above using another version of the CreateSatellitesFromRecords method that
// takes a SatelliteOutageFileReader.
receiver.NavigationSatellites = gpsSatellites;
// Optimization opportunity: Add the following code in a thread. This will help for long duration analyses.
// Now that we have the receiver and location setup, we need to evaluate all the pertinent data.
// using a SatelliteTrackingEvaluator, we can track satellites and using a DOP Evaluator,
// we can calculate DOP at a specified time.
// The receiver's GetSatelliteTrackingEvaluator method will provide a SatelliteTrackingEvaluator for you.
// Similarly, the GetDilutionOfPrecisionEvaluator provides the DOP evaluator.
// We create all evaluators in the same EvaluatorGroup for the best performance.
EvaluatorGroup group = new EvaluatorGroup();
Evaluator <int[]> satTrackingEvaluator = receiver.GetSatelliteTrackingIndexEvaluator(group);
Evaluator <DilutionOfPrecision> dopEvaluator = receiver.GetDilutionOfPrecisionEvaluator(group);
// We also need to create an evaluator to compute Azimuth/Elevation for each of the SVs
MotionEvaluator <AzimuthElevationRange>[] aerEvaluators = new MotionEvaluator <AzimuthElevationRange> [gpsSatellites.Count];
for (int i = 0; i < gpsSatellites.Count; ++i)
{
Platform satellite = receiver.NavigationSatellites[i];
VectorTrueDisplacement vector = new VectorTrueDisplacement(antenna.LocationPoint, satellite.LocationPoint);
aerEvaluators[i] = earth.GetAzimuthElevationRangeEvaluator(vector, group);
}
// First we'll initialize the data structures used to plot the data
for (int i = 0; i < DOPData.Length; i++)
{
// PointPairList is defined in the ZedGraph reference
DOPData[i] = new PointPairList();
}
// We need to know which time standard to use here. If the user has specified that GPS time is to be used
// we need to create the JulianDates with the GlobalPositioningSystemTime standard.
if (GPSTimeRadio.Checked)
{
startjd = new JulianDate(StartTime.Value, TimeStandard.GlobalPositioningSystemTime);
stopjd = new JulianDate(StopTime.Value, TimeStandard.GlobalPositioningSystemTime);
}
else
{
// otherwise, the default time standard is UTC
startjd = new JulianDate(StartTime.Value);
stopjd = new JulianDate(StopTime.Value);
}
// Now we''ll create the variables we'll need for iterating through time.
// The propagator requires a Duration type be used to specify the timestep.
Duration dur = stopjd - startjd;
double timestep = Double.Parse(TimeStep.Text);
// Initialize the progressbar with appropriate values
progressBar1.Maximum = (int)dur.TotalSeconds;
progressBar1.Step = (int)timestep;
// now we'll iterate through time by adding seconds to the start time JulianDate
// creating a new JulianDate 'evaluateTime' each time step.
for (double t = 0; t <= dur.TotalSeconds; t += timestep)
{
JulianDate evaluateTime = startjd.AddSeconds(t);
// The string 'trackedSVs' is the start of a string we'll continue to build through this time
// iteration. It will contain the info we'll need to put in the DOP graph tooltips for the different
// DOP series (VDOP, HDOP, etc.)
String trackedSVs = Localization.Tracked + ": ";
// The evaluator method GetTrackedSatellites will take the current time and the initial list of satellites and
// determine which satellites can be tracked based on the receiver constraints setup earlier. This method
// returns a PlatformCollection object as well (though we'll cast each member of the Collection to a GPSSatellite type)
int[] trackedSatellites = satTrackingEvaluator.Evaluate(evaluateTime);
foreach (int satelliteIndex in trackedSatellites)
{
Platform satellite = receiver.NavigationSatellites[satelliteIndex];
// Now we have access to a Platform object representing a GPS satellite and calculate the azimuth and elevation
// of each. Note that we're just calculating the azimuth and elevation, but could just as easily get the
// range as well.
AzimuthElevationRange aer = aerEvaluators[satelliteIndex].Evaluate(evaluateTime);
// Get the GpsSatelliteExtension attached to the platform. The extension extends a
// platform with GPS-specific information. In this case, we need the
// satellites PRN.
GpsSatelliteExtension extension = satellite.Extensions.GetByType <GpsSatelliteExtension>();
// Create two separate PointPairLists to hold the data stored by Time and Azimuth
PointPairList thisTimePointList, thisAzPointList;
// Before we can arbitrarily create new PointPair Lists, we have to see if the Data Storage structures already contain a list
// for this PRN.
// The variables AzElData_TimeBased and AzElData_AzimuthBased are dictionaries that hold the PointPairLists using the PRN
// as a key. We use this structure to store a large amount of data for every satellite in a single, easy to access, variable.
// if the satellite we're currently looking at already has a list defined in the dictionary, we'll use that one, otherwise
// we'll create a new list
if (AzElData_TimeBased.ContainsKey(extension.PseudoRandomNumber))
{
thisTimePointList = AzElData_TimeBased[extension.PseudoRandomNumber];
AzElData_TimeBased.Remove(extension.PseudoRandomNumber);
}
else
{
thisTimePointList = new PointPairList();
}
if (AzElData_AzimuthBased.ContainsKey(extension.PseudoRandomNumber))
{
thisAzPointList = AzElData_AzimuthBased[extension.PseudoRandomNumber];
AzElData_AzimuthBased.Remove(extension.PseudoRandomNumber);
}
else
{
thisAzPointList = new PointPairList();
}
// Now to get the actual Azimuth and elevation data
// Converting your Radians to degrees here makes the data appear in a more readable format. We also constrain the azimuth
// to be within the interval [0, 2*pi]
double azimuth = Trig.RadiansToDegrees(Trig.ZeroToTwoPi(aer.Azimuth));
double elevation = Trig.RadiansToDegrees(aer.Elevation);
#endregion
// now create the point for the Azimuth based data
PointPair thisAzPoint = new PointPair(azimuth, elevation);
// and add the tooltip (ZedGraph uses the Tag property on a PointPair for the tooltip on that datapoint )
thisAzPoint.Tag = String.Format("PRN {0}, {1}, " + Localization.Az + ": {2:0.000}, " + Localization.El + ": {3:0.000}", extension.PseudoRandomNumber, evaluateTime.ToDateTime().ToString("M/d/yyyy, h:mm tt"), azimuth, elevation);
// and finally add this point to the list
thisAzPointList.Add(thisAzPoint);
// now we'll do the same for the time-based data, instead of adding the Az and El, we'll add the time and El.
// Create a new XDate object to store the time for this point
double txd = (double)new XDate(evaluateTime.ToDateTime());
// add the time and elevation data to this point
PointPair thisTimePoint = new PointPair(txd, Trig.RadiansToDegrees(aer.Elevation));
// Create the tooltip tag
thisTimePoint.Tag = String.Format("PRN {0}, {1}, " + Localization.Az + ": {2:0.000}, " + Localization.El + ": {3:0.000}", extension.PseudoRandomNumber, evaluateTime.ToDateTime().ToString("M/d/yyyy, h:mm tt"), azimuth, elevation);
// finally add this point to the list
thisTimePointList.Add(thisTimePoint);
//Now that this data is all calculated, we'll add the point lists to the correct data structures for the GpsSatellite we're working with
AzElData_TimeBased.Add(extension.PseudoRandomNumber, thisTimePointList);
AzElData_AzimuthBased.Add(extension.PseudoRandomNumber, thisAzPointList);
// now update the 'trackedSVs' string to be used for the DOP data tooltip
// wee need to do this inside the GpsSatellite loop because we need to get the entire list of tracked SVs for this time step.
// we won't use this string until we're out of the loop however.
trackedSVs += extension.PseudoRandomNumber.ToString() + ", ";
}
// now we're out of the GpsSatellite loop, we'll do some string manipulation to get the tooltip for the DOP data.
// (gets rid of the last inserted comma)
string svs = trackedSVs.Substring(0, trackedSVs.LastIndexOf(' ') - 1);
try
{
// Now we use the evaluator to calculate the DilutionOfPrecision for us for this timestep
DilutionOfPrecision dop = dopEvaluator.Evaluate(evaluateTime);
// if the dop object throws an exception, there aren't enough tracked satellites to form a navigation solution (typically < 4 tracked)
// in that case we leave the data for this time unfilled. The graph will then have empty spots for this time.
// Here we create a new PointPair and a new XDate to add to the X-Axis
PointPair pp;
double txd = (double)new XDate(evaluateTime.ToDateTime());
// add the East DOP value and the time to the PointPair and set the tooltip tag property for this series.
pp = new PointPair(txd, dop.X);
pp.Tag = String.Format("{0}\n{1} " + Localization.EDOP + ": {2:0.000}", svs, evaluateTime.ToDateTime().ToString("M/d/yyyy, h:mm tt"), dop.X);
// add the point to the 0th element of the DOPData structure
DOPData[0].Add(pp);
// repeat for North DOP
pp = new PointPair(txd, dop.Y);
pp.Tag = String.Format("{0}\n{1} " + Localization.NDOP + ": {2:0.000}", svs, evaluateTime.ToDateTime().ToString("M/d/yyyy, h:mm tt"), dop.Y);
DOPData[1].Add(pp);
// repeat for the Vertical DOP
pp = new PointPair(txd, dop.Z);
pp.Tag = String.Format("{0}\n{1} " + Localization.VDOP + ": {2:0.000}", svs, evaluateTime.ToDateTime().ToString("M/d/yyyy, h:mm tt"), dop.Z);
DOPData[2].Add(pp);
// repeat for the Horizontal DOP
pp = new PointPair(txd, dop.XY);
pp.Tag = String.Format("{0}\n{1} " + Localization.HDOP + ": {2:0.000}", svs, evaluateTime.ToDateTime().ToString("M/d/yyyy, h:mm tt"), dop.XY);
DOPData[3].Add(pp);
// repeat for the Position DOP
pp = new PointPair(txd, dop.Position);
pp.Tag = String.Format("{0}\n{1} " + Localization.PDOP + ": {2:0.000}", svs, evaluateTime.ToDateTime().ToString("M/d/yyyy, h:mm tt"), dop.Position);
DOPData[4].Add(pp);
// repeat for the Time DOP
pp = new PointPair(txd, dop.Time);
pp.Tag = String.Format("{0}\n{1} " + Localization.TDOP + ": {2:0.000}", svs, evaluateTime.ToDateTime().ToString("M/d/yyyy, h:mm tt"), dop.Time);
DOPData[5].Add(pp);
// repeat for the Geometric DOP
pp = new PointPair(txd, dop.Geometric);
pp.Tag = String.Format("{0}\n{1} " + Localization.GDOP + ": {2:0.000}", svs, evaluateTime.ToDateTime().ToString("M/d/yyyy, h:mm tt"), dop.Geometric);
DOPData[6].Add(pp);
// Notice here that the different DOP values (East, North, etc) were denoted by the dop.X, dop.Y etc. This is because the
// DOP values could be in any coordinate system. In our case, we're in the ENU coordinate system an X represents East, Y
// represents North, Z represents Vertical, XY represents horizontal. You can change the reference frame the DOP is reported in
// but you will then have to understand that the dop.X value corresponds to your X-defined axis and so on.
}
catch
{
// Do Nothing here - we just won't add the data to the data list
}
// update the progress bar - we're done with this time step!
progressBar1.PerformStep();
}
// finally update the graphs
UpdateDopGraph();
updateAzElGraph();
// reset the progress bar
progressBar1.Value = 0;
// and set the appropriate button states
SetControlStates(true);
}
/// <summary>
/// Initializes a new instance of the <see cref="KalmanSystemState"/> struct.
/// </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="ellipsoid">The ellipsoid.</param>
internal KalmanSystemState(
Position3D gpsPosition, Distance deviceError,
DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP,
Ellipsoid ellipsoid)
: this(
gpsPosition, deviceError, verticalDOP, horizontalDOP, ellipsoid,
CartesianPoint.Empty, CartesianPoint.Invalid, gpsPosition.ToCartesianPoint(),
null, null, null,
null, null, null)
{ }
/// <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="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);
}
protected override void OnSentenceChanged()
{
base.OnSentenceChanged();
// Cache the sentence words
string[] words = base.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
#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
#region Build a Position from the latitude and longitude
_Position = new Position(
new Latitude(latitudeHours, latitudeDecimalMinutes, latitudeHemisphere),
new Longitude(longitudeHours, longitudeDecimalMinutes, longitudeHemisphere));
#endregion
}
else
{
_Position = Position.Invalid;
}
#endregion
#region Fix Mode
if (wordCount >= 9 && words[9].Length != 0)
{
if (words[9] == "A")
{
_FixMode = FixMode.Automatic;
}
else
{
_FixMode = FixMode.Manual;
}
}
else
{
_FixMode = FixMode.Unknown;
}
#endregion
#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
#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
#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
#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
}
/// <summary>
/// Kalman Filter with parameters
/// </summary>
/// <param name="initialObservation">The initial observation.</param>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="ellipsoid">The ellipsoid.</param>
public KalmanFilter(
Position3D initialObservation,
Distance deviceError,
DilutionOfPrecision horizontalDOP,
DilutionOfPrecision verticalDOP,
Ellipsoid ellipsoid)
{
_currentState = new KalmanSystemState(
initialObservation,
deviceError,
horizontalDOP,
verticalDOP,
ellipsoid);
}
/// <summary>
/// Copies the settings of the NMEA Emulator.
/// </summary>
/// <returns> A new NMEA Emulator with the same settings. </returns>
//public override Emulator Clone()
//{
// // Make a new emulator
// NmeaEmulator emulator = (NmeaEmulator)Clone(new NmeaEmulator());
// emulator._HorizontalDOP = _HorizontalDOP;
// emulator._VerticalDOP = _VerticalDOP;
// emulator._MeanDOP = _MeanDOP;
// emulator._FixMode = _FixMode;
// emulator._FixStatus = _FixStatus;
// emulator._FixMethod = _FixMethod;
// emulator._FixQuality = _FixQuality;
// emulator._MagneticVariation = _MagneticVariation;
// emulator._minHDOP = _minHDOP;
// emulator._maxHDOP = _maxHDOP;
// emulator._minVDOP = _minVDOP;
// emulator._maxVDOP = _maxVDOP;
// return emulator;
//}
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>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which tfilter is to begin opperating.</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>
/// Does the initialize.
/// </summary>
private void DoInitialize()
{
_altitude = Distance.Invalid;
_altitudeAboveEllipsoid = _altitude;
_bearing = Azimuth.Invalid;
_heading = Azimuth.Invalid;
_fixStatus = FixStatus.Unknown;
_horizontalDop = DilutionOfPrecision.Maximum;
_magneticVariation = Longitude.Invalid;
_position = Position.Invalid;
_speed = Speed.Invalid;
_meanDop = DilutionOfPrecision.Invalid;
_verticalDop = DilutionOfPrecision.Invalid;
if (_satellites != null)
_satellites.Clear();
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which tfilter is to begin opperating.</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>
/// Updates the precision as it relates to latitude, longitude and altitude.
/// </summary>
/// <param name="value">The value.</param>
protected virtual void SetMeanDilutionOfPrecision(DilutionOfPrecision value)
{
// If the new value is the same or it's invalid, ignore it
if (_meanDop.Equals(value) || value.IsInvalid)
return;
// Yes. Update the value
_meanDop = value;
// And notify of the change
if (MeanDilutionOfPrecisionChanged != null)
MeanDilutionOfPrecisionChanged(this, new DilutionOfPrecisionEventArgs(_meanDop));
}
/// <summary>
/// Returns the position
/// </summary>
/// <param name="gpsPosition">The gps Position</param>
/// <param name="currentDOP">The current dilution of precision</param>
/// <returns>A Position sturcture</returns>
public Position Filter(Position gpsPosition, DilutionOfPrecision currentDOP)
{
return Filter(gpsPosition, _currentState.DeviceError, currentDOP, currentDOP, Azimuth.Empty, Speed.AtRest);
}
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>
/// Adds a new observation and applies the filter.
/// </summary>
/// <param name="gpsPosition"> The new observation to add to the filter. </param>
/// <param name="deviceError"> Does not currently affect position averaging. </param>
/// <param name="horizontalDOP"> Does not currently affect position averaging. </param>
/// <param name="verticalDOP"> Does not currently affect position averaging. </param>
/// <param name="bearing"> Does not currently affect position averaging. </param>
/// <param name="speed"> Does not currently affect position averaging. </param>
/// <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);
}
internal KalmanSystemState(
Position3D gpsPosition, Distance deviceError,
DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP,
Ellipsoid ellipsoid,
CartesianPoint u, CartesianPoint x, CartesianPoint z,
SquareMatrix3D A, SquareMatrix3D B, SquareMatrix3D H,
SquareMatrix3D P, SquareMatrix3D Q, SquareMatrix3D R)
{
this._deviceError = deviceError.IsEmpty ? DilutionOfPrecision.CurrentAverageDevicePrecision.Value : deviceError.Value;
this._horizontalDOP = horizontalDOP.IsEmpty ? DilutionOfPrecision.Good.Value : horizontalDOP.Value;
this._verticalDOP = verticalDOP.IsEmpty ? DilutionOfPrecision.Good.Value : verticalDOP.Value;
this._ellipsoid = ellipsoid == null ? Ellipsoid.Default : ellipsoid;
double hCovariance = this._deviceError * this._horizontalDOP;
double vCovariance = this._deviceError * this._verticalDOP;
this.u = u.IsInvalid ? CartesianPoint.Empty : u;
this.x = x;
this.z = z.IsInvalid ? CartesianPoint.Empty : z;
this.A = A == null ? new SquareMatrix3D() : A;
this.B = B == null ? new SquareMatrix3D() : B;
this.H = H == null ? new SquareMatrix3D() : H;
this.P = P == null ? new SquareMatrix3D() : P;
this.Q = Q == null ? SquareMatrix3D.Default(0) : Q;
this.R = R == null ? new SquareMatrix3D(
hCovariance, 0, 0,
0, hCovariance, 0,
0, 0, vCovariance) : R;
this._interval = 0;
this._errorState = Math.Sqrt(Math.Pow(hCovariance, 2) + Math.Pow(vCovariance, 2));
this._delay = TimeSpan.MaxValue;
this._lastObservation = DateTime.MinValue;
if (!gpsPosition.IsEmpty)
Initialize(gpsPosition);
}
protected Emulator(string name)
{
_Name = name;
// Create new buffers for reading and writing
_ReadBuffer = new List<byte>(_DefaultReadBufferSize);
_WriteBuffer = new List<byte>(_DefaultWriteBufferSize);
// Initialize simulated values
_ReadDataAvailableWaitHandle = new ManualResetEvent(false);
_WriteDataAvailableWaitHandle = new ManualResetEvent(false);
_EmulationIntervalWaitHandle = new ManualResetEvent(false);
// Default timeouts for reading and writing
_ReadTimeout = _DefaultReadTimeout;
_WriteTimeout = _DefaultWriteTimeout;
// Simulated values
_seed = new Random();
_UtcDateTime = DateTime.UtcNow;
_CurrentPosition = GeoFramework.Position.Empty;
_Altitude = Distance.FromFeet(1000);
_Route = new List<Position>();
_Satellites = new List<Satellite>();
_FixQuality = FixQuality.GpsFix;
_FixMode = FixMode.Automatic;
_FixMethod = FixMethod.Fix3D;
_FixStatus = FixStatus.Fix;
_HorizontalDop = DilutionOfPrecision.Good;
_VerticalDop = DilutionOfPrecision.Good;
_MeanDop = DilutionOfPrecision.Good;
_Speed = Speed.FromStatuteMilesPerHour(20);
_speedLow = Speed.FromKilometersPerSecond(10).Value;
_speedHigh = Speed.FromKilometersPerSecond(25).Value;
_Bearing = Azimuth.Southwest;
_bearingStart = _seed.NextDouble() * 360;
_bearingArc = 10;
}
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)));
}
