예제 #1
0
        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 }
            }));
        }