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