/// <summary> /// Return filtered position from specified parameters /// </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="bearing">The bearing.</param> /// <param name="speed">The speed.</param> /// <returns></returns> /// <inheritdocs/> public override Position Filter(Position gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed) { Position3D pos3D = Filter(new Position3D(gpsPosition), deviceError, horizontalDOP, verticalDOP, bearing, speed); return(new Position(pos3D.Latitude, pos3D.Longitude)); }
/// <summary> /// Return a filtered Position3D from the specified parameters /// </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="bearing">The bearing.</param> /// <param name="speed">The speed.</param> /// <returns></returns> public abstract Position3D Filter(Position3D gpsPosition, Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed);
/// <summary> /// Initializes the Kalman Filter using an initial observation (position) /// </summary> /// <param name="gpsPosition">The position at which tfilter is to begin opperating.</param> /// <param name="meanDOP">The mean dilution of precision</param> public void Initialize(Position3D gpsPosition, DilutionOfPrecision meanDOP) { Initialize(gpsPosition, DilutionOfPrecision.CurrentAverageDevicePrecision, meanDOP, meanDOP, Ellipsoid.Default); }
/// <summary> /// Initializes the Kalman Filter using an initial observation (position) /// </summary> /// <param name="gpsPosition">The position at which tfilter is to begin opperating.</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 tfilter is to begin opperating.</param> /// <remarks>The results of a Kalman filter are cumulative. Each /// position processed changes the state of the filter, thus making /// the results more accurate with each subsequent position.</remarks> public override void Initialize(Position3D gpsPosition) { Initialize(gpsPosition, DilutionOfPrecision.CurrentAverageDevicePrecision, DilutionOfPrecision.Good, Ellipsoid.Default); }
/// <summary> /// Initializes the Kalman Filter using an initial observation (position) /// </summary> /// <param name="gpsPosition">The position at which tfilter is to begin opperating.</param> /// <param name="deviceError">A distance measure of device error</param> public void Initialize(Position3D gpsPosition, Distance deviceError) { Initialize(gpsPosition, deviceError, DilutionOfPrecision.Good, Ellipsoid.Default); }
/// <summary> /// Updates the state. /// </summary> /// <param name="currentDOP">The current DOP.</param> /// <param name="z">The z.</param> public void UpdateState(DilutionOfPrecision currentDOP, Position3D z) { UpdateState(Distance.FromMeters(_deviceError), currentDOP, currentDOP, Azimuth.Empty, Speed.AtRest, z); }
/// <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 }
/// <summary> /// Returns the 3D position /// </summary> /// <param name="gpsPosition">The gps Position</param> /// <returns>A Position3D sturcture</returns> public override Position3D Filter(Position3D gpsPosition) { return(Filter(gpsPosition, _currentState.DeviceError, _currentState.HorizontalDilutionOfPrecision, _currentState.VerticalDilutionOfPrecision, Azimuth.Empty, Speed.AtRest)); }
/// <summary> /// Returns the 3D position /// </summary> /// <param name="gpsPosition">The gps Position</param> /// <param name="currentDOP">The current dilution of precision</param> /// <param name="bearing">the directional azimuth</param> /// <param name="speed">the magnitude of the velocity</param> /// <returns>A Position3D sturcture</returns> public Position3D Filter(Position3D gpsPosition, DilutionOfPrecision currentDOP, Azimuth bearing, Speed speed) { return(Filter(gpsPosition, _currentState.DeviceError, currentDOP, currentDOP, bearing, Speed.AtRest)); }
/// <summary>Converts an ECEF point to a NED point, relative to given reference point.</summary> public static NedPoint ToNedPoint(this CartesianPoint ecef, Position3D reference) { return(ecef.ToNedPoint(reference, Ellipsoid.Wgs1984)); }
/// <summary> /// Adds a new observation and applies the filter. /// </summary> /// <param name="gpsPosition">The new observation to add to the filter.</param> /// <returns>The filtered position.</returns> /// <remarks>This method updates the FilteredLocation property without consideration for SampleCount.</remarks> public override Position3D Filter(Position3D gpsPosition) { return(Filter(gpsPosition)); }
/// <summary> /// Adds a new observation and applies the filter. /// </summary> /// <param name="gpsPosition">The new observation to add to the filter.</param> /// <returns>The filtered position.</returns> /// <remarks>This method updates the FilteredLocation property without consideration for SampleCount.</remarks> public override Position Filter(Position gpsPosition) { Position3D pos3D = Filter(new Position3D(gpsPosition)); return(new Position(pos3D.Latitude, pos3D.Longitude)); }
/// <summary> /// Initialise the filter from a specified Position3D /// </summary> /// <param name="gpsPosition">The GPS position.</param> public abstract void Initialize(Position3D 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> /// Return a filtered Position3d /// </summary> /// <param name="gpsPosition">The GPS position.</param> /// <returns></returns> public abstract Position3D Filter(Position3D gpsPosition);
/// <summary>Converts the current instance to a geodetic (latitude/longitude) coordinate.</summary> /// <param name="reference">Origin of NED local-frame.</param> /// <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). /// </remarks> public Position3D ToPosition3D(Position3D reference) { return(ToPosition3D(reference, Ellipsoid.Wgs1984)); }