/// <summary>Converts an ECEF point to a NED point, relative to given reference point.</summary>
public static NedPoint ToNedPoint(this CartesianPoint ecef, Position3D reference, Ellipsoid ellipsoid)
{
CartesianPoint refEcef = reference.ToCartesianPoint(ellipsoid);
double sLat = Math.Sin(reference.Latitude.ToRadians().Value);
double sLon = Math.Sin(reference.Longitude.ToRadians().Value);
double cLat = Math.Cos(reference.Latitude.ToRadians().Value);
double cLon = Math.Cos(reference.Longitude.ToRadians().Value);
double r11 = -sLat * cLon;
double r12 = -sLat * sLon;
double r13 = cLat;
double r21 = -sLon;
double r22 = cLon;
double r23 = 0.0;
double r31 = -cLat * cLon;
double r32 = -cLat * sLon;
double r33 = -sLat;
double v1 = ecef.X.Value - refEcef.X.Value;
double v2 = ecef.Y.Value - refEcef.Y.Value;
double v3 = ecef.Z.Value - refEcef.Z.Value;
double n1 = r11 * v1 + r12 * v2 + r13 * v3;
double n2 = r21 * v1 + r22 * v2 + r23 * v3;
double n3 = r31 * v1 + r32 * v2 + r33 * v3;
Distance n = new Distance(n1, refEcef.X.Units);
Distance e = new Distance(n2, refEcef.Y.Units);
Distance d = new Distance(n3, refEcef.Z.Units);
return(new NedPoint(n, e, d));
}
private readonly SquareMatrix3D _h; // observation matrix.
/// <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="meanDOP">The mean DOP.</param>
/// <param name="ellipsoid">The ellipsoid.</param>
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">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>
/// 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>
/// Converts the current instance to a geodetic (latitude/longitude) coordinate using the
/// specified ellipsoid.
/// </summary>
/// <param name="reference">Origin of NED local-frame.</param>
/// <param name="ellipsoid">The ellipsoid.</param>
/// <returns>A <strong>Position</strong> object containing the converted result.</returns>
public Position3D ToPosition3D(Position3D reference, Ellipsoid ellipsoid)
{
if (ellipsoid == null)
{
throw new ArgumentNullException("ellipsoid");
}
CartesianPoint refEcef = reference.ToCartesianPoint(ellipsoid);
double sLat = Math.Sin(reference.Latitude.ToRadians().Value);
double sLon = Math.Sin(reference.Longitude.ToRadians().Value);
double cLat = Math.Cos(reference.Latitude.ToRadians().Value);
double cLon = Math.Cos(reference.Longitude.ToRadians().Value);
double r11 = -sLat * cLon;
double r21 = -sLat * sLon;
double r31 = cLat;
double r12 = -sLon;
double r22 = cLon;
double r32 = 0.0;
double r13 = -cLat * cLon;
double r23 = -cLat * sLon;
double r33 = -sLat;
double n1 = _n.Value;
double n2 = _e.Value;
double n3 = _d.Value;
double v1 = r11 * n1 + r12 * n2 + r13 * n3;
double v2 = r21 * n1 + r22 * n2 + r23 * n3;
double v3 = r31 * n1 + r32 * n2 + r33 * n3;
Distance _x = new Distance(v1, refEcef.X.Units) + refEcef.X;
Distance _y = new Distance(v2, refEcef.Y.Units) + refEcef.Y;
Distance _z = new Distance(v3, refEcef.Z.Units) + refEcef.Z;
#region New code
/*
* % ECEF2LLA - convert earth-centered earth-fixed (ECEF)
* % cartesian coordinates to latitude, longitude,
* % and altitude
* %
* % USAGE:
* % [lat, lon, alt] = ecef2lla(x, y, z)
* %
* % lat = geodetic latitude (radians)
* % lon = longitude (radians)
* % alt = height above WGS84 ellipsoid (m)
* % x = ECEF X-coordinate (m)
* % y = ECEF Y-coordinate (m)
* % z = ECEF Z-coordinate (m)
* %
* % Notes: (1) This function assumes the WGS84 model.
* % (2) Latitude is customary geodetic (not geocentric).
* % (3) Inputs may be scalars, vectors, or matrices of the same
* % size and shape. Outputs will have that same size and shape.
* % (4) Tested but no warranty; use at your own risk.
* % (5) Michael Kleder, April 2006
*
* function [lat, lon, alt] = ecef2lla(x, y, z)
*
* % WGS84 ellipsoid constants:
* a = 6378137;
* e = 8.1819190842622e-2;
*
* % calculations:
* b = sqrt(a^2*(1-e^2));
* ep = sqrt((a^2-b^2)/b^2);
* p = sqrt(x.^2+y.^2);
* th = atan2(a*z, b*p);
* lon = atan2(y, x);
* lat = atan2((z+ep^2.*b.*sin(th).^3), (p-e^2.*a.*cos(th).^3));
* N = a./sqrt(1-e^2.*sin(lat).^2);
* alt = p./cos(lat)-N;
*
* % return lon in range [0, 2*pi)
* lon = mod(lon, 2*pi);
*
* % correct for numerical instability in altitude near exact poles:
* % (after this correction, error is about 2 millimeters, which is about
* % the same as the numerical precision of the overall function)
*
* k=abs(x)<1 & abs(y)<1;
* alt(k) = abs(z(k))-b;
*
* return
*/
double x = _x.ToMeters().Value;
double y = _y.ToMeters().Value;
double z = _z.ToMeters().Value;
//% WGS84 ellipsoid constants:
// a = 6378137;
double a = ellipsoid.EquatorialRadius.ToMeters().Value;
// e = 8.1819190842622e-2;
double e = ellipsoid.Eccentricity;
//% calculations:
// b = sqrt(a^2*(1-e^2));
double b = Math.Sqrt(Math.Pow(a, 2) * (1 - Math.Pow(e, 2)));
// ep = sqrt((a^2-b^2)/b^2);
double ep = Math.Sqrt((Math.Pow(a, 2) - Math.Pow(b, 2)) / Math.Pow(b, 2));
// p = sqrt(x.^2+y.^2);
double p = Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2));
// th = atan2(a*z, b*p);
double th = Math.Atan2(a * z, b * p);
// lon = atan2(y, x);
double lon = Math.Atan2(y, x);
// lat = atan2((z+ep^2.*b.*sin(th).^3), (p-e^2.*a.*cos(th).^3));
double lat = Math.Atan2((z + Math.Pow(ep, 2) * b * Math.Pow(Math.Sin(th), 3)), (p - Math.Pow(e, 2) * a * Math.Pow(Math.Cos(th), 3)));
// N = a./sqrt(1-e^2.*sin(lat).^2);
double n = a / Math.Sqrt(1 - Math.Pow(e, 2) * Math.Pow(Math.Sin(lat), 2));
// alt = p./cos(lat)-N;
double alt = p / Math.Cos(lat) - n;
//% return lon in range [0, 2*pi)
// lon = mod(lon, 2*pi);
lon = lon % (2 * Math.PI);
//% correct for numerical instability in altitude near exact poles:
//% (after this correction, error is about 2 millimeters, which is about
//% the same as the numerical precision of the overall function)
// k=abs(x)<1 & abs(y)<1;
bool k = Math.Abs(x) < 1.0 && Math.Abs(y) < 1.0;
// alt(k) = abs(z(k))-b;
if (k)
{
alt = Math.Abs(z) - b;
}
// return
return(new Position3D(
Distance.FromMeters(alt),
Latitude.FromRadians(lat),
Longitude.FromRadians(lon)));
#endregion New code
}