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