Example #1
0
        /// <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;
        }
Example #2
0
        /// <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;
        }