C# DenseMatrix.At方法代码示例

        public override ulong Create(BitmapData image)
        {
            var data = new DenseMatrix(32, 32);
            using (var unmanaged = UnmanagedImage.FromManagedImage(image))
            using (var filtered = ExtractChannel.Apply(unmanaged))
            {
                new Convolution(Filter, 9).ApplyInPlace(filtered);
                using (var imgdata = new ResizeNearestNeighbor(32, 32).Apply(filtered))
                {
                    unsafe
                    {
                        byte* src = (byte*)imgdata.ImageData.ToPointer();
                        int offset = imgdata.Stride - imgdata.Width;
                        int width = imgdata.Width;
                        int height = imgdata.Height;
                        for (int y = 0; y < height; y++)
                        {
                            for (int x = 0; x < width; x++, src++)
                            {
                                data.At(y, x, (float)*src / 255);
                            }
                            src += offset;
                        }
                    }
                }
            }

            var dct = DctMatrix.FastDCT(data);

            int dctsize = 8;
            var vals = new List<double>();
            for (int r = 1; r <= dctsize; r++)
            {
                for (int c = 1; c <= dctsize; c++)
                {
                    vals.Add(dct[r, c]);
                }
            }

            var sorted = new List<double>(vals);
            sorted.Sort();
            var mid = dctsize * dctsize / 2;
            double median = (sorted[mid - 1] + sorted[mid]) / 2d;

            ulong index = 1;
            ulong result = 0;
            for (int i = 0; i < dctsize * dctsize; i++)
            {
                if (vals[i] > median)
                {
                    result |= index;
                }

                index = index << 1;
            }

            return result;
        }

        public void NormalizeMatrixTest()
        {
            //
            Matrix matrix = new DenseMatrix(3, 3, -15);
            Random random = new Random(2);
            foreach (var elem in matrix.IndexedEnumerator())
            {
                matrix.At(elem.Item1, elem.Item2, elem.Item3 + 5 * random.NextDouble());
            }
            double min = 1.0, max = 2.5;

            //
            var normalizedMatrix = matrix.Normalize(max, min);

            //
            foreach (var row in normalizedMatrix.RowEnumerator())
            {
                Assert.IsTrue(row.Item2.Minimum() >= min);
                Assert.IsTrue(row.Item2.Maximum() <= max);
            }
        }

 private double[] single_correction(double[] coefs, double xdata, double ydata, double zdata)
 {
     double[] result = new double[3];
     Matrix B = new DiagonalMatrix(3, 3, 1);
     Matrix A = new DenseMatrix(3, 3);
     A.At(0, 0, coefs[0]);
     A.At(0, 1, coefs[3]);
     A.At(0, 2, coefs[4]);
     A.At(1, 0, coefs[5]);
     A.At(1, 1, coefs[1]);
     A.At(1, 2, coefs[6]);
     A.At(2, 0, coefs[7]);
     A.At(2, 1, coefs[8]);
     A.At(2, 2, coefs[2]);
     Matrix B1 = Kalman_class.Matrix_Minus(B, A);
     Matrix C = new DenseMatrix(3, 1);
     C.At(0, 0, xdata);
     C.At(1, 0, ydata);
     C.At(2, 0, zdata);
     Matrix D = new DenseMatrix(3, 1);
     D.At(0, 0, coefs[9]);
     D.At(1, 0, coefs[10]);
     D.At(2, 0, coefs[11]);
     Matrix res = new DenseMatrix(3, 1);
     res = Kalman_class.Matrix_Mult(B1, Kalman_class.Matrix_Minus(C, D));
     result[0] = res.At(0, 0);
     result[1] = res.At(1, 0);
     result[2] = res.At(2, 0);
     return result;
 }

        /// <summary>
        /// Run example
        /// </summary>
        public void Run()
        {
            // Format vector output to console
            var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
            formatProvider.TextInfo.ListSeparator = " ";

            // Create new empty square matrix
            var matrix = new DenseMatrix(10);
            Console.WriteLine(@"Empty matrix");
            Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 1. Fill matrix by data using indexer []
            var k = 0;
            for (var i = 0; i < matrix.RowCount; i++)
            {
                for (var j = 0; j < matrix.ColumnCount; j++)
                {
                    matrix[i, j] = k++;
                }
            }

            Console.WriteLine(@"1. Fill matrix by data using indexer []");
            Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 2. Fill matrix by data using At. The element is set without range checking.
            for (var i = 0; i < matrix.RowCount; i++)
            {
                for (var j = 0; j < matrix.ColumnCount; j++)
                {
                    matrix.At(i, j, k--);
                }
            }

            Console.WriteLine(@"2. Fill matrix by data using At");
            Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 3. Clone matrix
            var clone = matrix.Clone();
            Console.WriteLine(@"3. Clone matrix");
            Console.WriteLine(clone.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 4. Clear matrix
            clone.Clear();
            Console.WriteLine(@"4. Clear matrix");
            Console.WriteLine(clone.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 5. Copy matrix into another matrix
            matrix.CopyTo(clone);
            Console.WriteLine(@"5. Copy matrix into another matrix");
            Console.WriteLine(clone.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 6. Get submatrix into another matrix
            var submatrix = matrix.SubMatrix(2, 2, 3, 3);
            Console.WriteLine(@"6. Copy submatrix into another matrix");
            Console.WriteLine(submatrix.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 7. Get part of the row as vector. In this example: get 4 elements from row 5 starting from column 3
            var row = matrix.Row(5, 3, 4);
            Console.WriteLine(@"7. Get part of the row as vector");
            Console.WriteLine(row.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 8. Get part of the column as vector. In this example: get 3 elements from column 2 starting from row 6
            var column = matrix.Column(2, 6, 3);
            Console.WriteLine(@"8. Get part of the column as vector");
            Console.WriteLine(column.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 9. Get columns using column enumerator. If you need all columns you may use ColumnEnumerator without parameters
            Console.WriteLine(@"9. Get columns using column enumerator");
            foreach (var keyValuePair in matrix.ColumnEnumerator(2, 4))
            {
                Console.WriteLine(@"Column {0}: {1}", keyValuePair.Item1, keyValuePair.Item2.ToString("#0.00\t", formatProvider));
            }

            Console.WriteLine();

            // 10. Get rows using row enumerator. If you need all rows you may use RowEnumerator without parameters
            Console.WriteLine(@"10. Get rows using row enumerator");
            foreach (var keyValuePair in matrix.RowEnumerator(4, 3))
            {
                Console.WriteLine(@"Row {0}: {1}", keyValuePair.Item1, keyValuePair.Item2.ToString("#0.00\t", formatProvider));
            }

            Console.WriteLine();

            // 11. Convert matrix into multidimensional array
            var data = matrix.ToArray();
            Console.WriteLine(@"11. Convert matrix into multidimensional array");
            for (var i = 0; i < data.GetLongLength(0); i++)
//.........这里部分代码省略.........

        /// <summary>
        /// Create a matrix based on this vector in row form (one single row).
        /// </summary>
        /// <returns>This vector as a row matrix.</returns>
        public override Matrix<double> ToRowMatrix()
        {
            var matrix = new DenseMatrix(1, _length);
            for (var i = 0; i < _values.Length; i++)
            {
                matrix.At(0, i, _values[i]);
            }

            return matrix;
        }

 public static Matrix quat_to_DCM(Matrix q)
 {
     Matrix DCM = new DenseMatrix(3, 3, 0);
     q = quat_norm(q);
     DCM.At(0, 0, (q.At(0, 0) * q.At(0, 0) + q.At(0, 1) * q.At(0, 1) - q.At(0, 2) * q.At(0, 2) - q.At(0, 3) * q.At(0, 3)));
     DCM.At(0, 1, 2 * (q.At(0, 1) * q.At(0, 2) + q.At(0, 0) * q.At(0, 3)));
     DCM.At(0, 2, 2 * (q.At(0, 1) * q.At(0, 3) - q.At(0, 0) * q.At(0, 2)));
     DCM.At(1, 0, 2 * (q.At(0, 1) * q.At(0, 2) - q.At(0, 0) * q.At(0, 3)));
     DCM.At(1, 1, (q.At(0, 0) * q.At(0, 0) - q.At(0, 1) * q.At(0, 1) + q.At(0, 2) * q.At(0, 2) - q.At(0, 3) * q.At(0, 3)));
     DCM.At(1, 2, 2 * (q.At(0, 2) * q.At(0, 3) + q.At(0, 0) * q.At(0, 1)));
     DCM.At(2, 0, 2 * (q.At(0, 1) * q.At(0, 3) + q.At(0, 0) * q.At(0, 2)));
     DCM.At(2, 1, 2 * (q.At(0, 2) * q.At(0, 3) - q.At(0, 0) * q.At(0, 1)));
     DCM.At(2, 2, (q.At(0, 0) * q.At(0, 0) - q.At(0, 1) * q.At(0, 1) - q.At(0, 2) * q.At(0, 2) + q.At(0, 3) * q.At(0, 3)));
     return DCM;
 }

 public static Matrix mrotate(Matrix q, Matrix w, double dT)
 {
     double Fx = w.At(0, 0) * dT;
     double Fy = w.At(1, 0) * dT;
     double Fz = w.At(2, 0) * dT;
     double Fm = (double)Math.Sqrt(Fx * Fx + Fy * Fy + Fz * Fz);
     double sinFm2 = (double)Math.Sin(Fm / 2);
     double cosFm2 = (double)Math.Cos(Fm / 2);
     Matrix Nd = new DenseMatrix(1, 4, 0);
     if (Fm != (double)0)
     {
         Nd.At(0, 0, cosFm2);
         Nd.At(0, 1, (Fx / Fm) * sinFm2);
         Nd.At(0, 2, (Fy / Fm) * sinFm2);
         Nd.At(0, 3, (Fz / Fm) * sinFm2);
     }
     else Nd.At(0, 0, 1);
     q = quat_multi(q, Nd);
     return q;
 }

 public static Matrix Matrix_Plus(Matrix A, Matrix B)
 {
     Matrix C = new DenseMatrix(A.RowCount, A.ColumnCount, 0);
     int i, j;
     if (A.RowCount != B.RowCount | A.ColumnCount != B.ColumnCount) return C;
     for (i = 0; i < A.RowCount; i++)
     {
         for (j = 0; j < A.ColumnCount; j++)
         {
             C.At(i, j, A.At(i, j) + B.At(i, j));
         }
     }
     return C;
 }

        public Matrix<double> Upsample_CrossAverage(Matrix<double> baseImage)
        {
            int rows2 = baseImage.RowCount * 2;
            int cols2 = baseImage.ColumnCount * 2;
            double sum;
            int lr = baseImage.RowCount - 1;
            int lc = baseImage.ColumnCount - 1;
            var scaledImage = new DenseMatrix(rows2, cols2);

            // TODO consider borders
            // 1) Base image pixels have (2r0,2c0) coords
            // 2) For each (2r0+1,2c0+1) let it be average of 4 diagonal neighbours
            // 3 For each (2r0,2c0+1),(2r0+2,2c0) let it be average of 4 edge neighbours (including thise from 2)
            for(int c = 0; c < lc; ++c)
            {
                for(int r = 0; r < lr; ++r)
                {
                    sum = baseImage.At(r, c) + baseImage.At(r + 1, c + 1)
                        + baseImage.At(r, c + 1) + baseImage.At(r + 1, c);

                    scaledImage.At(2 * r, 2 * c, baseImage.At(r, c));
                    scaledImage.At(2 * r + 1, 2 * c + 1, 0.25 * sum);
                }

                sum = baseImage.At(lr, c) + baseImage.At(lr - 1, c + 1)
                    + baseImage.At(lr, c + 1) + baseImage.At(lr - 1, c);

                scaledImage.At(2 * lr, 2 * c, baseImage.At(lr, c));
                scaledImage.At(2 * lr + 1, 2 * c + 1, 0.25 * sum);

                scaledImage.At(lr, c, sum);
            }
            // Last column -> mirror one out of bounds
            for(int r = 0; r < lr; ++r)
            {
                sum = baseImage.At(r, lc) + baseImage.At(r + 1, lc - 1)
                    + baseImage.At(r, lc - 1) + baseImage.At(r + 1, lc);

                scaledImage.At(2 * r, 2 * lc, baseImage.At(r, lc));
                scaledImage.At(2 * r + 1, 2 * lc + 1, 0.25 * sum);
            }
            // Last row -> mirror row/col out of bounds
            sum = baseImage.At(lr, lc) + baseImage.At(lr - 1, lc - 1)
                + baseImage.At(lr, lc - 1) + baseImage.At(lr - 1, lc);

            scaledImage.At(2 * lr, 2 * lc, baseImage.At(lr, lc));
            scaledImage.At(2 * lr + 1, 2 * lc + 1, 0.25 * sum);

            // First row/column
            for(int r = 0; r < lr; ++r)
            {
                sum = baseImage.At(r, 0) + baseImage.At(r + 1, 0) +
                    scaledImage.At(2 * r + 1, 1) * 2.0;
                scaledImage.At(2 * r + 1, 0, 0.25 * sum);
            }
            sum = baseImage.At(lr, 0) + baseImage.At(lr - 1, 0) +
                scaledImage.At(2 * lr + 1, 1) * 2.0;
            scaledImage.At(2 * lr + 1, 0, 0.25 * sum);

            for(int c = 0; c < lc; ++c)
            {
                sum = baseImage.At(0, c) + baseImage.At(0, c + 1) +
                    scaledImage.At(1, 2 * c + 1) * 2.0;
                scaledImage.At(0, 2 * c + 1, 0.25 * sum);
            }
            sum = baseImage.At(0, lc) + baseImage.At(0, lc - 1) +
                scaledImage.At(1, 2 * lc + 1) * 2.0;
            scaledImage.At(0, 2 * lc + 1, 0.25 * sum);

            for(int c = 0; c < lc; ++c)
            {
                for(int r = 0; r < lr; ++r)
                {
                    sum = baseImage.At(r, c) + baseImage.At(r + 1, c) +
                        scaledImage.At(2 * r + 1, 2 * c + 1) + scaledImage.At(2 * r + 1, 2 * c - 1);
                    scaledImage.At(2 * r + 1, 2 * c, 0.25 * sum);

                    sum = baseImage.At(r, c) + baseImage.At(r, c + 1) +
                        scaledImage.At(2 * r + 1, 2 * c + 1) + scaledImage.At(2 * r - 1, 2 * c + 1);
                    scaledImage.At(2 * r, 2 * c + 1, 0.25 * sum);
                }
                sum = baseImage.At(lr, c) + baseImage.At(lr - 1, c) +
                    scaledImage.At(2 * lr + 1, 2 * c + 1) + scaledImage.At(2 * lr + 1, 2 * c - 1);
                scaledImage.At(2 * lr + 1, 2 * c, 0.25 * sum);

                sum = baseImage.At(lr, c) + baseImage.At(lr, c + 1) +
                    scaledImage.At(2 * lr + 1, 2 * c + 1) + scaledImage.At(2 * lr - 1, 2 * c + 1);
                scaledImage.At(2 * lr, 2 * c + 1, 0.25 * sum);
            }

            for(int r = 0; r < lr; ++r)
            {
                sum = baseImage.At(r, lc) + baseImage.At(r + 1, lc) +
                    scaledImage.At(2 * r + 1, 2 * lc + 1) + scaledImage.At(2 * r + 1, 2 * lc - 1);
                scaledImage.At(2 * r + 1, 2 * lc, 0.25 * sum);

                sum = baseImage.At(r, lc) + baseImage.At(r, lc - 1) +
                    scaledImage.At(2 * r + 1, 2 * lc + 1) + scaledImage.At(2 * r - 1, 2 * lc + 1);
                scaledImage.At(2 * r, 2 * lc + 1, 0.25 * sum);
            }
//.........这里部分代码省略.........

        public Matrix<double> Upsample_Copy(Matrix<double> baseImage)
        {
            int rows2 = baseImage.RowCount * 2;
            int cols2 = baseImage.ColumnCount * 2;

            var scaledImage = new DenseMatrix(rows2, cols2);

            for(int c = 0; c < baseImage.ColumnCount; ++c)
            {
                for(int r = 0; r < baseImage.RowCount; ++r)
                {
                    scaledImage.At(2 * r, 2 * c, baseImage.At(r, c));
                    scaledImage.At(2 * r + 1, 2 * c, baseImage.At(r, c));
                    scaledImage.At(2 * r, 2 * c + 1, baseImage.At(r, c));
                    scaledImage.At(2 * r + 1, 2 * c + 1, baseImage.At(r, c));
                }
            }

            return scaledImage;
        }

        public Matrix<double> Downsample_Skippixel(Matrix<double> baseImage)
        {
            int rows2 = baseImage.RowCount / 2;
            int cols2 = baseImage.ColumnCount / 2;

            var scaledImage = new DenseMatrix(rows2, cols2);

            // Save each second pixel
            for(int c = 0; c < scaledImage.ColumnCount; ++c)
            {
                for(int r = 0; r < scaledImage.RowCount; ++r)
                {
                    scaledImage.At(r, c,
                        baseImage.At(2 * r, 2 * c));
                }
            }

            return scaledImage;
        }

        public Matrix<double> Downsample_Gauss33(Matrix<double> baseImage)
        {
            int rows2 = baseImage.RowCount / 2;
            int cols2 = baseImage.ColumnCount / 2;

            var scaledImage = new DenseMatrix(rows2, cols2);
            double sum;
            int lr = scaledImage.RowCount - 1;
            int lc = scaledImage.ColumnCount - 1;
            // TODO: borders
            for(int c = 0; c < lc; ++c)
            {
                for(int r = 0; r < lr; ++r)
                {
                    sum = baseImage.At(2 * r, 2 * c) * _gauss33[0] +
                        baseImage.At(2 * r, 2 * c + 1) * _gauss33[1] +
                        baseImage.At(2 * r, 2 * c + 2) * _gauss33[0] +
                        baseImage.At(2 * r + 1, 2 * c) * _gauss33[1] +
                        baseImage.At(2 * r + 1, 2 * c + 1) * _gauss33[2] +
                        baseImage.At(2 * r + 1, 2 * c + 2) * _gauss33[1] +
                        baseImage.At(2 * r + 2, 2 * c) * _gauss33[0] +
                        baseImage.At(2 * r + 2, 2 * c + 1) * _gauss33[1] +
                        baseImage.At(2 * r + 2, 2 * c + 2) * _gauss33[0];

                    scaledImage.At(r, c, sum);
                }
                // Last row -> mirror one out of bounds
                sum = baseImage.At(2 * lr, 2 * c) * _gauss33[0] * 2.0 +
                    baseImage.At(2 * lr, 2 * c + 1) * _gauss33[1] * 2.0 +
                    baseImage.At(2 * lr, 2 * c + 2) * _gauss33[0] * 2.0 +
                    baseImage.At(2 * lr + 1, 2 * c) * _gauss33[1] +
                    baseImage.At(2 * lr + 1, 2 * c + 1) * _gauss33[2] +
                    baseImage.At(2 * lr + 1, 2 * c + 2) * _gauss33[1];

                scaledImage.At(lr, c, sum);
            }
            // Last column -> mirror one out of bounds
            for(int r = 0; r < lr; ++r)
            {
                sum = baseImage.At(2 * r, 2 * lc) * _gauss33[0] * 2.0 +
                    baseImage.At(2 * r, 2 * lc + 1) * _gauss33[1] +
                    baseImage.At(2 * r + 1, 2 * lc) * _gauss33[1] * 2.0 +
                    baseImage.At(2 * r + 1, 2 * lc + 1) * _gauss33[2] +
                    baseImage.At(2 * r + 2, 2 * lc) * _gauss33[0] * 2.0 +
                    baseImage.At(2 * r + 2, 2 * lc + 1) * _gauss33[1];

                scaledImage.At(r, lc, sum);
            }
            // Last row -> mirror row/col out of bounds=
            sum = baseImage.At(2 * lr, 2 * lc) * _gauss33[0] * 4.0 +
                baseImage.At(2 * lr, 2 * lc + 1) * _gauss33[1] * 2.0 +
                baseImage.At(2 * lr + 1, 2 * lc) * _gauss33[1] * 2.0 +
                baseImage.At(2 * lr + 1, 2 * lc + 1) * _gauss33[2];

            scaledImage.At(lr, lc, sum);

            return scaledImage;
        }

        public Matrix<double> Downsample_Average22(Matrix<double> baseImage)
        {
            int rows2 = baseImage.RowCount / 2;
            int cols2 = baseImage.ColumnCount / 2;

            var scaledImage = new DenseMatrix(rows2, cols2);

            // For each pixel set average of 2x2 neighbourhood
            for(int c = 0; c < scaledImage.ColumnCount; ++c)
            {
                for(int r = 0; r < scaledImage.RowCount; ++r)
                {
                    double sum = baseImage.At(2 * r, 2 * c) + baseImage.At(2 * r, 2 * c + 1) +
                        baseImage.At(2 * r + 1, 2 * c) + baseImage.At(2 * r + 1, 2 * c + 1);
                    scaledImage.At(r, c, 0.25 * sum);
                }
            }

            return scaledImage;
        }

        private void write_data(packet[] source, int index)
        {
            int length = source.Length;
            saveBox.Text = "Сохранение файла " + index;
            saveBox.Update();
            progressBar.Value = 0;
            progressBar.Maximum = length;
            string additional = "";
            double[] magn_c = new double[12];
            double[] accl_c = new double[12];
            double[] gyro_c = new double[12];
            double Mk = 1;
            double Wk = 1;
            double Ak = 1;
            switch (index)
            {
                case 1:
                    additional = "left_oar";
                    double[] temp1 = { 0.021012275928952,   0.067860169975568,   0.000127689343889,   0.045296151352541,
                                      -0.009324017077391,   0.026722923577867,   0.008307857594568,   0.005284287915262,
                                      -0.035947192687517,  -0.012542925115207,   0.061512131892943,   0.004409782721854 };

                    accl_c = temp1;
                    Ak = 0.996259867299029;
                    double[] temp11 = { 0.002873025761360,  -0.000277783847693,  -0.008949237789463,   0.024839518300883,
                                        0.018134114269133,   0.004957678597933,   0.059903966032509,  -0.020001702990621,
                                       -0.048455202844697,   0.040007724599001,  -0.002723436449578,   0.028657182612509 };
                    magn_c = temp11;
                    Mk = 1.989928230657113;
                    Wk = 1.024951581925949;
                    break;
                case 2:
                    additional = "right_oar";
                    double[] temp2 = { 0.006489103530672,   0.007711972047671,   0.008910927008256,   0.004526628966860,
                                      -0.030613701020120,   0.009733926042102,   0.000520434141698,  -0.020438431372878,
                                      -0.000690207812167,   0.003527827615877,   0.003268096379523,  -0.009084146831565 };

                    accl_c = temp2;
                    Ak = 0.993917643789002;
                    double[] temp22 = {-0.004338472524034,  -0.007279143920897,  -0.016596561254121,  -0.040100105586272,
                                        0.026447190185438,   0.062723828297715,   0.014235202706375,  -0.032778454891706,
                                       -0.016686216543873,   0.049183804843751,  -0.170661765769371,   0.174784618340082 };
                    magn_c = temp22;
                    Mk = 1.981050375613622;
                    Wk = 0.999473669119779;
                    break;
                case 3:
                    additional = "first_hand";
                    double[] temp3 = { 0.012248789806271,   0.010963952889031,   0.010082940049261,   0.017732310046796,
                                      -0.010807018986018,  -0.017866987099261,  -0.038976713533177,   0.011937965171044,
                                       0.017418474194167,   0.010157002539499,   0.006639710879831,  -0.008997250633174 };

                    accl_c = temp3;
                    Ak = 0.995116429313122;
                    double[] temp33 = {-0.024902579646778,  -0.020478721707093,  -0.026353375666180,  -0.001744353714878,
                                        0.001038721251414,  -0.008552827405651,   0.000013148017514,   0.005334631470956,
                                       -0.004628038571913,  -0.041660570389140,   0.203013174006507,   0.110944181992475 };
                    magn_c = temp33;
                    Mk = 2.000804739331122;
                    Wk = 1.005957834380545;
                    break;
                case 4:
                    additional = "second_hand";
                    double[] temp4 = { 0.009773445955991,   0.010515969102165,   0.046332523300049,  -0.009734857220358,
                                       0.088901575635824,   0.011068909952491,  -0.012572850762404,   0.170713670954205,
                                      -0.008974449587517,   0.001063872406408,  -0.005104328004269,  -0.003338723771119 };
                    accl_c = temp4;
                    Ak = 0.995096553223738;
                    double[] temp44 = { 0.014780841871083,   0.018746310904878,   0.009620181136691,  -0.006116082059700,
                                       -0.000324924134046,  -0.004326570176434,  -0.007611544691470,   0.009652319917883,
                                        0.002813293660957,   0.093081170636918,   0.035125384985654,  -0.014316950547061 };
                    magn_c = temp44;
                    Mk = 2.063752665877927;
                    Wk = 1.013532577606638;
                    break;
                case 5:
                    additional = "seat";
                    double[] temp5 = { 0.018860976044349,   0.012994456079329,  -0.017726498806178,   0.014155888603851,
                                       0.135660705289298,  -0.005517590513633,  -0.006515740524433,   0.089222505526992,
                                       0.007556195473340,  -0.034224056011697,  -0.032625814493799,  -0.091725250270515 };
                    accl_c = temp5;
                    Ak = 0.990392687940513;
                    break;
            }
            FileStream fs_imu = File.Create(saveFileDialog.FileName + "_" + additional + ".imu", 2048, FileOptions.None);
            BinaryWriter str_imu = new BinaryWriter(fs_imu);
            Int16 buf16; Byte buf8; Int32 buf32;
            Double bufD; Single bufS; UInt32 bufU32;

            DenseVector Magn_coefs = new DenseVector(12);
            DenseVector Accl_coefs = new DenseVector(12);
            DenseVector Gyro_coefs = new DenseVector(12);
            Kalman_class.Parameters Parameters = new Kalman_class.Parameters(Accl_coefs, Magn_coefs, Gyro_coefs);
            Kalman_class.Sensors Sensors = new Kalman_class.Sensors(new DenseMatrix(1, 3, 0), new DenseMatrix(1, 3, 0), new DenseMatrix(1, 3, 0));
            Matrix Initia_quat = new DenseMatrix(1, 4, 0);
            Initia_quat.At(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);

            double[] angles = new double[3];
//.........这里部分代码省略.........

 public static Matrix Matrix_Const_Mult(Matrix A, double B)
 {
     Matrix C = new DenseMatrix(A.RowCount, A.ColumnCount, 0);
     int i, j;
     for (i = 0; i < A.RowCount; i++)
     {
         for (j = 0; j < A.ColumnCount; j++)
         {
             C.At(i, j, A.At(i, j)*B);
         }
     }
     return C;
 }