/**
   * 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;
  }
Ejemplo n.º 2
0
 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;
 }