C# DilutionOfPrecision类代码示例

        /// <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>
        /// 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>
        /// 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>
 /// 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);
 }

        /// <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>
        /// 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 =
//.........这里部分代码省略.........

        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;
//.........这里部分代码省略.........

 /// <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);

        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 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>
        /// 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);
        }

        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));

//.........这里部分代码省略.........

//.........这里部分代码省略.........
                        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;
            }
        }

     /// <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);
     }

        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,"); 
//.........这里部分代码省略.........