Beispiel #1
0
        public static void writeVectorToFile(MathNet.Numerics.LinearAlgebra.Double.DenseVector vector, string file, System.Windows.Forms.RichTextBox tb1, MainForm frm)
        {
            System.IO.Stream       fileStream = new System.IO.FileStream(file, System.IO.FileMode.Create);
            System.IO.BinaryWriter bw         = new System.IO.BinaryWriter(fileStream, System.Text.Encoding.Default);
            double[]             arr          = vector.Values;
            System.IO.TextWriter tr           = new System.IO.StreamWriter(file + ".metainfo");
            tr.WriteLine("Items count");
            tr.WriteLine(vector.Count);
            tr.Flush();
            tr.Close();
            int len = arr.Length;

            vector = null;
            int index = 0;

            byte[] bytes = new byte[arr.Length * 8];
            foreach (double item in arr)
            {
                BitConverter.GetBytes(item).CopyTo(bytes, index);
                if (index / 8 / 500D == Math.Round(index / 8 / 500D))
                {
                    tb1.BeginInvoke(new MainForm.setProgressDel(frm.addVal), new object[] { (int)index / 8, len, "Записано" });
                }
                index += 8;
            }
            bw.Write(bytes);
            bw.Flush();
            tb1.BeginInvoke(new MainForm.setProgressDel(frm.addVal), new object[] { (int)0, len, "" });
            bw.Flush();
            bw.Close();
            fileStream.Close();
        }
Beispiel #2
0
        private void Simulate(int expiry, int tenor)
        {
            var accumulateSum = new DenseVector[tenor + 1];

            for (int i = 0; i < tenor + 1; i++)
            {
                accumulateSum[i] = new DenseVector(_param.NumberOfFactors);
            }
            for (int i = 0; i < expiry; i++)
            {
                var bm = BrownianMotion.BMStep(0.25, _param.NumberOfFactors);
                accumulateSum[i].Clear();
                for (int j = i + 1; j < tenor + 1; j++)
                {
                    double temp = System.Math.Max(0, System.Math.Min(1, _v[i][j] / _discount[i][j]));
                    accumulateSum[j] = accumulateSum[j - 1] + temp * _xi[i].RowD(j - i - 1);//accumulateSum[j - 1] + temp * _xi[i][j - i - 1, Range.All];
                }
                for (int j = tenor; j > i; j--)
                {
                    var vol = _xi[i].RowD(j - i - 1) - accumulateSum[j]; //_xi[i][j - i - 1, Range.All] - accumulateSum[j];
                    _v[i + 1][j] = _v[i][j] * System.Math.Exp(vol * bm - 0.5 * (vol * vol) * 0.25);
                    if (j < tenor)
                    {
                        _discount[i + 1][j] = _discount[i + 1][j + 1] * (1 - 0.25 * _shift[j]) + _v[i + 1][j];
                    }
                    else
                    {
                        _discount[i + 1][j] = _v[i + 1][j];
                    }
                }
            }
        }
        private ExitCondition ExitCriteriaSatisfied(IEvaluation candidate_point, IEvaluation last_point)
        {
            Vector <double> rel_grad          = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(candidate_point.Point.Count);
            double          relative_gradient = 0.0;
            double          normalizer        = Math.Max(Math.Abs(candidate_point.Value), 1.0);

            for (int ii = 0; ii < rel_grad.Count; ++ii)
            {
                double tmp = candidate_point.Gradient[ii] * Math.Max(Math.Abs(candidate_point.Point[ii]), 1.0) / normalizer;
                relative_gradient = Math.Max(relative_gradient, Math.Abs(tmp));
            }
            if (relative_gradient < this.GradientTolerance)
            {
                return(ExitCondition.RelativeGradient);
            }

            if (last_point != null)
            {
                double most_progress = 0.0;
                for (int ii = 0; ii < candidate_point.Point.Count; ++ii)
                {
                    var tmp = Math.Abs(candidate_point.Point[ii] - last_point.Point[ii]) / Math.Max(Math.Abs(last_point.Point[ii]), 1.0);
                    most_progress = Math.Max(most_progress, tmp);
                }
                if (most_progress < this.ParameterTolerance)
                {
                    return(ExitCondition.LackOfProgress);
                }
            }

            return(ExitCondition.None);
        }
Beispiel #4
0
        public static void writeVectorToTxtFile(MathNet.Numerics.LinearAlgebra.Double.DenseVector dm, string file)
        {
            StringBuilder sb = new StringBuilder();

            foreach (var item in dm.Values)
            {
                sb.AppendFormat("{0,5:0.0000000000000e+00}\r\n", item);
            }
            System.IO.File.WriteAllText(file, sb.ToString());
        }
Beispiel #5
0
        ///<summary>
        ///</summary>
        ///<param name="i"></param>
        ///<returns></returns>
        public MathNet.Numerics.LinearAlgebra.Double.DenseVector RowD(int i)
        {
            var sp = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(ColumnCount);

            for (int j = 0; j < ColumnCount; j++)
            {
                sp[j] = this[i, j];
            }
            return(sp);
        }
Beispiel #6
0
        /// <summary>
        /// Run tests for Math.Net
        /// </summary>
        /// <seealso cref="http://www.mathdotnet.com/"/>
        /// <param name="results">The collection of results to add to</param>
        /// <remarks>Included using Nuget</remarks>
        private static void RunMathNetTests(Dictionary <string, long> results)
        {
            var libraryName = "MathNet";

            var a = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(new double[] { 2, 4, 6 });
            var b = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(new double[] { 5, 7, 10 });

            results.Add(
                "Cross Product => " + libraryName,
                Measure(
                    iterations,
                    () =>
            {
                var result = a.OuterProduct(b);             // Not sure if this is cross product or not
            }));

            results.Add(
                "Dot Product => " + libraryName,
                Measure(
                    iterations,
                    () =>
            {
                var result = a.DotProduct(b);
            }));

            results.Add(
                "Normalize => " + libraryName,
                Measure(
                    iterations,
                    () =>
            {
                var result = a.Normalize(0);
            }));

            results.Add(
                "Add => " + libraryName,
                Measure(
                    iterations,
                    () =>
            {
                var result = a + b;
            }));

            results.Add(
                "Subtract => " + libraryName,
                Measure(
                    iterations,
                    () =>
            {
                var result = a - b;
            }));
        }
Beispiel #7
0
        public double Get3DNoiseValue(double x, double y, double z)
        {
            // find unit grid cell containing point
            int floorX = (int)Math.Floor(x);
            int floorY = (int)Math.Floor(y);
            int floorZ = (int)Math.Floor(z);

            // get relative XYZ coordinates of point in cell
            double relX = x - floorX;
            double relY = y - floorY;
            double relZ = z - floorZ;

            //gradients of cube vertices
            VectorD g000 = GetGradient(floorX, floorY, floorZ);
            VectorD g001 = GetGradient(floorX, floorY, floorZ + 1);
            VectorD g010 = GetGradient(floorX, floorY + 1, floorZ);
            VectorD g011 = GetGradient(floorX, floorY + 1, floorZ + 1);
            VectorD g100 = GetGradient(floorX + 1, floorY, floorZ);
            VectorD g101 = GetGradient(floorX + 1, floorY, floorZ + 1);
            VectorD g110 = GetGradient(floorX + 1, floorY + 1, floorZ);
            VectorD g111 = GetGradient(floorX + 1, floorY + 1, floorZ + 1);

            // noise contribution from each of the eight corner
            double n000 = g000 * new VectorD(new[] { relX, relY, relZ });
            double n100 = g100 * new VectorD(new[] { relX - 1, relY, relZ });
            double n010 = g010 * new VectorD(new[] { relX, relY - 1, relZ });
            double n110 = g110 * new VectorD(new[] { relX - 1, relY - 1, relZ });
            double n001 = g001 * new VectorD(new[] { relX, relY, relZ - 1 });
            double n101 = g101 * new VectorD(new[] { relX - 1, relY, relZ - 1 });
            double n011 = g011 * new VectorD(new[] { relX, relY - 1, relZ - 1 });
            double n111 = g111 * new VectorD(new[] { relX - 1, relY - 1, relZ - 1 });

            // compute the fade curve value for each x, y, z
            double u = BlendingCurve(relX);
            double v = BlendingCurve(relY);
            double w = BlendingCurve(relZ);

            // interpolate along x the contribution from each of the corners
            double nx00 = Interpolation(n000, n100, u);
            double nx01 = Interpolation(n001, n101, u);
            double nx10 = Interpolation(n010, n110, u);
            double nx11 = Interpolation(n011, n111, u);

            // interpolate the four results along y
            double nxy0 = Interpolation(nx00, nx10, v);
            double nxy1 = Interpolation(nx01, nx11, v);

            // interpolate the two last results along z
            double nxyz = Interpolation(nxy0, nxy1, w);

            return(nxyz);
        }
Beispiel #8
0
        private void AddCorner(int id, int t1, int t2, int t3)
        {
            TCorner c = Corners[id];

            TTile[] t = new[] { Tiles[t1], Tiles[t2], Tiles[t3] };
            VectorD v = t[0].TileCenterPosition + t[1].TileCenterPosition + t[2].TileCenterPosition;


            c.V = (VectorD)v.Normalize(2.0);
            for (int i = 0; i < 3; i++)
            {
                t[i].Corners[t[i].GetTilePosition(t[(i + 2) % 3])] = c;
                c.Tiles[i] = t[i];
            }
        }
Beispiel #9
0
        public static MathNet.Numerics.LinearAlgebra.Double.DenseVector readDenceVectorFromBinFile(string file, MainForm.setProgressDel d)
        {
            string[]               meta = System.IO.File.ReadAllLines(file + ".metainfo");
            System.IO.FileStream   fs   = new System.IO.FileStream(file, System.IO.FileMode.Open);
            System.IO.BinaryReader br   = new System.IO.BinaryReader(fs, System.Text.Encoding.Default);
            int cols = int.Parse(meta[1]);

            MathNet.Numerics.LinearAlgebra.Double.DenseVector dv = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(cols);
            for (int j = 0; j < cols; j++)
            {
                dv[j] = br.ReadDouble();
                d.Invoke(br.BaseStream.Position / br.BaseStream.Length, 1, "Завантаження");
            }
            br.Close();
            fs.Close();
            fs.Dispose();
            return(dv);
        }
Beispiel #10
0
        public Vector3 GetDerivative(float t, int order)
        {
            // make sure t is in range [0,1]
            Assert.IsFalse(t < 0.0f || 1.0f < t, "t is not in Range [0,1]: t = " + t);

            Matrix <double> casteljau = GetCasteljauMatrix(pointsList.Count);
            Matrix <double> points    = GetPointsMatrix();

            // use calculus derivative rules to generically derive the curve
            //tVector = ((n-o)!*t^(n-o), (n-o-1)!*t^(n-o-2), ... 1);
            Vector <double> tVector = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(pointsList.Count);

            for (int i = 0; i < pointsList.Count; i++)
            {
                int    exponent = pointsList.Count - i - 1; //count down - 1
                double derivationFactor;
                if (order == 0)                             // no derivative
                {
                    derivationFactor = 1;
                }
                else if (exponent <= order - 1) // constants and below are thrown away thus set to 0
                {
                    tVector[i] = 0;
                    continue;
                }
                else
                {
                    derivationFactor = SpecialFunctions.Factorial(exponent) / SpecialFunctions.Factorial(exponent - order);
                }
                tVector[i] = derivationFactor * Mathf.Pow(t, exponent - order);
            }
            //now calculate the actual result
            Vector <double> result = points * casteljau * tVector;

            Vector3 ret;

            ret.x = (float)result[0];
            ret.y = (float)result[1];
            ret.z = (float)result[2];
            return(ret);
        }
        private D gradfD(double z, double[] a)
        {
            D      result;
            double estimatedError;
            D      zD = D.NewD(z);

            var xvec = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(a.Length);
            var yvec = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(a.Length);
            var dydx = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(a.Length);

            //Func<double, double> fbeta = x => fD(z, a);
            //Func<D, D[], D> fbeta2 = (x, y) => fD(zD, a);



            DiffSharp.Interop.D[] aD = new DiffSharp.Interop.D[100];
            D[] aDF = new D[100];

            // convert double arr to D[], two types
            // also put in to densearrays
            for (int i = 0; i < a.Length; i++)
            {
                aD[i] = a[i];                  // interop type
                //aDF[i] = fbeta2(zD, aDF);      // forward type
            }

            // nonsense
            //result = fbeta2(zD, aDF);

            //result = fbeta.ForwardDerivative(a[0], out estimatedError);


            //var gfDel = fbeta2(zD, aDF);

            //var gf = DiffSharp.Interop.AD.Grad(x => fbeta(z, a), a);
            //var gf1 = DiffOps.grad(fbeta2(zD, aDF), aDF);

            //var iRes = gf.DynamicInvoke(aD);
            return(D.One);
        }
Beispiel #12
0
        private double[] Polyfit(double[] x, double[] y, int degree)
        {
            // Vandermonde matrix
            var v = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(x.Length, degree + 1);

            for (int i = 0; i < v.RowCount; i++)
            {
                for (int j = 0; j <= degree; j++)
                {
                    v[i, j] = Math.Pow(x[i], j);
                }
            }
            var         yv = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(y).ToColumnMatrix();
            QR <double> qr = v.QR(QRMethod.Full);
            // Math.Net doesn't have an "economy" QR, so:
            // cut R short to square upper triangle, then recompute Q
            var r = qr.R.SubMatrix(0, degree + 1, 0, degree + 1);
            var q = v.Multiply(r.Inverse());
            var p = r.Inverse().Multiply(q.TransposeThisAndMultiply(yv));

            return(p.Column(0).ToArray());
        }
Beispiel #13
0
        public static double GetNumericCLC(List<double> d)
        {
            //d = new List<double> { 2, 3, 4, 5, 6, 7, 8 };

            List<double> xs = new List<double>();
            for (int i = 0; i < d.Count; ++i)
                xs.Add(i + 1);
            var X = MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.CreateFromColumns(
                new[] { new MathNet.Numerics.LinearAlgebra.Double.DenseVector(xs.Count, 1),
                    new MathNet.Numerics.LinearAlgebra.Double.DenseVector(xs.ToArray<double>()) });
            var y = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(d.ToArray<double>());
            var p = MathNet.Numerics.LinearAlgebra.Double.ExtensionMethods.QR(X).Solve(y);
            var a = p[0];
            var b = p[1];

            double sum = 0;
            for (int i = 0; i < d.Count; ++i)
            {
                double yy = xs[i] * a + b;
                sum += Math.Abs(yy - d[i]);
            }
            return sum / d.Count;
        }
Beispiel #14
0
        /// <summary>
        /// Find Homography.
        /// </summary>
        /// <param name="source"> The source Transformer. </param>
        /// <param name="destination"> The destination Transformer. </param>
        /// <returns> The homologous matrix. </returns>
        public static Matrix3x2 FindHomography(ITransformerLTRB source, ITransformerLTRB destination)
        {
            float x0 = source.LeftTop.X, x1 = source.RightTop.X, x2 = source.LeftBottom.X, x3 = source.RightBottom.X;
            float y0 = source.LeftTop.Y, y1 = source.RightTop.Y, y2 = source.LeftBottom.Y, y3 = source.RightBottom.Y;
            float u0 = destination.LeftTop.X, u1 = destination.RightTop.X, u2 = destination.LeftBottom.X, u3 = destination.RightBottom.X;
            float v0 = destination.LeftTop.Y, v1 = destination.RightTop.Y, v2 = destination.LeftBottom.Y, v3 = destination.RightBottom.Y;

            MathNet.Numerics.LinearAlgebra.Double.DenseMatrix matrix = MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.OfArray(new double[008, 008]
            {
                { x0, y0, 1, 0, 0, 0, -u0 * x0, -u0 * y0 },
                { 0, 0, 0, x0, y0, 1, -v0 * x0, -v0 * y0 },
                { x1, y1, 1, 0, 0, 0, -u1 * x1, -u1 * y1 },
                { 0, 0, 0, x1, y1, 1, -v1 * x1, -v1 * y1 },
                { x3, y3, 1, 0, 0, 0, -u3 * x3, -u3 * y3 },
                { 0, 0, 0, x3, y3, 1, -v3 * x3, -v3 * y3 },
                { x2, y2, 1, 0, 0, 0, -u2 * x2, -u2 * y2 },
                { 0, 0, 0, x2, y2, 1, -v2 * x2, -v2 * y2 },
            });

            MathNet.Numerics.LinearAlgebra.Double.DenseVector vector = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(new double[008]
            {
                u0, v0, u1, v1,
                u3, v3, u2, v2
            });

            MathNet.Numerics.LinearAlgebra.Matrix <double> PseudoInverse = Transformer.PseudoInverse2(matrix);

            MathNet.Numerics.LinearAlgebra.Vector <double> ret = Transformer.Multiply2(PseudoInverse, vector);

            return(new Matrix3x2
                   (
                       m11: (float)ret[0], m12: (float)ret[3],
                       m21: (float)ret[1], m22: (float)ret[4],
                       m31: (float)ret[2], m32: (float)ret[5]
                   ));
        }
Beispiel #15
0
 public static MathNet.Numerics.LinearAlgebra.Double.DenseVector readDenceVectorFromBinFile(string file, MainForm.setProgressDel d)
 {
     string[] meta = System.IO.File.ReadAllLines(file + ".metainfo");
     System.IO.FileStream fs = new System.IO.FileStream(file, System.IO.FileMode.Open);
     System.IO.BinaryReader br = new System.IO.BinaryReader(fs, System.Text.Encoding.Default);
     int cols = int.Parse(meta[1]);
     MathNet.Numerics.LinearAlgebra.Double.DenseVector dv = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(cols);
         for (int j = 0; j < cols; j++)
         {
             dv[j] = br.ReadDouble();
             d.Invoke(br.BaseStream.Position / br.BaseStream.Length, 1, "Завантаження");
         }
     br.Close();
     fs.Close();
     fs.Dispose();
     return dv;
 }
Beispiel #16
0
        /// <summary>
        /// Create steering vectors geometrically for each discrete frequency under the far-field assumption.
        /// </summary>
        /// <param name="microphones">The microphones.</param>
        /// <param name="direction">The horizontal direction of arrival of the sound.</param>
        /// <param name="pitch">The vertical direction of arrival of the sound.</param>
        /// <param name="sampleRate">The sampling frequency.</param>
        /// <param name="dftLength">The length of the DFT.</param>
        /// <returns>
        /// The steering vectors for each discrete frequency.
        /// Since the components higher than the Nyquist frequency are discarded,
        /// the length of the returned array is dftLength / 2 + 1.
        /// </returns>
        public static Vector <Complex>[] FromFarFieldGeometry(IReadOnlyList <Microphone> microphones, double direction, double pitch, int sampleRate, int dftLength)
        {
            if (microphones == null)
            {
                throw new ArgumentNullException(nameof(microphones));
            }
            if (microphones.Count == 0)
            {
                throw new ArgumentException(nameof(microphones), "The number of microphones must be greater than zero.");
            }
            if (microphones.Any(mic => mic == null))
            {
                throw new ArgumentException(nameof(microphones), "All the microphone object must be non-null.");
            }

            if (sampleRate <= 0)
            {
                throw new ArgumentException(nameof(sampleRate), "The sampling frequency must be greater than zero.");
            }

            if (dftLength <= 0 || dftLength % 2 != 0)
            {
                throw new ArgumentException(nameof(dftLength), "The length of the DFT must be positive and even.");
            }

            var channelCount = microphones.Count;

            var destination = new Vector <Complex> [dftLength / 2 + 1];

            for (var w = 0; w < destination.Length; w++)
            {
                destination[w] = new DenseVector(channelCount);
            }

            var dv = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(3);

            dv[0] = Math.Cos(direction) * Math.Cos(pitch);
            dv[1] = Math.Sin(direction) * Math.Cos(pitch);
            dv[2] = Math.Sin(pitch);

            var distances = new double[channelCount];

            for (var ch = 0; ch < channelCount; ch++)
            {
                distances[ch] = dv * microphones[ch].Position;
            }
            var offset = distances.Min();

            for (var ch = 0; ch < channelCount; ch++)
            {
                distances[ch] -= offset;
            }

            for (var ch = 0; ch < channelCount; ch++)
            {
                var distance         = distances[ch];
                var time             = distance / AcousticConstants.SoundSpeed;
                var delaySampleCount = sampleRate * time;
                var delayFilter      = Filtering.CreateFrequencyDomainDelayFilter(dftLength, delaySampleCount);

                for (var w = 0; w < destination.Length; w++)
                {
                    destination[w][ch] = delayFilter[w];
                }
            }

            return(destination);
        }
Beispiel #17
0
        private void button4_Click(object sender, EventArgs e)
        {
            Action fileProc = () =>
            {
                System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
                MathNet.Numerics.Control.LinearAlgebraProvider = new MathNet.Numerics.Algorithms.LinearAlgebra.Mkl.MklLinearAlgebraProvider();
                MathNet.Numerics.Control.NumberOfParallelWorkerThreads = Environment.ProcessorCount;
                addText("Обробка файлу вимірювань...\r\n");
                double[][] SGG_data = null;
                if (System.IO.File.Exists("sgg_data.bin"))
                {
                    SGG_data = IOFunc.binLoad_SGG_data("sgg_data.bin");
                }
                else
                {
                    SGG_data = Data.IOFunc.read_SGG_data(SphericalHarmonicAnalyze.Properties.Settings.Default.SGG_measures, new setProgressDel(addVal));
                    IOFunc.binwrite_SGG_data("sgg_data.bin", SGG_data);
                }
                addText("Дані вимірювань оброблено: {0} шт.\r\n", SGG_data.Length); Thread.Sleep(500);
                ReferenceSystem elipsoid = new ReferenceSystem(ReferenceSystem.Default.TideFree);
                elipsoid.gridParameters.cellSize = SphericalHarmonicAnalyze.Properties.Settings.Default.GridCellSize;
                elipsoid.gridParameters.coLatitudeBounds = SphericalHarmonicAnalyze.Properties.Settings.Default.minCoLatitude;
                elipsoid.maxDegree = SphericalHarmonicAnalyze.Properties.Settings.Default.modelMaxOrder;
                int greedColumnsCount, greedRowsCount;
                List<double[]> greed = MathFunc.generateGrid(elipsoid.gridParameters.cellSize, out greedColumnsCount, out greedRowsCount, elipsoid.gridParameters.coLatitudeBounds,180 - elipsoid.gridParameters.coLatitudeBounds);
                addText("Сітку згенеровано: {0} комірок \r\n", greed.Count);
                double avgR = MathFunc.getAvgRadius(SGG_data);
                List<int>[] map = MathFunc.getMappingOfPoints(elipsoid, SGG_data, greed.ToArray(), greedRowsCount, greedColumnsCount, avgR); sw.Stop(); addText("Точки віднесено до комірок сітки за: {0}.\r\n", sw.Elapsed.ToString());
                addText("Кількість клітинок сітки всього: {0}\r\n", greed.Count);
                int res1 = 0; foreach (var item in map) { res1 += item.Count; } addText("Використано вимірів: {0}\r\nСер радіус: {1}\r\n", res1, avgR);
                test.checkMap(SGG_data, map, greed, elipsoid);
                List<int>[] newMap = null;
                MathFunc.checkGreed(ref greed, map, out newMap);
                addText("Кількість клітинок сітки, в яких присутні дані вимірювань: {0}\r\n", greed.Count);
                map = newMap; newMap = null;
                IOFunc.writeGreedToCsvFileWithMeasureCount(greed, map, "greed_new_map.txt");
                double[] avgRadius; sw.Restart();
                double[] regularisedValues = MathFunc.regularization(SGG_data, greed.ToArray(), map, out avgRadius); sw.Stop(); addText("Регуляризація (на основі сферичної відстані) виконана за: {0}.\r\n", sw.Elapsed.ToString());
                IOFunc.writeGreedToCsvFileWithMeasureS(greed,regularisedValues, "greed_regular_grad.txt");
                avgRadius[0] = Math.Round(avgRadius[0]);
                elipsoid.satelliteSphere = avgRadius[0];
                addText("Середній радіус: {0,10:0.000}.\r\nМінімальний радіус: {1,10:0.0000}\r\nМаксимальний радіус:{2,10:0.0000}\r\n", avgRadius[0], avgRadius[1], avgRadius[2]);
                SGG_data = null; map = null;
                int[][] t_nm = MathFunc.get_nm(elipsoid.maxDegree);
                sw.Restart();
                MathNet.Numerics.LinearAlgebra.Double.DenseMatrix dm = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(greed.Count, (MathFunc.getArraySize(elipsoid.maxDegree) - 3) * 2 - (elipsoid.maxDegree-1));
                sw.Stop(); addText("Пам'ять для матриці коефіцієнтів виділено за: {0}.\r\n", sw.Elapsed.ToString());
                sw.Restart();
                int progress = 0;
                //Обчислення елементів матриці
                var p= Parallel.For(0, dm.RowCount, (i) =>
                {
                    double[] line = MathFunc.getCoefMatrixLineKoop(elipsoid, elipsoid.maxDegree, t_nm, elipsoid.satelliteSphere, greed[i][0], greed[i][1]);
                    lock (dm)
                    {
                         dm.SetRow(i,line);

                    }
                    progress++;
                    if (progress / 100D == Math.Round(progress / 100D)) {addVal(progress, dm.RowCount, "Визначено");}
                });

                if (!p.IsCompleted) { throw new Exception("Parallel.For"); };
                IOFunc.writeMatrixToMatLabFile(dm, @"matlab\A.mat","A");
                sw.Stop();
                richTextBox1.Invoke(new setProgressDel(addVal), new object[] { 0, dm.RowCount, "" });
                addText("Матриця {0} на {1} ({2}MB) згенерована за: {3,10}\r\n", dm.RowCount, dm.ColumnCount, dm.ColumnCount * dm.RowCount * 8 / 1000000,sw.Elapsed.ToString()/* + "\r\nЗапис у файл...\r\n"*/);
                if(true){
                GravityModel gm08 = new GravityModel(elipsoid.maxDegree);
                gm08.loadFromFile("GO_CONS_EGM_GCF_2.gfc", new setProgressDel(addVal));
                MathNet.Numerics.LinearAlgebra.Double.DenseVector dmL = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm08.getGradientForGrid(elipsoid,greed));//regularisedValues);
                MathNet.Numerics.LinearAlgebra.Double.DenseVector dmL2;
                GravityModel gm = new GravityModel(elipsoid.maxDegree);
                    if (radioButton1.Checked) {
                    sw.Restart();
                    gm.loadFromFile(SphericalHarmonicAnalyze.Properties.Settings.Default.inGravityModel, new setProgressDel(addVal));
                    sw.Stop(); addText("Вихідна модель завантажена за: {0}.\r\n", sw.Elapsed.ToString());
                    sw.Restart();
                    dmL2 = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm.getGradientForGrid(elipsoid,greed));
                    sw.Stop(); addText("Градієнти за вихідною моделлю обчислені для сітки за: {0}.\r\n", sw.Elapsed.ToString());
                }
                else
                {
                    sw.Restart();
                    gm = GravityModel.getNormalModel(elipsoid, elipsoid.maxDegree);
                    dmL2 = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm.getGradientForGrid(elipsoid, greed));
                    sw.Stop(); addText("Нормальні градієнти обчислені для сітки за: {0}.\r\n", sw.Elapsed.ToString());
                }
                dmL = dmL - dmL2;
                dmL2 = null;
                IOFunc.writeMatrixToMatLabFile(dmL.ToColumnMatrix(), @"matlab\L.mat", "L");
                    sw.Restart();
                MathNet.Numerics.LinearAlgebra.Double.DenseVector dmLNormal = null;
                dmLNormal = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)dm.TransposeThisAndMultiply(dmL);
                dmL = null;
                IOFunc.writeMatrixToMatLabFile(dmLNormal.ToColumnMatrix(), @"matlab\LNorm.mat", "LNorm");
                sw.Stop(); addText("Стовпчик вільних членів обчислений за: {0}.\r\n", sw.Elapsed.ToString());
                MathNet.Numerics.LinearAlgebra.Double.DenseMatrix dmANorm = null;
                sw.Restart();
                dmANorm = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)dm.TransposeThisAndMultiply(dm); dm = null;
                sw.Stop(); addText("Нормальна матриця коефіціэнтів обчислена за: {0}.\r\n", sw.Elapsed.ToString());
                IOFunc.writeMatrixToMatLabFile(dmANorm, @"matlab\ANorm.mat", "ANorm");
                //dmLNormal = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)dmLNormal.Multiply(5e-8);
                var x = dmANorm.Inverse();
                var res = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)x.Multiply(dmLNormal);
                IOFunc.writeModeVectorlToTxtFile(res, elipsoid, @"matlab\_out.AL");
                addText(@"Результат за методом A\L знайдено...");
                x = null;
                GravityModel gm_R = new GravityModel(gm);
                gm_R.addDeltaCoef(res.ToArray()); res = null;
                double[] h = GravityModel.getGeoidHeight(elipsoid, gm_R, greed);
                double[] dg = GravityModel.getAnomaly(elipsoid, gm_R, greed);
                IOFunc.writeGeoidHeightsAndAnomalysToTxt(greed, h, dg, elipsoid, @"output\result_AL.txt");
                IOFunc.writeGravityModelToTxtFile(gm_R, @"output\model_AL.gcf");
                sw.Restart();
                addText(dmANorm.Rank().ToString() + "\r\n");
                dmANorm = null;
                dmLNormal = null;
                sw.Stop(); addText("Невідомі знайдено за: {0}.\r\n", sw.Elapsed.ToString());

            }
            };

            if (System.IO.File.Exists(SphericalHarmonicAnalyze.Properties.Settings.Default.inGravityModel)) {
            tabControl1.SelectedTab = tabControl1.TabPages[1];
            this.UseWaitCursor = true;
            ts = new CancellationTokenSource();
            ct = ts.Token;
            tsk = Task.Factory.StartNew(fileProc,ct);
            var setCur = Task.Factory.StartNew(() => { tsk.Wait(); this.UseWaitCursor = false; addText("Обчислення завершені!"); });
            richTextBox1.SaveFile(@"output\zvit.rtf");
            }
        }
Beispiel #18
0
        private double PolynomialRegressionAndDerivativeAtMidPoint_MathNet(Vector3[] points, ref Vector2 midPoint)
        {
            int numPoints = points.Length;

            double[] xd = new double[points.Length];
            double[] yd = new double[points.Length];

            //checking if the points are actualy spread over X-axis.
            //other wise I swap the X-Y axises to make te polyfit function of matlab work.

            Vector2 first      = points[0].Xy;
            Vector2 last       = points[points.Length - 1].Xy;
            bool    invereseXY = false;

            if (Math.Abs(first.X - last.X) < Math.Abs(first.Y - last.Y))
            {
                invereseXY = true;
            }

            if (invereseXY)
            {
                first = new Vector2(first.Y, first.X);
                last  = new Vector2(last.Y, last.X);
            }
            Vector2 midOfFirstLast = (first + last) / 2;

            Common.Line2D FLLine      = new Common.Line2D(first, last);
            Common.Line2D midPerpLine = new Common.Line2D(midOfFirstLast, FLLine.PerpendicularMu());

            if (!invereseXY)
            {
                for (int i = 0; i < points.Length; i++)
                {
                    xd[i] = points[i].X;
                    yd[i] = points[i].Y;
                }
            }
            else
            {
                for (int i = 0; i < points.Length; i++)
                {
                    xd[i] = points[i].Y;
                    yd[i] = points[i].X;
                }
            }


            //Vector2d midPoint = new Vector2d(xd[minDistToMidPerpLineIndex], yd[minDistToMidPerpLineIndex]);

            var vx = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(xd);
            var vy = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(yd);

            Common.PolynominalRegression poly = new Common.PolynominalRegression(vx, vy, 3);



            switch (config.bbpChoice)
            {
            case OrthoAid.Common.BBPChoice.User:
                if (invereseXY)
                {
                    midPoint = new Vector2(midPoint.Y, midPoint.X);
                }
                break;

            case OrthoAid.Common.BBPChoice.AutoMiddle:
                Vector2[] crossPoints = midPerpLine.CrosspointWithCurve(poly.GetCoefficients());
                if (crossPoints.Length == 1)
                {
                    midPoint = crossPoints[0];
                }
                else
                {
                    midPoint = crossPoints[0];
                    for (int i = 1; i < crossPoints.Length; i++)
                    {
                        if ((crossPoints[i] - midOfFirstLast).LengthSquared <
                            (midPoint - midOfFirstLast).LengthSquared)
                        {
                            midPoint = crossPoints[i];
                        }
                    }
                }
                break;

            default:
                break;
            }


            double mu = poly.DerivativeAtPoint(midPoint.X);

            //var t = new MathNet.Numerics.Interpolation.Algorithms.LinearSplineInterpolation(


            if (invereseXY)
            {
                mu       = 1 / mu;
                midPoint = new Vector2(midPoint.Y, midPoint.X);
            }


            return(mu);
        }
Beispiel #19
0
        private void save_thread(object none)
        {
            int selected = selected_record;
            byte[] full_file = File.ReadAllBytes("C:\\temp\\log_" + selected + ".txt");
            Dispatcher.Invoke(new Action(() => set_stage("stage 1")));
            uint[] ticks = new uint[full_file.Length / 35 + 1];   // примерное количество IMU пакетов
            uint[] ticks2 = new uint[ticks.Length/15];   // примерное количество GPS пакетов неизвестно,
            byte[] type = new byte[ticks.Length];   // но оно примерно в 20 раз меньше (взято с запасом)

            byte[] pack = new byte[32];
            byte[] pack2 = new byte[26];
            int crc;
            int k = 0;
            int k2 = 0;
            int tt = 0;
            int[] counter = new int[ticks.Length];
            double[,] a = new double[ticks.Length, 3];
            double[,] w = new double[ticks.Length, 3];
            double[,] m = new double[ticks.Length, 3];
            double[,] q = new double[ticks.Length, 4];
            double[] anglex = new double[ticks.Length];
            double[] angley = new double[ticks.Length];
            double[] anglez = new double[ticks.Length];
            double[] lat = new double[ticks2.Length];
            double[] lon = new double[ticks2.Length];
            double[] speed = new double[ticks2.Length];
            double[] course = new double[ticks2.Length];
            double[] time = new double[ticks2.Length];
            double[] stat = new double[ticks2.Length];
            double[] date = new double[ticks2.Length];
            byte[] buffer = new byte[2];
            byte[] buffer2 = new byte[4];
            double[] corr_mag = {   1, 2.2391, 1, 2.1586, 2.3980,
                                    1, 1, 1.9201, 1.9963, 2.1538,
                                    1, 1, 2.7105, 1.9782, 1.9013 };
            double[] corr_accl = {   1, 0.9778, 1, 0.8928, 1.3899,
                                     1, 0.9722, 0.9766, 0.9599, 0.9379,
                                     0.9183, 0.9924, 1.2657, 0.9892, 0.9598 };
            double[,] corr_gyr = {   {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 1
                                     {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 2
                                     {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 3
                                     {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 4
                                     {-0.00373042367688305, 1.4558979622744, -0.0258818845609887, 0.00460253725775466, 1.4522057450117,
                                         -0.035372440971534, -0.00301630637615598, 1.46124337866467, 0.0112681875599293}, // 5
                                     {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 6
                                     {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 7
                                     {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 8
                                     {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 9
                                     {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 10
                                     {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 11
                                     {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 12
                                     {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 13
                                     {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 14
                                     {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 15
                  }; // Gyro correction coeffs are listed in XYZ order
            double accl_scale = 0;
            if (block_index < 16)
                accl_scale = corr_accl[block_index - 1];
            double mag_scale = 0;
            if (block_index < 16)
                mag_scale = corr_mag[block_index - 1];
            double[] gyr_scale = new double[9];
            if (block_index < 16)
                for (int t = 0; t < 9; t++)
                    gyr_scale[t] = corr_gyr[block_index - 1, t];

               addText("\n" + get_time() + " Начало сохранения файла...");
            for (int i = 0; i < full_file.Length - 30; i++)
            {
                if ((i < full_file.Length - 35) && (full_file[i + 34] == 3) && (full_file[i + 33] == 16) &&
                    (full_file[i] == 16) && (full_file[i + 1] == 49))   // условие начала IMU пакета
                {
                    crc = 0;
                    for (int j = 0; j < 32; j++)
                    {
                        pack[j] = full_file[i + j + 1];
                        if (j < 31)
                            crc = crc ^ pack[j];
                    }
                    if (crc == pack[pack.Length - 1])
                    {
                        ticks[k] = BitConverter.ToUInt32(pack, 1);
                        type[k] = pack[0];
                        if (type[k] == 49)
                        {
                            buffer[0] = pack[7]; buffer[1] = pack[8];
                            a[k, 0] = ((double)BitConverter.ToInt16(buffer, 0)) * 0.001766834114354 *accl_scale;
                            buffer[0] = pack[5]; buffer[1] = pack[6];
                            a[k, 1] = (double)BitConverter.ToInt16(buffer, 0) * 0.001766834114354 *accl_scale;
                            buffer[0] = pack[9]; buffer[1] = pack[10];
                            a[k, 2] = -(double)BitConverter.ToInt16(buffer, 0) * 0.001766834114354 *accl_scale;

                            buffer[0] = pack[13]; buffer[1] = pack[14];
                            w[k, 0] = (double)BitConverter.ToInt16(buffer, 0) * 0.00053264;
                            buffer[0] = pack[11]; buffer[1] = pack[12];
                            w[k, 1] = (double)BitConverter.ToInt16(buffer, 0) * 0.00053264;
                            buffer[0] = pack[15]; buffer[1] = pack[16];
                            w[k, 2] = -(double)BitConverter.ToInt16(buffer, 0) * 0.00053264;
                            w[k, 0] = gyr_scale[0] * Math.Pow(w[k, 0], 2) + gyr_scale[1] * w[k, 0] + gyr_scale[2];
                            w[k, 1] = gyr_scale[3] * Math.Pow(w[k, 1], 2) + gyr_scale[4] * w[k, 1] + gyr_scale[5];
                            w[k, 2] = gyr_scale[6] * Math.Pow(w[k, 2], 2) + gyr_scale[7] * w[k, 2] + gyr_scale[8];

                            buffer[0] = pack[17]; buffer[1] = pack[18];
                            m[k, 0] = ((double)BitConverter.ToInt16(buffer, 0) * 0.00030518) * mag_scale;
                            buffer[0] = pack[19]; buffer[1] = pack[20];
                            m[k, 1] = -((double)BitConverter.ToInt16(buffer, 0) * 0.00030518) * mag_scale;
                            buffer[0] = pack[21]; buffer[1] = pack[22];
                            m[k, 2] = ((double)BitConverter.ToInt16(buffer, 0) * 0.00030518) * mag_scale;

                            buffer[0] = pack[23]; buffer[1] = pack[24];
                            q[k, 0] = (double)BitConverter.ToInt16(buffer, 0);
                            buffer[0] = pack[25]; buffer[1] = pack[26];
                            q[k, 1] = (double)BitConverter.ToInt16(buffer, 0) * 0.00003125;
                            buffer[0] = pack[27]; buffer[1] = pack[28];
                            q[k, 2] = (double)BitConverter.ToInt16(buffer, 0) * 0.00003125;
                            buffer[0] = pack[29]; buffer[1] = pack[30];
                            q[k, 3] = (double)BitConverter.ToInt16(buffer, 0) * 0.00003125;
                        }
                        counter[k] = k2;
                        k++;
                    }
                    else
                        tt++;
                }
                if ((full_file[i + 29] == 3) && (full_file[i + 28] == 16) && (full_file[i] == 16) &&
                    (full_file[i + 1] == 50))   // условие начала GPS пакета
                {
                    crc = 50;
                    for (int j = 0; j < 26; j++)
                    {
                        pack2[j] = full_file[i + j + 2];
                        if (j < 25)
                            crc = crc ^ pack2[j];
                    }
                    if (crc == pack2[pack2.Length - 1])
                    {
                        ticks2[k2] = BitConverter.ToUInt32(pack2, 0);
                        buffer2[0] = pack2[4]; buffer2[1] = pack2[5]; buffer2[2] = pack2[6]; buffer2[3] = pack2[7];
                        lat[k2] = ((double)BitConverter.ToInt32(buffer2, 0)) / 600000;
                        buffer2[0] = pack2[8]; buffer2[1] = pack2[9]; buffer2[2] = pack2[10]; buffer2[3] = pack2[11];
                        lon[k2] = ((double)BitConverter.ToInt32(buffer2, 0)) / 600000;
                        buffer[0] = pack2[12]; buffer[1] = pack2[13];
                        speed[k2] = (double)BitConverter.ToInt16(buffer, 0) / 100;
                        buffer[0] = pack2[14]; buffer[1] = pack2[15];
                        course[k2] = (double)BitConverter.ToInt16(buffer, 0) / 160;
                        buffer2[0] = pack2[16]; buffer2[1] = pack2[17]; buffer2[2] = pack2[18]; buffer2[3] = pack2[19];
                        time[k2] = ((double)BitConverter.ToInt32(buffer2, 0)) / 10;
                        stat[k2] = pack2[20];
                        buffer2[0] = pack2[21]; buffer2[1] = pack2[22]; buffer2[2] = pack2[23]; buffer2[3] = pack2[24];
                        date[k2] = ((double)BitConverter.ToInt32(buffer2, 0));
                        k2++;

                    }
                }
            }

            // -------------- Сохранение в IMU/GPS

            double[] angles = new double[3];
            double[] mw, ma, mm;
            ma = new double[3];
            mw = new double[3];
            mm = new double[3];

            FileStream fs_imu = File.Create(file2save + ".imu", 2048, FileOptions.None);
            BinaryWriter str_imu = new BinaryWriter(fs_imu);
            FileStream fs_gps = File.Create(file2save + ".gps", 2048, FileOptions.None);
            BinaryWriter str_gps = new BinaryWriter(fs_gps);
            Int16 buf16; Byte buf8; Int32 buf32;
            Double bufD; Single bufS; UInt32 bufU32;

            addText("\n" + get_time() + " Сохранение файлов:\n" + file2save + ".imu\n" + file2save + ".gps");
            int pbar_adj = 50;
            Dispatcher.Invoke(new Action(() => progressBar1.Maximum = (k+k2)/pbar_adj + 1));
            Dispatcher.Invoke(new Action(() => progressBar1.Value = 0));

            double[] magn_c = get_magn_coefs(block_index);
            double[] accl_c = get_accl_coefs(block_index);
            double[] gyro_c = new double[12];

            double[] w_helper = new double[ticks.Length];

            // Получение уголов путем интегрирования угловых скоростей (простой вариант)
            //for (int i = 0; i < w_helper.Length; i++) w_helper[i] = w[i, 0];
            //anglex = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
            //for (int i = 0; i < w_helper.Length; i++) w_helper[i] = w[i, 1];
            //angley = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
            //for (int i = 0; i < w_helper.Length; i++) w_helper[i] = w[i, 2];
            //anglez = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
            //-----------------------------------------------------------------------------------------------
            // Получение углов путем использования фильтра Калмана (сложный вариант)
            MathNet.Numerics.LinearAlgebra.Double.DenseVector Magn_coefs =
                new MathNet.Numerics.LinearAlgebra.Double.DenseVector(get_magn_coefs(block_index));
            MathNet.Numerics.LinearAlgebra.Double.DenseVector Accl_coefs =
                new MathNet.Numerics.LinearAlgebra.Double.DenseVector(get_accl_coefs(block_index));
            MathNet.Numerics.LinearAlgebra.Double.DenseVector Gyro_coefs = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(12);
            Kalman_class.Parameters Parameters = new Kalman_class.Parameters(Accl_coefs, Magn_coefs, Gyro_coefs);
            System.Func<int, int, double> filler = (x, y) => 0;
            //Kalman_class.Sensors Sensors = new Kalman_class.Sensors(new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(1, 3, 0),
            //    new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(1, 3, 0), new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(1, 3, 0));
            //MathNet.Numerics.LinearAlgebra.Double.Matrix Initia_quat = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(1, 4, 0);
            Kalman_class.Sensors Sensors = new Kalman_class.Sensors(MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.Create(1,3, filler),
                MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.Create(1, 3, filler), MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.Create(1, 3, filler));
            MathNet.Numerics.LinearAlgebra.Double.Matrix Initia_quat = MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.Create(1, 4, filler);
            Initia_quat[0, 0] = 1;

            Kalman_class.State State = new Kalman_class.State(Kalman_class.ACCLERATION_NOISE, Kalman_class.MAGNETIC_FIELD_NOISE, Kalman_class.ANGULAR_VELOCITY_NOISE,
                Math.Pow(10, -6), Math.Pow(10, -15), Math.Pow(10, -15), Initia_quat);
            Tuple<MathNet.Numerics.LinearAlgebra.Double.Vector, Kalman_class.Sensors, Kalman_class.State> AHRS_result;
            //----------------------------------------------------------------------------------------------
            System.Diagnostics.Stopwatch timer = new System.Diagnostics.Stopwatch();
            timer.Start();
            for (int i = 0; i < k; i++)
            {
                if (i % pbar_adj == 0)
                    Dispatcher.Invoke(new Action(() => progressBar1.Value++));
                if (i == ((int)(k + k2) / 20))
                {
                    long min = (timer.ElapsedMilliseconds * 16) / 1000 / 60;
                    long sec = (timer.ElapsedMilliseconds * 16 - min * 1000 * 60) / 1000;
                    addText("\n" + get_time() + " Сохранение файлов займет около\n" +
                        min + " минут(ы) и " + sec + " секунд(ы).");
                }

                //------------------------------------------------------------------
                // Легкий варинат - интегрирование угловых скоростей
                mm = single_correction(magn_c, m[i, 0], m[i, 1], m[i, 2]);
                ma = single_correction(accl_c, a[i, 0], a[i, 1], a[i, 2]);
                mw = single_correction(gyro_c, w[i, 0], w[i, 1], w[i, 2]);

                //angles[0] = (anglez[i]);
                //angles[1] = (angley[i]);
                //angles[2] = (anglex[i]);
                //----------------------------------------------------------------------
                // Сложный вариант - фильтр Калмана
                for (int j = 0; j < 3; j++)
                {
                    Sensors.a.At(0, j, a[i, j]);
                    Sensors.w.At(0, j, w[i, j]);
                    Sensors.m.At(0, j, m[i, j]);
                }
                //Sensors.a.At(0, 0, a[i, 0]);
                //Sensors.a.At(0, 1, a[i, 1]);
                //Sensors.a.At(0, 2, a[i, 2]);

                //Sensors.w.At(0, 0, w[i, 0]);
                //Sensors.w.At(0, 1, w[i, 1]);
                //Sensors.w.At(0, 2, w[i, 2]);

                //Sensors.m.At(0, 0, m[i, 0]);
                //Sensors.m.At(0, 1, m[i, 1]);
                //Sensors.m.At(0, 2, m[i, 2]);

                AHRS_result = Kalman_class.AHRS_LKF_EULER(Sensors, State, Parameters);
                State = AHRS_result.Item3;

                //ma[0] = AHRS_result.Item2.a[0, 0];
                //ma[1] = AHRS_result.Item2.a[0, 1];
                //ma[2] = AHRS_result.Item2.a[0, 2];

                //mw[0] = w[i, 0];
                //mw[1] = w[i, 1];
                //mw[2] = w[i, 2];
                //ma[0] = a[i, 0];
                //ma[1] = a[i, 1];
                //ma[2] = a[i, 2];
                //mm[0] = m[i, 0];
                //mm[1] = m[i, 1];
                //mm[2] = m[i, 2];

                angles[0] = (AHRS_result.Item1.At(0));
                angles[1] = (AHRS_result.Item1.At(1));
                angles[2] = (AHRS_result.Item1.At(2));
                //------------------------------------------------------------------------

                // IMU
                buf16 = (Int16)(angles[0] * 10000);
                str_imu.Write(buf16);
                buf16 = (Int16)(angles[1] * 10000);
                str_imu.Write(buf16);
                buf16 = (Int16)(angles[2] * 10000);
                str_imu.Write(buf16);

                buf16 = (Int16)(mw[0] * 3000);
                str_imu.Write(buf16);
                buf16 = (Int16)(mw[1] * 3000);
                str_imu.Write(buf16);
                buf16 = (Int16)(mw[2] * 3000);
                str_imu.Write(buf16);

                buf16 = (Int16)(ma[0] * 3000);
                str_imu.Write(buf16);
                buf16 = (Int16)(ma[1] * 3000);
                str_imu.Write(buf16);
                buf16 = (Int16)(ma[2] * 3000);
                str_imu.Write(buf16);

                buf16 = (Int16)(mm[0] * 3000);
                str_imu.Write(buf16);
                buf16 = (Int16)(mm[1] * 3000);
                str_imu.Write(buf16);
                buf16 = (Int16)(mm[2] * 3000);
                str_imu.Write(buf16);

                buf16 = (Int16)(q[i, 0]);
                str_imu.Write(buf16);

                buf32 = (Int32)(ticks[i]);
                str_imu.Write(buf32);

                buf8 = (Byte)(0);
                str_imu.Write(buf8);
            }

            for (int i = 0; i < k2; i++)
            {
                if (i % pbar_adj == 0)
                    Dispatcher.Invoke(new Action(() => progressBar1.Value++));
                // GPS
                bufD = (Double)(lat[i]) / ((180 / Math.PI) * 16.66);
                str_gps.Write(bufD);
                bufD = (Double)(lon[i]) / ((180 / Math.PI) * 16.66);
                str_gps.Write(bufD);
                bufD = (Double)(0);
                str_gps.Write(bufD);

                bufS = (Single)(time[i]);
                str_gps.Write(bufS);
                bufS = (Single)(speed[i]);
                str_gps.Write(bufS);
                bufS = (Single)(0);
                str_gps.Write(bufS);
                str_gps.Write(bufS);

                bufU32 = (UInt32)(ticks2[i]);
                str_gps.Write(bufU32);
                buf8 = (Byte)(0);
                str_gps.Write(buf8);
                str_gps.Write(buf8);
                str_gps.Write(buf8);
            }
            // Запись даты в конец gps файла
            bool inertial = false, sattelite = false;
            if (k > 0)
                inertial = true;
            if (k2 > 0)
            {
                sattelite = true;
                int day = (int)date[k2 - 1] / 10000;
                int month = (int)(date[k2 - 1] - day * 10000) / 100;
                int year = (int)(2000 + date[k2 - 1] - day * 10000 - month * 100);
                string datarec = String.Format("{0:d2}.{1:d2}.{2:d4}", day, month, year);
                str_gps.Write(datarec);
            }

            if (!inertial || !sattelite)
                addText("\n" + get_time() + "Неполный комплект данных:\nИнерциальные данные - " + inertial + "\nСпутниковые данные - " + sattelite);
            str_imu.Flush();
            str_imu.Close();
            str_gps.Flush();
            str_gps.Close();

            Dispatcher.Invoke(new Action(() => progressBar1.Value = (k+k2)/pbar_adj + 1));
            //addText("\n" + get_time() + " Сохранение завершено! Всего " + timer.ElapsedMilliseconds / 1000 + " секунд!");
            addText("\n" + get_time() + " Сохранение завершено!");
            System.Media.SystemSounds.Asterisk.Play();
            timer.Stop();
            Dispatcher.Invoke(new Action(() => set_stage("stage 2")));
        }
Beispiel #20
0
        private void button4_Click(object sender, EventArgs e)
        {
            Action fileProc = () =>
            {
                System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
                MathNet.Numerics.Control.LinearAlgebraProvider         = new MathNet.Numerics.Algorithms.LinearAlgebra.Mkl.MklLinearAlgebraProvider();
                MathNet.Numerics.Control.NumberOfParallelWorkerThreads = Environment.ProcessorCount;
                addText("Обробка файлу вимірювань...\r\n");
                double[][] SGG_data = null;
                if (System.IO.File.Exists("sgg_data.bin"))
                {
                    SGG_data = IOFunc.binLoad_SGG_data("sgg_data.bin");
                }
                else
                {
                    SGG_data = Data.IOFunc.read_SGG_data(SphericalHarmonicAnalyze.Properties.Settings.Default.SGG_measures, new setProgressDel(addVal));
                    IOFunc.binwrite_SGG_data("sgg_data.bin", SGG_data);
                }
                addText("Дані вимірювань оброблено: {0} шт.\r\n", SGG_data.Length); Thread.Sleep(500);
                ReferenceSystem elipsoid = new ReferenceSystem(ReferenceSystem.Default.TideFree);
                elipsoid.gridParameters.cellSize         = SphericalHarmonicAnalyze.Properties.Settings.Default.GridCellSize;
                elipsoid.gridParameters.coLatitudeBounds = SphericalHarmonicAnalyze.Properties.Settings.Default.minCoLatitude;
                elipsoid.maxDegree = SphericalHarmonicAnalyze.Properties.Settings.Default.modelMaxOrder;
                int             greedColumnsCount, greedRowsCount;
                List <double[]> greed = MathFunc.generateGrid(elipsoid.gridParameters.cellSize, out greedColumnsCount, out greedRowsCount, elipsoid.gridParameters.coLatitudeBounds, 180 - elipsoid.gridParameters.coLatitudeBounds);
                addText("Сітку згенеровано: {0} комірок \r\n", greed.Count);
                double       avgR = MathFunc.getAvgRadius(SGG_data);
                List <int>[] map  = MathFunc.getMappingOfPoints(elipsoid, SGG_data, greed.ToArray(), greedRowsCount, greedColumnsCount, avgR); sw.Stop(); addText("Точки віднесено до комірок сітки за: {0}.\r\n", sw.Elapsed.ToString());
                addText("Кількість клітинок сітки всього: {0}\r\n", greed.Count);
                int res1 = 0; foreach (var item in map)
                {
                    res1 += item.Count;
                }
                addText("Використано вимірів: {0}\r\nСер радіус: {1}\r\n", res1, avgR);
                test.checkMap(SGG_data, map, greed, elipsoid);
                List <int>[] newMap = null;
                MathFunc.checkGreed(ref greed, map, out newMap);
                addText("Кількість клітинок сітки, в яких присутні дані вимірювань: {0}\r\n", greed.Count);
                map = newMap; newMap = null;
                IOFunc.writeGreedToCsvFileWithMeasureCount(greed, map, "greed_new_map.txt");
                double[] avgRadius; sw.Restart();
                double[] regularisedValues = MathFunc.regularization(SGG_data, greed.ToArray(), map, out avgRadius); sw.Stop(); addText("Регуляризація (на основі сферичної відстані) виконана за: {0}.\r\n", sw.Elapsed.ToString());
                IOFunc.writeGreedToCsvFileWithMeasureS(greed, regularisedValues, "greed_regular_grad.txt");
                avgRadius[0]             = Math.Round(avgRadius[0]);
                elipsoid.satelliteSphere = avgRadius[0];
                addText("Середній радіус: {0,10:0.000}.\r\nМінімальний радіус: {1,10:0.0000}\r\nМаксимальний радіус:{2,10:0.0000}\r\n", avgRadius[0], avgRadius[1], avgRadius[2]);
                SGG_data = null; map = null;
                int[][] t_nm = MathFunc.get_nm(elipsoid.maxDegree);
                sw.Restart();
                MathNet.Numerics.LinearAlgebra.Double.DenseMatrix dm = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(greed.Count, (MathFunc.getArraySize(elipsoid.maxDegree) - 3) * 2 - (elipsoid.maxDegree - 1));
                sw.Stop(); addText("Пам'ять для матриці коефіцієнтів виділено за: {0}.\r\n", sw.Elapsed.ToString());
                sw.Restart();
                int progress = 0;
                //Обчислення елементів матриці
                var p = Parallel.For(0, dm.RowCount, (i) =>
                {
                    double[] line = MathFunc.getCoefMatrixLineKoop(elipsoid, elipsoid.maxDegree, t_nm, elipsoid.satelliteSphere, greed[i][0], greed[i][1]);
                    lock (dm)
                    {
                        dm.SetRow(i, line);
                    }
                    progress++;
                    if (progress / 100D == Math.Round(progress / 100D))
                    {
                        addVal(progress, dm.RowCount, "Визначено");
                    }
                });

                if (!p.IsCompleted)
                {
                    throw new Exception("Parallel.For");
                }
                ;
                IOFunc.writeMatrixToMatLabFile(dm, @"matlab\A.mat", "A");
                sw.Stop();
                richTextBox1.Invoke(new setProgressDel(addVal), new object[] { 0, dm.RowCount, "" });
                addText("Матриця {0} на {1} ({2}MB) згенерована за: {3,10}\r\n", dm.RowCount, dm.ColumnCount, dm.ColumnCount * dm.RowCount * 8 / 1000000, sw.Elapsed.ToString() /* + "\r\nЗапис у файл...\r\n"*/);
                if (true)
                {
                    GravityModel gm08 = new GravityModel(elipsoid.maxDegree);
                    gm08.loadFromFile("GO_CONS_EGM_GCF_2.gfc", new setProgressDel(addVal));
                    MathNet.Numerics.LinearAlgebra.Double.DenseVector dmL = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm08.getGradientForGrid(elipsoid, greed));//regularisedValues);
                    MathNet.Numerics.LinearAlgebra.Double.DenseVector dmL2;
                    GravityModel gm = new GravityModel(elipsoid.maxDegree);
                    if (radioButton1.Checked)
                    {
                        sw.Restart();
                        gm.loadFromFile(SphericalHarmonicAnalyze.Properties.Settings.Default.inGravityModel, new setProgressDel(addVal));
                        sw.Stop(); addText("Вихідна модель завантажена за: {0}.\r\n", sw.Elapsed.ToString());
                        sw.Restart();
                        dmL2 = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm.getGradientForGrid(elipsoid, greed));
                        sw.Stop(); addText("Градієнти за вихідною моделлю обчислені для сітки за: {0}.\r\n", sw.Elapsed.ToString());
                    }
                    else
                    {
                        sw.Restart();
                        gm   = GravityModel.getNormalModel(elipsoid, elipsoid.maxDegree);
                        dmL2 = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm.getGradientForGrid(elipsoid, greed));
                        sw.Stop(); addText("Нормальні градієнти обчислені для сітки за: {0}.\r\n", sw.Elapsed.ToString());
                    }
                    dmL  = dmL - dmL2;
                    dmL2 = null;
                    IOFunc.writeMatrixToMatLabFile(dmL.ToColumnMatrix(), @"matlab\L.mat", "L");
                    sw.Restart();
                    MathNet.Numerics.LinearAlgebra.Double.DenseVector dmLNormal = null;
                    dmLNormal = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)dm.TransposeThisAndMultiply(dmL);
                    dmL       = null;
                    IOFunc.writeMatrixToMatLabFile(dmLNormal.ToColumnMatrix(), @"matlab\LNorm.mat", "LNorm");
                    sw.Stop(); addText("Стовпчик вільних членів обчислений за: {0}.\r\n", sw.Elapsed.ToString());
                    MathNet.Numerics.LinearAlgebra.Double.DenseMatrix dmANorm = null;
                    sw.Restart();
                    dmANorm = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)dm.TransposeThisAndMultiply(dm); dm = null;
                    sw.Stop(); addText("Нормальна матриця коефіціэнтів обчислена за: {0}.\r\n", sw.Elapsed.ToString());
                    IOFunc.writeMatrixToMatLabFile(dmANorm, @"matlab\ANorm.mat", "ANorm");
                    //dmLNormal = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)dmLNormal.Multiply(5e-8);
                    var x   = dmANorm.Inverse();
                    var res = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)x.Multiply(dmLNormal);
                    IOFunc.writeModeVectorlToTxtFile(res, elipsoid, @"matlab\_out.AL");
                    addText(@"Результат за методом A\L знайдено...");
                    x = null;
                    GravityModel gm_R = new GravityModel(gm);
                    gm_R.addDeltaCoef(res.ToArray()); res = null;
                    double[] h  = GravityModel.getGeoidHeight(elipsoid, gm_R, greed);
                    double[] dg = GravityModel.getAnomaly(elipsoid, gm_R, greed);
                    IOFunc.writeGeoidHeightsAndAnomalysToTxt(greed, h, dg, elipsoid, @"output\result_AL.txt");
                    IOFunc.writeGravityModelToTxtFile(gm_R, @"output\model_AL.gcf");
                    sw.Restart();
                    addText(dmANorm.Rank().ToString() + "\r\n");
                    dmANorm   = null;
                    dmLNormal = null;
                    sw.Stop(); addText("Невідомі знайдено за: {0}.\r\n", sw.Elapsed.ToString());
                }
            };

            if (System.IO.File.Exists(SphericalHarmonicAnalyze.Properties.Settings.Default.inGravityModel))
            {
                tabControl1.SelectedTab = tabControl1.TabPages[1];
                this.UseWaitCursor      = true;
                ts  = new CancellationTokenSource();
                ct  = ts.Token;
                tsk = Task.Factory.StartNew(fileProc, ct);
                var setCur = Task.Factory.StartNew(() => { tsk.Wait(); this.UseWaitCursor = false; addText("Обчислення завершені!"); });
                richTextBox1.SaveFile(@"output\zvit.rtf");
            }
        }
Beispiel #21
0
 public static Vector3 ToVector3(this VectorD vector)
 {
     return(new Vector3((float)vector[0], (float)vector[1], (float)vector[2]));
 }
 /// <summary>
 /// Returns unityvector holding the direction
 /// </summary>
 /// <param name="p1"></param>
 /// <param name="p2"></param>
 /// <returns></returns>
 public static XYPoint GetDirection(IXYPoint p1, IXYPoint p2)
 {
   MathNet.Numerics.LinearAlgebra.Double.DenseVector v = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(2);
   v[0] = p2.X - p1.X;
   v[1] = p2.Y - p1.Y;
   v.Normalize(1);
   return new XYPoint(v[0], v[1]);
 }
Beispiel #23
0
 public static void writeModeVectorlToTxtFile(MathNet.Numerics.LinearAlgebra.Double.DenseVector dm, ReferenceSystem rs, string file)
 {
     System.IO.File.WriteAllText(file, matrixToString((MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)dm.ToColumnMatrix(), rs), System.Text.Encoding.Default);
 }