/// <summary>
/// Toes the cartesian point.
/// </summary>
/// <param name="ellipsoid">The ellipsoid.</param>
/// <returns></returns>
public CartesianPoint ToCartesianPoint(Ellipsoid ellipsoid)
{
return(_position.ToCartesianPoint(ellipsoid, _altitude));
}
/// <summary>
/// Returns a coordinate which has been shifted the specified bearing and distance.
/// </summary>
/// <param name="bearing">The bearing.</param>
/// <param name="distance">The distance.</param>
/// <param name="ellipsoid">The ellipsoid.</param>
/// <returns></returns>
public Position3D TranslateTo(Azimuth bearing, Distance distance, Ellipsoid ellipsoid)
{
return(new Position3D(_altitude, _position.TranslateTo(bearing, distance, ellipsoid)));
}
/// <summary>
/// Converts the current instance to a geodetic (latitude/longitude) coordinate using the specified ellipsoid.
/// </summary>
/// <returns>A <strong>Position</strong> object containing the converted result.</returns>
/// <remarks>The conversion formula will convert the Cartesian coordinate to
/// latitude and longitude using the WGS1984 ellipsoid (the default ellipsoid for
/// GPS coordinates). The resulting three-dimensional coordinate is accurate to within two millimeters
/// (2 mm).</remarks>
public Position3D ToPosition3D(Ellipsoid ellipsoid)
{
if (ellipsoid == null)
{
throw new ArgumentNullException("ellipsoid");
}
#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
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which filter is to begin operating.</param>
/// <param name="deviceError">A distance measure of device error</param>
/// <param name="horizontalDOP">The horizontal dilution of precision</param>
/// <param name="verticalDOP">The vertical dilution of precision</param>
/// <param name="ellipsoid">The ellipsoid</param>
public void Initialize(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Ellipsoid ellipsoid)
{
double fail = horizontalDOP.Value * verticalDOP.Value * deviceError.Value;
if (fail == 0 || double.IsNaN(fail) || double.IsInfinity(fail))
{
throw new ArgumentException(
"Parameters deviceError, horizontalDOP and verticalDOP must be greater than zero.");
}
_currentState = new KalmanSystemState(gpsPosition, deviceError, horizontalDOP, verticalDOP, ellipsoid);
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which filter is to begin operating.</param>
/// <param name="deviceError">A distance measure of device error</param>
/// <param name="meanDOP">The mean dilution of precision</param>
/// <param name="ellipsoid">The ellipsoid</param>
public void Initialize(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision meanDOP, Ellipsoid ellipsoid)
{
Initialize(gpsPosition, deviceError, meanDOP, meanDOP, ellipsoid);
}
/// <summary>
/// Initializes the Kalman Filter using an initial observation (position)
/// </summary>
/// <param name="gpsPosition">The position at which filter is to begin operating.</param>
/// <param name="deviceError">Distance of the error</param>
/// <param name="horizontalDOP">The horizontal dilution of precision</param>
/// <param name="verticalDOP">The vertical dilution of precision</param>
/// <param name="ellipsoid">The ellipsoid</param>
public void Initialize(Position gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Ellipsoid ellipsoid)
{
_currentState = new KalmanSystemState(new Position3D(gpsPosition), deviceError, horizontalDOP, verticalDOP, ellipsoid);
}
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)
{
}
