当前位置: 首页>>代码示例>>C#>>正文


C# Position3D.ToCartesianPoint方法代码示例

本文整理汇总了C#中Position3D.ToCartesianPoint方法的典型用法代码示例。如果您正苦于以下问题:C# Position3D.ToCartesianPoint方法的具体用法?C# Position3D.ToCartesianPoint怎么用?C# Position3D.ToCartesianPoint使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在Position3D的用法示例。


在下文中一共展示了Position3D.ToCartesianPoint方法的4个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C#代码示例。

示例1: UpdateState

        /// <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 =
//.........这里部分代码省略.........
开发者ID:DIVEROVIEDO,项目名称:DotSpatial,代码行数:101,代码来源:KalmanFilter.cs

示例2: KalmanSystemState

     /// <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)
     { }
开发者ID:DIVEROVIEDO,项目名称:DotSpatial,代码行数:18,代码来源:KalmanFilter.cs

示例3: Initialize

        /// <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;
        }
开发者ID:DIVEROVIEDO,项目名称:DotSpatial,代码行数:19,代码来源:KalmanFilter.cs

示例4: Initialize

        /// <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;
        }
开发者ID:ExRam,项目名称:DotSpatial-PCL,代码行数:19,代码来源:KalmanFilter.cs


注:本文中的Position3D.ToCartesianPoint方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。