Example #1
0
    public static double[][] MatrixInversion(double[][] matrix)
    {
        int matrixLength = matrix.Length;

        double[][] matrixTemp = JaggedArray.CreateJaggedArray <double[][]>(matrixLength, 2 * matrixLength);

        double[][] result = JaggedArray.CreateJaggedArray <double[][]>(matrixLength, matrixLength);

        for (int i = 0; i < matrixLength; i++)
        {
            for (int j = 0; j < matrixLength; j++)
            {
                matrixTemp[i][j] = matrix[i][j];
            }
        }

        for (int k = 0; k < matrixLength; k++)
        {
            for (int t = matrixLength; t < matrixLength * 2; t++)
            {
                matrixTemp[k][t] = t - k == matrixLength ? 1.0 : 0;
            }
        }

        for (int k = 0; k < matrixLength; k++)
        {
            if (matrixTemp[k][k] != 1)
            {
                double bs = matrixTemp[k][k];
                matrixTemp[k][k] = 1;
                for (int p = k; p < matrixLength * 2; p++)
                {
                    matrixTemp[k][p] /= bs;
                }
            }
            for (int q = 0; q < matrixLength; q++)
            {
                if (q == k)
                {
                    continue;
                }

                double bs = matrixTemp[q][k];

                for (int p = 0; p < matrixLength * 2; p++)
                {
                    matrixTemp[q][p] -= bs * matrixTemp[k][p];
                }
            }
        }
        for (int x = 0; x < matrixLength; x++)
        {
            for (int y = matrixLength; y < matrixLength * 2; y++)
            {
                result[x][y - matrixLength] = matrixTemp[x][y];
            }
        }

        return(result);
    }
Example #2
0
    private static double[][] Calc(IReadOnlyList <double> segment, double d, IReadOnlyList <double> segment2)
    {
        double[][] ret = JaggedArray.CreateJaggedArray <double[][]>(segment.Count, 1);

        for (int i = 0; i < segment.Count; i++)
        {
            ret[i][0] = segment[i] - d * segment2[i];
        }

        return(ret);
    }
Example #3
0
    public static double[][] MatrixMultiplication(double[][] a, double[][] b)
    {
        int hang = a.Length;
        int lie  = b[0].Length;

        double[][] result = JaggedArray.CreateJaggedArray <double[][]>(hang, lie);

        for (int i = 0; i < hang; i++)
        {
            for (int j = 0; j < lie; j++)
            {
                result[i][j] = b.Select((t, k) => a[i][k] * t[j]).Sum();
            }
        }

        return(result);
    }
Example #4
0
    public static List <double> DoFiltfilt(List <double> b, List <double> a, List <double> x)
    {
        if (b == null)
        {
            throw new ArgumentNullException(nameof(b));
        }
        if (a == null)
        {
            throw new ArgumentNullException(nameof(a));
        }
        if (x == null)
        {
            throw new ArgumentNullException(nameof(x));
        }

        int len   = x.Count;
        int na    = a.Count;
        int nb    = b.Count;
        int nFilt = nb > na ? nb : na;
        int nFact = 3 * (nFilt - 1);

        if (len <= nFact)
        {
            throw new Exception("Length X is too small");
        }

        Resize(b, nFilt, 0);
        Resize(a, nFilt, 0);

        List <int> rows = new List <int>();
        List <int> cols = new List <int>();

        AddIndexRange(rows, 0, nFilt - 2, 1);
        if (nFilt > 2)
        {
            AddIndexRange(rows, 1, nFilt - 2, 1);
            AddIndexRange(rows, 0, nFilt - 3, 1);
        }

        AddIndexConst(cols, 0, nFilt - 1);
        if (nFilt > 2)
        {
            AddIndexRange(cols, 1, nFilt - 2, 1);
            AddIndexRange(cols, 1, nFilt - 2, 1);
        }
        int           kLen = rows.Count;
        List <double> data = new List <double>();

        Resize(data, kLen, 0);
        data[0] = 1 + a[1];
        int j = 1;

        if (nFilt > 2)
        {
            for (int i = 2; i < nFilt; i++)
            {
                data[j++] = a[i];
            }
            for (int i = 0; i < nFilt - 2; i++)
            {
                data[j++] = 1.0;
            }
            for (int i = 0; i < nFilt - 2; i++)
            {
                data[j++] = -1.0;
            }
        }

        List <double> leftPad = SubvectorReverse(x, nFact, 1);

        leftPad = leftPad.Select(q => 2 * x[0] - q).ToList();

        List <double> rightPad = SubvectorReverse(x, len - 2, len - nFact - 1);

        rightPad = rightPad.Select(q => 2 * x[len - 1] - q).ToList();

        List <double> signal1 = new List <double>();
        List <double> signal2 = new List <double>();
        List <double> zi      = new List <double>();

        signal1.AddRange(leftPad);
        signal1.AddRange(x);
        signal1.AddRange(rightPad);

        double[][] sp = JaggedArray.CreateJaggedArray <double[][]>(rows.Max() + 1, cols.Max() + 1);

        for (int k = 0; k < kLen; ++k)
        {
            sp[rows[k]][cols[k]] = data[k];
        }

        double[][] zZi = MatrixMultiplication(MatrixInversion(sp), Calc(Segment(b.ToArray(), 1, nFilt - 1), b.ToArray()[0], Segment(a.ToArray(), 1, nFilt - 1)));

        Resize(zi, zZi.Length, 1);

        ChangeZi(zZi, zi, signal1[0]);

        Filter(b, a, signal1, signal2, zi);

        signal2.Reverse();

        ChangeZi(zZi, zi, signal2[0]);

        Filter(b, a, signal2, signal1, zi);

        List <double> y = SubvectorReverse(signal1, signal1.Count - nFact - 1, nFact);

        return(y);
    }