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);
}
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);
}
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);
}
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);
}
