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;
}
