/** * Standard 2D tracking model with the following state equation: {@latex[ D_ x_t = G x_ t-1} + A * \epsilon_t} Also, when angle != null, a constraint matrix is created for the state covariance, * with perpendicular variance a0Variance. * * @param gVariance * @param aVariance * @param a0Variance * @param angle */ public Standard2DTrackingFilter( double gVariance, double aVariance, double a0Variance, Double angle) { super( VectorFactory.getDefault().createVector(4), createStateCovarianceMatrix(1d, aVariance, a0Variance, angle), MatrixFactory.getDefault().createIdentity(2, 2).scale(gVariance)); this.aVariance = aVariance; this.gVariance = gVariance; this.a0Variance = a0Variance; this.angle = angle; final LinearDynamicalSystem model = new LinearDynamicalSystem(0, 4); final Matrix Gct = createStateTransitionMatrix(currentTimeDiff); final Matrix G = MatrixFactory.getDefault().createIdentity(4, 4); G.setSubMatrix(0, 0, Gct); model.setA(G); model.setB(MatrixFactory.getDefault().createMatrix(4, 4)); model.setC(O); this.model = model; }
public static Matrix vstack(MatrixFactory<? extends Matrix> matrixFactory, Matrix... matricies) { int nrows = 0; int ncols = 0; for (Matrix matrix : matricies) { nrows += matrix.getNumRows(); ncols = matrix.getNumColumns(); } Matrix ret = matrixFactory.createMatrix(nrows, ncols); int currentRow = 0; for (Matrix matrix : matricies) { ret.setSubMatrix(currentRow, 0, matrix); currentRow += matrix.getNumRows(); } return ret; }