public static double SpeedOx(this Coordinate coordinate)
{
var point = VincentyEllipsoid.GetPointFromDistance(coordinate.Azimuth, coordinate.Speed, coordinate.Longitude,
coordinate.Latitude);
return(point.X - coordinate.Longitude);
}
public static double AccuracyOy(this Coordinate coordinate)
{
var point = VincentyEllipsoid.GetPointFromDistance(0, coordinate.Accuracy, coordinate.Longitude,
coordinate.Latitude);
return(point.Y - coordinate.Latitude);
}
private double AccelerationOy(Coordinate coordinate)
{
var point = VincentyEllipsoid.GetPointFromDistance(0, Math.Sqrt(AccelerationVariance), coordinate.Longitude,
coordinate.Latitude);
var res = point.Y - coordinate.Latitude;
return(res * res);
}
public void TestConvertation()
{
//тестирование проводилось методом тщательного всматривания в яндекс-карты
var coordinate = new Coordinate()
{
Longitude = 37.512353, Latitude = 55.769474
};
var point = VincentyEllipsoid.GetPointFromDistance(Math.PI / 2, 200, coordinate.Longitude, coordinate.Latitude);
Debugger.Break();
}
private Matrix <double> CalculateMeasureErrorCovariance(Coordinate coordinate)
{
//TODO actual error matrix calculation
var speedx = VincentyEllipsoid.GetPointFromDistance(90, 0.0005, coordinate.Longitude, coordinate.Latitude);
var speedy = VincentyEllipsoid.GetPointFromDistance(0, 0.0005, coordinate.Longitude, coordinate.Latitude);
return(Matrix <double> .Build.DenseOfArray(new[, ]
{
{ coordinate.AccuracyOx() / 50, 0, 0, 0 },
{ 0, coordinate.AccuracyOy() / 50, 0, 0 },
{ 0, 0, (speedx.X - coordinate.Longitude) / 10, 0 },
{ 0, 0, 0, (speedy.Y - coordinate.Latitude) / 10 }
}));
}
