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