/**
* Performs sanity checks on the input data and reshapes internal matrices. By reshaping
* a matrix it will only declare new memory when needed.
*/
public void configure(DMatrixRMaj initParam, DMatrixRMaj X, DMatrixRMaj Y)
{
if (Y.getNumRows() != X.getNumRows())
{
throw new ArgumentException("Different vector lengths");
}
else if (Y.getNumCols() != 1 || X.getNumCols() != 1)
{
throw new ArgumentException("Inputs must be a column vector");
}
int numParam = initParam.getNumElements();
int numPoints = Y.getNumRows();
if (param.getNumElements() != initParam.getNumElements())
{
// reshaping a matrix means that new memory is only declared when needed
this.param.reshape(numParam, 1, false);
this.d.reshape(numParam, 1, false);
this.H.reshape(numParam, numParam, false);
this.negDelta.reshape(numParam, 1, false);
this.tempParam.reshape(numParam, 1, false);
this.A.reshape(numParam, numParam, false);
}
param.set(initParam);
// reshaping a matrix means that new memory is only declared when needed
temp0.reshape(numPoints, 1, false);
temp1.reshape(numPoints, 1, false);
tempDH.reshape(numPoints, 1, false);
jacobian.reshape(numParam, numPoints, false);
}
/**
* Converts {@link DMatrixRMaj} into {@link DMatrix6}
*
* @param input Input matrix.
* @param output Output matrix. If null a new matrix will be declared.
* @return Converted matrix.
*/
public static DMatrix6 convert(DMatrixRMaj input, DMatrix6 output)
{
if (output == null)
{
output = new DMatrix6();
}
if (input.getNumRows() != 1 && input.getNumCols() != 1)
{
throw new ArgumentException("One row or column must have a length of 1 for it to be a vector");
}
int length = Math.Max(input.getNumRows(), input.getNumCols());
if (length != 6)
{
throw new ArgumentException("Length of input vector is not 6. It is " + length);
}
output.a1 = input.data[0];
output.a2 = input.data[1];
output.a3 = input.data[2];
output.a4 = input.data[3];
output.a5 = input.data[4];
output.a6 = input.data[5];
return(output);
}
public static DMatrixSparseTriplet convert(DMatrixRMaj src, DMatrixSparseTriplet dst, double tol)
{
if (dst == null)
{
dst = new DMatrixSparseTriplet(src.numRows, src.numCols, src.numRows * src.numCols);
}
else
{
dst.reshape(src.numRows, src.numCols);
}
int index = 0;
for (int row = 0; row < src.numRows; row++)
{
for (int col = 0; col < src.numCols; col++)
{
double value = src.data[index++];
if (Math.Abs(value) > tol)
{
dst.addItem(row, col, value);
}
}
}
return(dst);
}
/**
* <p>
* Sets each element in the matrix to a value drawn from an Gaussian distribution with the specified mean and
* standard deviation
* </p>
*
* @param numRow Number of rows in the new matrix.
* @param numCol Number of columns in the new matrix.
* @param mean Mean value in the distribution
* @param stdev Standard deviation in the distribution
* @param randJava.Util.Random number generator used to fill the matrix.
*/
public static DMatrixRMaj rectangleGaussian(int numRow, int numCol, double mean, double stdev, Java.Util.Random rand)
{
DMatrixRMaj m = new DMatrixRMaj(numRow, numCol);
fillGaussian(m, mean, stdev, rand);
return(m);
}
/**
* Converts {@link DMatrix6} into {@link DMatrixRMaj}.
*
* @param input Input matrix.
* @param output Output matrix. If null a new matrix will be declared.
* @return Converted matrix.
*/
public static DMatrixRMaj convert(DMatrix6 input, DMatrixRMaj output)
{
if (output == null)
{
output = new DMatrixRMaj(6, 1);
}
if (output.getNumRows() != 1 && output.getNumCols() != 1)
{
throw new ArgumentException("One row or column must have a length of 1 for it to be a vector");
}
int length = Math.Max(output.getNumRows(), output.getNumCols());
if (length != 6)
{
throw new ArgumentException("Length of input vector is not 6. It is " + length);
}
output.data[0] = input.a1;
output.data[1] = input.a2;
output.data[2] = input.a3;
output.data[3] = input.a4;
output.data[4] = input.a5;
output.data[5] = input.a6;
return(output);
}
/**
* <p>
* The condition p = 2 number of a matrix is used to measure the sensitivity of the linear
* system <b>Ax=b</b>. A value near one indicates that it is a well conditioned matrix.<br>
* <br>
* κ<sub>2</sub> = ||A||<sub>2</sub>||A<sup>-1</sup>||<sub>2</sub>
* </p>
* <p>
* This is also known as the spectral condition number.
* </p>
*
* @param A The matrix.
* @return The condition number.
*/
public static double conditionP2(DMatrixRMaj A)
{
SingularValueDecomposition_F64 <DMatrixRMaj> svd =
DecompositionFactory_DDRM.svd(A.numRows, A.numCols, false, false, true);
svd.decompose(A);
double[] singularValues = svd.getSingularValues();
int n = SingularOps_DDRM.rank(svd, UtilEjml.TEST_F64);
if (n == 0)
{
return(0);
}
double smallest = double.MaxValue;
double largest = double.MinValue;
foreach (double s in singularValues)
{
if (s < smallest)
{
smallest = s;
}
if (s > largest)
{
largest = s;
}
}
return(largest / smallest);
}
/**
* Performs QR decomposition on A
*
* @param A not modified.
*/
public override bool setA(DMatrixRMaj A)
{
if (A.numRows < A.numCols)
{
throw new ArgumentException("Can't solve for wide systems. More variables than equations.");
}
if (A.numRows > maxRows || A.numCols > maxCols)
{
setMaxSize(A.numRows, A.numCols);
}
R.reshape(A.numCols, A.numCols);
a.reshape(A.numRows, 1);
temp.reshape(A.numRows, 1);
_setA(A);
if (!decomposer.decompose(A))
{
return(false);
}
gammas = decomposer.getGammas();
QR = decomposer.getQR();
decomposer.getR(R, true);
return(true);
}
/**
* Takes a matrix and splits it into a set of row or column vectors.
*
* @param A original matrix.
* @param column If true then column vectors will be created.
* @return Set of vectors.
*/
public static DMatrixRMaj[] splitIntoVectors(DMatrix1Row A, bool column)
{
int w = column ? A.numCols : A.numRows;
int M = column ? A.numRows : 1;
int N = column ? 1 : A.numCols;
int o = Math.Max(M, N);
DMatrixRMaj[] ret = new DMatrixRMaj[w];
for (int i = 0; i < w; i++)
{
DMatrixRMaj a = new DMatrixRMaj(M, N);
if (column)
{
subvector(A, 0, i, o, false, 0, a);
}
else
{
subvector(A, i, 0, o, true, 0, a);
}
ret[i] = a;
}
return(ret);
}
/**
* <p>
* Creates a pivot matrix that exchanges the rows in a matrix:
* <br>
* A' = P*A<br>
* </p>
* <p>
* For example, if element 0 in 'pivots' is 2 then the first row in A' will be the 3rd row in A.
* </p>
*
* @param ret If null then a new matrix is declared otherwise the results are written to it. Is modified.
* @param pivots Specifies the new order of rows in a matrix.
* @param numPivots How many elements in pivots are being used.
* @param transposed If the transpose of the matrix is returned.
* @return A pivot matrix.
*/
public static DMatrixRMaj pivotMatrix(DMatrixRMaj ret, int[] pivots, int numPivots, bool transposed)
{
if (ret == null)
{
ret = new DMatrixRMaj(numPivots, numPivots);
}
else
{
if (ret.numCols != numPivots || ret.numRows != numPivots)
{
throw new ArgumentException("Unexpected matrix dimension");
}
CommonOps_DDRM.fill(ret, 0);
}
if (transposed)
{
for (int i = 0; i < numPivots; i++)
{
ret.set(pivots[i], i, 1);
}
}
else
{
for (int i = 0; i < numPivots; i++)
{
ret.set(i, pivots[i], 1);
}
}
return(ret);
}
/**
* Performs L = L<sup>T</sup>*L
*/
public static void multLowerTranA(DMatrixRMaj mat)
{
int m = mat.numCols;
double[] L = mat.data;
for (int i = 0; i < m; i++)
{
for (int j = m - 1; j >= i; j--)
{
double val = 0;
for (int k = j; k < m; k++)
{
val += L[k * m + i] * L[k * m + j];
}
L[i * m + j] = val;
}
}
// copy the results into the lower portion
for (int i = 0; i < m; i++)
{
for (int j = 0; j < i; j++)
{
L[i * m + j] = L[j * m + i];
}
}
}
/**
* Copies just the upper or lower triangular portion of a matrix.
*
* @param src Matrix being copied. Not modified.
* @param dst Where just a triangle from src is copied. If null a new one will be created. Modified.
* @param upper If the upper or lower triangle should be copied.
* @return The copied matrix.
*/
public static DMatrixRMaj copyTriangle(DMatrixRMaj src, DMatrixRMaj dst, bool upper)
{
if (dst == null)
{
dst = new DMatrixRMaj(src.numRows, src.numCols);
}
else if (src.numRows != dst.numRows || src.numCols != dst.numCols)
{
throw new ArgumentException("src and dst must have the same dimensions.");
}
if (upper)
{
int N = Math.Min(src.numRows, src.numCols);
for (int i = 0; i < N; i++)
{
int index = i * src.numCols + i;
Array.Copy(src.data, index, dst.data, index, src.numCols - i);
}
}
else
{
for (int i = 0; i < src.numRows; i++)
{
int length = Math.Min(i + 1, src.numCols);
int index = i * src.numCols;
Array.Copy(src.data, index, dst.data, index, length);
}
}
return(dst);
}
private static void swapRowOrCol(DMatrixRMaj M, bool tran, int i, int bigIndex)
{
double tmp;
if (tran)
{
// swap the rows
for (int col = 0; col < M.numCols; col++)
{
tmp = M.get(i, col);
M.set(i, col, M.get(bigIndex, col));
M.set(bigIndex, col, tmp);
}
}
else
{
// swap the columns
for (int row = 0; row < M.numRows; row++)
{
tmp = M.get(row, i);
M.set(row, i, M.get(row, bigIndex));
M.set(row, bigIndex, tmp);
}
}
}
/**
* Performs a matrix inversion operations that takes advantage of the special
* properties of a covariance matrix.
*
* @param cov A covariance matrix. Not modified.
* @param cov_inv The inverse of cov. Modified.
* @return true if it could invert the matrix false if it could not.
*/
public static bool invert(DMatrixRMaj cov, DMatrixRMaj cov_inv)
{
if (cov.numCols <= 4)
{
if (cov.numCols != cov.numRows)
{
throw new ArgumentException("Must be a square matrix.");
}
if (cov.numCols >= 2)
{
UnrolledInverseFromMinor_DDRM.inv(cov, cov_inv);
}
else
{
cov_inv.data[0] = 1.0 / cov_inv.data[0];
}
}
else
{
LinearSolverDense <DMatrixRMaj> solver = LinearSolverFactory_DDRM.symmPosDef(cov.numRows);
// wrap it to make sure the covariance is not modified.
solver = new LinearSolverSafe <DMatrixRMaj>(solver);
if (!solver.setA(cov))
{
return(false);
}
solver.invert(cov_inv);
}
return(true);
}
/**
* Computes the d and H parameters. Where d is the average error gradient and
* H is an approximation of the hessian.
*/
private void computeDandH(DMatrixRMaj param, DMatrixRMaj x, DMatrixRMaj y)
{
func.compute(param, x, tempDH);
CommonOps_DDRM.subtractEquals(tempDH, y);
computeNumericalJacobian(param, x, jacobian);
int numParam = param.getNumElements();
int length = x.getNumElements();
// d = average{ (f(x_i;p) - y_i) * jacobian(:,i) }
for (int i = 0; i < numParam; i++)
{
double total = 0;
for (int j = 0; j < length; j++)
{
total += tempDH.get(j, 0) * jacobian.get(i, j);
}
d.set(i, 0, total / length);
}
// compute the approximation of the hessian
CommonOps_DDRM.multTransB(jacobian, jacobian, H);
CommonOps_DDRM.scale(1.0 / length, H);
}
/**
* Checks to see if the provided matrix is within tolerance to an identity matrix.
*
* @param mat Matrix being exaMined. Not modified.
* @param tol Tolerance.
* @return True if it is within tolerance to an identify matrix.
*/
public static bool isIdentity(DMatrixRMaj mat, double tol)
{
// see if the result is an identity matrix
int index = 0;
for (int i = 0; i < mat.numRows; i++)
{
for (int j = 0; j < mat.numCols; j++)
{
if (i == j)
{
if (!(Math.Abs(mat.get(index++) - 1) <= tol))
{
return(false);
}
}
else
{
if (!(Math.Abs(mat.get(index++)) <= tol))
{
return(false);
}
}
}
}
return(true);
}
//@Override
public void configure(DMatrixRMaj F, DMatrixRMaj Q, DMatrixRMaj H)
{
int dimenX = F.numCols;
x = new DMatrixRMaj(dimenX, 1);
P = new DMatrixRMaj(dimenX, dimenX);
eq = new Equation.Equation();
// Provide aliases between the symbolic variables and matrices we normally interact with
// The names do not have to be the same.
eq.alias(x, "x", P, "P", Q, "Q", F, "F", H, "H");
// Dummy matrix place holder to avoid compiler errors. Will be replaced later on
eq.alias(new DMatrixRMaj(1, 1), "z");
eq.alias(new DMatrixRMaj(1, 1), "R");
// Pre-compile so that it doesn't have to compile it each time it's invoked. More cumbersome
// but for small matrices the overhead is significant
predictX = eq.compile("x = F*x");
predictP = eq.compile("P = F*P*F' + Q");
updateY = eq.compile("y = z - H*x");
updateK = eq.compile("K = P*H'*inv( H*P*H' + R )");
updateX = eq.compile("x = x + K*y");
updateP = eq.compile("P = P-K*(H*P)");
}
/**
* <p>
* Given a set of polynomial coefficients, compute the roots of the polynomial. Depending on
* the polynomial being considered the roots may contain complex number. When complex numbers are
* present they will come in pairs of complex conjugates.
* </p>
*
* <p>
* Coefficients are ordered from least to most significant, e.g: y = c[0] + x*c[1] + x*x*c[2].
* </p>
*
* @param coefficients Coefficients of the polynomial.
* @return The roots of the polynomial
*/
public static Complex_F64[] findRoots(params double[] coefficients)
{
int N = coefficients.Length - 1;
// Construct the companion matrix
DMatrixRMaj c = new DMatrixRMaj(N, N);
double a = coefficients[N];
for (int i = 0; i < N; i++)
{
c.set(i, N - 1, -coefficients[i] / a);
}
for (int i = 1; i < N; i++)
{
c.set(i, i - 1, 1);
}
// use generalized eigenvalue decomposition to find the roots
EigenDecomposition_F64 <DMatrixRMaj> evd = DecompositionFactory_DDRM.eig(N, false);
evd.decompose(c);
Complex_F64[] roots = new Complex_F64[N];
for (int i = 0; i < N; i++)
{
roots[i] = evd.getEigenvalue(i);
}
return(roots);
}
/**
* Converts the transpose of a row major matrix into a row major block matrix.
*
* @param src Original DMatrixRMaj. Not modified.
* @param dst Equivalent DMatrixRBlock. Modified.
*/
public static void convertTranSrc(DMatrixRMaj src, DMatrixRBlock dst)
{
if (src.numRows != dst.numCols || src.numCols != dst.numRows)
{
throw new ArgumentException("Incompatible matrix shapes.");
}
for (int i = 0; i < dst.numRows; i += dst.blockLength)
{
int blockHeight = Math.Min(dst.blockLength, dst.numRows - i);
for (int j = 0; j < dst.numCols; j += dst.blockLength)
{
int blockWidth = Math.Min(dst.blockLength, dst.numCols - j);
int indexDst = i * dst.numCols + blockHeight * j;
int indexSrc = j * src.numCols + i;
for (int l = 0; l < blockWidth; l++)
{
int rowSrc = indexSrc + l * src.numCols;
int rowDst = indexDst + l;
for (int k = 0; k < blockHeight; k++, rowDst += blockWidth)
{
dst.data[rowDst] = src.data[rowSrc++];
}
}
}
}
}
/**
* <p>
* The condition number of a matrix is used to measure the sensitivity of the linear
* system <b>Ax=b</b>. A value near one indicates that it is a well conditioned matrix.<br>
* <br>
* κ<sub>p</sub> = ||A||<sub>p</sub>||A<sup>-1</sup>||<sub>p</sub>
* </p>
* <p>
* If the matrix is not square then the condition of either A<sup>T</sup>A or AA<sup>T</sup> is computed.
* <p>
* @param A The matrix.
* @param p p-norm
* @return The condition number.
*/
public static double conditionP(DMatrixRMaj A, double p)
{
if (p == 2)
{
return(conditionP2(A));
}
else if (A.numRows == A.numCols)
{
// square matrices are the typical case
DMatrixRMaj A_inv = new DMatrixRMaj(A.numRows, A.numCols);
if (!CommonOps_DDRM.invert(A, A_inv))
{
throw new ArgumentException("A can't be inverted.");
}
return(normP(A, p) * normP(A_inv, p));
}
else
{
DMatrixRMaj pinv = new DMatrixRMaj(A.numCols, A.numRows);
CommonOps_DDRM.pinv(A, pinv);
return(normP(A, p) * normP(pinv, p));
}
}
public static DMatrixRBlock convert(DMatrixRMaj A, int blockLength)
{
DMatrixRBlock ret = new DMatrixRBlock(A.numRows, A.numCols, blockLength);
convert(A, ret);
return(ret);
}
public static void mult(DMatrixSparseCSC A, DMatrixRMaj B, DMatrixRMaj C)
{
C.zero();
// C(i,j) = sum_k A(i,k) * B(k,j)
for (int k = 0; k < A.numCols; k++)
{
int idx0 = A.col_idx[k];
int idx1 = A.col_idx[k + 1];
for (int indexA = idx0; indexA < idx1; indexA++)
{
int i = A.nz_rows[indexA];
double valueA = A.nz_values[indexA];
int indexB = k * B.numCols;
int indexC = i * C.numCols;
int end = indexB + B.numCols;
// for (int j = 0; j < B.numCols; j++) {
while (indexB < end)
{
C.data[indexC++] += valueA * B.data[indexB++];
}
}
}
}
public static DMatrixRBlock convert(DMatrixRMaj A)
{
DMatrixRBlock ret = new DMatrixRBlock(A.numRows, A.numCols);
convert(A, ret);
return(ret);
}
/**
* Converts {@link DMatrixRMaj} into {@link DMatrix4x4}
*
* @param input Input matrix.
* @param output Output matrix. If null a new matrix will be declared.
* @return Converted matrix.
*/
public static DMatrix4x4 convert(DMatrixRMaj input, DMatrix4x4 output)
{
if (output == null)
{
output = new DMatrix4x4();
}
if (input.getNumRows() != output.getNumRows())
{
throw new ArgumentException("Number of rows do not match");
}
if (input.getNumCols() != output.getNumCols())
{
throw new ArgumentException("Number of columns do not match");
}
output.a11 = input.data[0];
output.a12 = input.data[1];
output.a13 = input.data[2];
output.a14 = input.data[3];
output.a21 = input.data[4];
output.a22 = input.data[5];
output.a23 = input.data[6];
output.a24 = input.data[7];
output.a31 = input.data[8];
output.a32 = input.data[9];
output.a33 = input.data[10];
output.a34 = input.data[11];
output.a41 = input.data[12];
output.a42 = input.data[13];
output.a43 = input.data[14];
output.a44 = input.data[15];
return(output);
}
/**
* <p>
* Creates a matrix where all but the diagonal elements are zero. The values
* of the diagonal elements are specified by the parameter 'vals'.
* </p>
*
* <p>
* To extract the diagonal elements from a matrix see {@link #diag()}.
* </p>
*
* @see CommonOps_DDRM#diag(double...)
*
* @param vals The values of the diagonal elements.
* @return A diagonal matrix.
*/
public static SimpleMatrix <T> diag(double[] vals)
{
DMatrixRMaj m = CommonOps_DDRM.diag(vals);
SimpleMatrix <T> ret = wrap(m as T);
return(ret);
}
/**
* Converts {@link DMatrix3x3} into {@link DMatrixRMaj}.
*
* @param input Input matrix.
* @param output Output matrix. If null a new matrix will be declared.
* @return Converted matrix.
*/
public static DMatrixRMaj convert(DMatrix3x3 input, DMatrixRMaj output)
{
if (output == null)
{
output = new DMatrixRMaj(3, 3);
}
if (input.getNumRows() != output.getNumRows())
{
throw new ArgumentException("Number of rows do not match");
}
if (input.getNumCols() != output.getNumCols())
{
throw new ArgumentException("Number of columns do not match");
}
output.data[0] = input.a11;
output.data[1] = input.a12;
output.data[2] = input.a13;
output.data[3] = input.a21;
output.data[4] = input.a22;
output.data[5] = input.a23;
output.data[6] = input.a31;
output.data[7] = input.a32;
output.data[8] = input.a33;
return(output);
}
/**
* <p>
* Returns true if the matrix is symmetric within the tolerance. Only square matrices can be
* symmetric.
* </p>
* <p>
* A matrix is symmetric if:<br>
* |a<sub>ij</sub> - a<sub>ji</sub>| ≤ tol
* </p>
*
* @param m A matrix. Not modified.
* @param tol Tolerance for how similar two elements need to be.
* @return true if it is symmetric and false if it is not.
*/
public static bool isSymmetric(DMatrixRMaj m, double tol)
{
if (m.numCols != m.numRows)
{
return(false);
}
double max = CommonOps_DDRM.elementMaxAbs(m);
for (int i = 0; i < m.numRows; i++)
{
for (int j = 0; j < i; j++)
{
double a = m.get(i, j) / max;
double b = m.get(j, i) / max;
double diff = Math.Abs(a - b);
if (!(diff <= tol))
{
return(false);
}
}
}
return(true);
}
/**
* Converts {@link DMatrixRMaj} into {@link DMatrixRBlock}
*
* Can't handle null output matrix since block size needs to be specified.
*
* @param src Input matrix.
* @param dst Output matrix.
*/
public static void convert(DMatrixRMaj src, DMatrixRBlock dst)
{
if (src.numRows != dst.numRows || src.numCols != dst.numCols)
{
throw new ArgumentException("Must be the same size.");
}
for (int i = 0; i < dst.numRows; i += dst.blockLength)
{
int blockHeight = Math.Min(dst.blockLength, dst.numRows - i);
for (int j = 0; j < dst.numCols; j += dst.blockLength)
{
int blockWidth = Math.Min(dst.blockLength, dst.numCols - j);
int indexDst = i * dst.numCols + blockHeight * j;
int indexSrcRow = i * dst.numCols + j;
for (int k = 0; k < blockHeight; k++)
{
Array.Copy(src.data, indexSrcRow, dst.data, indexDst, blockWidth);
indexDst += blockWidth;
indexSrcRow += dst.numCols;
}
}
}
}
/**
* <p>
* Checks to see if a matrix is orthogonal or isometric.
* </p>
*
* @param Q The matrix being tested. Not modified.
* @param tol Tolerance.
* @return True if it passes the test.
*/
public static bool isOrthogonal(DMatrixRMaj Q, double tol)
{
if (Q.numRows < Q.numCols)
{
throw new ArgumentException("The number of rows must be more than or equal to the number of columns");
}
DMatrixRMaj[] u = CommonOps_DDRM.columnsToVector(Q, null);
for (int i = 0; i < u.Count(); i++)
{
DMatrixRMaj a = u[i];
for (int j = i + 1; j < u.Count(); j++)
{
double val = VectorVectorMult_DDRM.innerProd(a, u[j]);
if (!(Math.Abs(val) <= tol))
{
return(false);
}
}
}
return(true);
}
public static DMatrixRMaj convert(DMatrixSparseCSC src, DMatrixRMaj dst)
{
if (dst == null)
{
dst = new DMatrixRMaj(src.numRows, src.numCols);
}
else
{
dst.reshape(src.numRows, src.numCols);
dst.zero();
}
int idx0 = src.col_idx[0];
for (int j = 1; j <= src.numCols; j++)
{
int idx1 = src.col_idx[j];
for (int i = idx0; i < idx1; i++)
{
int row = src.nz_rows[i];
double val = src.nz_values[i];
dst.unsafe_set(row, j - 1, val);
}
idx0 = idx1;
}
return(dst);
}
public void run()
{
DMatrixRMaj priorX = new DMatrixRMaj(9, 1, true, 0.5, -0.2, 0, 0, 0.2, -0.9, 0, 0.2, -0.5);
DMatrixRMaj priorP = CommonOps_DDRM.identity(9);
DMatrixRMaj trueX = new DMatrixRMaj(9, 1, true, 0, 0, 0, 0.2, 0.2, 0.2, 0.5, 0.1, 0.6);
List <DMatrixRMaj> meas = createSimulatedMeas(trueX);
DMatrixRMaj F = createF(T);
DMatrixRMaj Q = createQ(T, 0.1);
DMatrixRMaj H = createH();
foreach (KalmanFilter f in filters)
{
long timeBefore = DateTimeHelper.CurrentTimeMilliseconds;
f.configure(F, Q, H);
for (int trial = 0; trial < NUM_TRIALS; trial++)
{
f.setState(priorX, priorP);
processMeas(f, meas);
}
long timeAfter = DateTimeHelper.CurrentTimeMilliseconds;
Console.WriteLine("Filter = " + f.GetType().Name);
Console.WriteLine("Elapsed time: " + (timeAfter - timeBefore));
//System.gc();
GC.Collect();
GC.WaitForPendingFinalizers();
}
}
