private Vector <double> GetCircleNextPosition(IEnumerable <GeoData> previous, GeoData current, ProviderState state, int pointCount) { if (previous == null) { throw new ArgumentNullException("previous"); } if (current == null) { throw new ArgumentNullException("current"); } if (state == null) { throw new ArgumentNullException("state"); } if (pointCount < 0) { throw new ArgumentOutOfRangeException("pointCount"); } double currentSpeed = current.VelocityData.SpeedKmh; var last = previous.Last(); double distance = CalculateAverageDistanceBySpeed(currentSpeed, this.Frequency); var currentPosition = GpsHelper.ConvertGeoCoordinateToCartesian(last.PositionData.Latitude, last.PositionData.Longitude); var newPosition = PrimitivesFitting.GetCartesianPoint(currentPosition, state.CircleData, pointCount, distance); return(GpsHelper.ConvertCartesianToGeoCoordinate(newPosition[0], newPosition[1], newPosition[2])); }
private void SetCurrentPrimitiveFittingModel(IEnumerable <GeoData> previous, GeoData current, ProviderState state) { if (previous == null) { throw new ArgumentNullException("previous"); } if (current == null) { throw new ArgumentNullException("current"); } if (state == null) { throw new ArgumentNullException("state"); } state.LineData = new Line3DData(previous.Count()); state.CircleData = new Circle3DData(); Matrix <double> gpsCoordinates = ConvertPositionBufferToCartesianMatrix(previous, current, state); if (!PrimitivesFitting.ValidateDispersion(gpsCoordinates, DispersionRatio)) { return; } if (state.LineData.ResidualsNorm > CircleFitting) { state.CircleData = PrimitivesFitting.Ls3DCircle(gpsCoordinates); } else { state.LineData = PrimitivesFitting.Ls3Dline(gpsCoordinates); } SetAltitudeEstimation(previous, current, state); state.LastRealPositionTime = previous.Last().PositionData.Utc; state.EstimateCpt = 0; }