C# Position3D.ToCartesianPoint方法代码示例

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

     /// <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>
        /// Initializes the state to the supplied observation.
        /// </summary>
        /// <param name="z">The z.</param>
        public void Initialize(Position3D z)
        {
            SquareMatrix3D hi = SquareMatrix3D.Invert(_h);

            _z = z.ToCartesianPoint(_ellipsoid);

            //s.x = inv(s.H)*s.z;
            _x = hi * _z;

            //s.P = inv(s.H)*s.R*inv(s.H');
            _p = hi * _r * SquareMatrix3D.Invert(SquareMatrix3D.Transpose(_h));

            _lastObservation = DateTime.Now;
            _delay = TimeSpan.Zero;
        }

        /// <summary>
        /// Initializes the state to the supplied observation.
        /// </summary>
        /// <param name="z"></param>
        public void Initialize(Position3D z)
        {
            SquareMatrix3D Hi = SquareMatrix3D.Invert(this.H);

            this.z = z.ToCartesianPoint(_ellipsoid);

            //s.x = inv(s.H)*s.z;
            this.x = Hi * this.z;

            //s.P = inv(s.H)*s.R*inv(s.H'); 
            this.P = Hi * this.R * SquareMatrix3D.Invert(SquareMatrix3D.Transpose(this.H));
        
            _lastObservation = DateTime.Now;
            _delay = TimeSpan.Zero;
        }