public static Vector <double> ConvertGeoCoordinateToCartesian(double lat, double lon, double heightAboveEllipsoid) { Position3D position3D = new Position3D(new Latitude(lat), new Longitude(lon), new Distance(heightAboveEllipsoid, DistanceUnit.Meters)); CartesianPoint cartesianPoint = position3D.ToCartesianPoint(DotSpatial.Positioning.Ellipsoid.Default); return(Vector <double> .Build.Dense(new[] { cartesianPoint.X.Value, cartesianPoint.Y.Value, cartesianPoint.Z.Value })); }
private SquareMatrix3D H; // observation matrix. internal KalmanSystemState(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision meanDOP, Ellipsoid ellipsoid) : this( gpsPosition, deviceError, meanDOP, meanDOP, 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"></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; }
/// <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; }
/// <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 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) { }
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; }
/// <summary>Converts a 3D position to a NED point, relative to given reference point.</summary> public static NedPoint ToNedPoint(this Position3D position, Position3D reference, Ellipsoid ellipsoid) { CartesianPoint ecef = position.ToCartesianPoint(ellipsoid); return(ecef.ToNedPoint(reference)); }