/**
   * 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;
  }
  @Override
  protected boolean initializeAlgorithm() {
    // Due to bizarre inheretance, this.data is the function to minimize...
    DifferentiableEvaluator<? super Vector, Double, Vector> f = this.data;

    this.result =
        new DefaultInputOutputPair<Vector, Double>(
            this.initialGuess.clone(), f.evaluate(this.initialGuess));

    this.gradient = f.differentiate(this.initialGuess);

    // Load up the line function with the current direction and
    // the search direction, which is the negative gradient, in other words
    // the direction of steepest descent
    this.lineFunction =
        new DirectionalVectorToDifferentiableScalarFunction(
            f, this.result.getInput(), this.gradient.scale(-1.0));

    this.dimensionality = this.initialGuess.getDimensionality();
    this.hessianInverse =
        MatrixFactory.getDefault()
            .createIdentity(this.dimensionality, this.dimensionality)
            .scale(0.5)
    //            .scale( Math.sqrt(this.getTolerance()) )
    ;

    return true;
  }
Ejemplo n.º 3
0
  /** Path & edge are the same, directions are reverse, neg. to pos. */
  @Test
  public void testPathStateConvert7() {

    final Path startPath =
        TestUtils.makeTmpPath(
            graph,
            true,
            new Coordinate(20, -10),
            new Coordinate(10, -10),
            new Coordinate(10, 0),
            new Coordinate(0, 0));
    final Matrix covar =
        MatrixFactory.getDefault()
            .copyArray(new double[][] {new double[] {126.56, 8.44}, new double[] {8.44, 0.56}});
    final MultivariateGaussian startBelief =
        new AdjMultivariateGaussian(VectorFactory.getDefault().createVector2D(-2.5d, 1d), covar);
    final PathStateDistribution currentBelief = new PathStateDistribution(startPath, startBelief);

    final Path newPath =
        TestUtils.makeTmpPath(graph, false, new Coordinate(10, 0), new Coordinate(0, 0));

    final PathStateDistribution result = currentBelief.convertToPath(newPath);

    AssertJUnit.assertEquals("distance", 7.5d, result.getMean().getElement(0), 0d);
    AssertJUnit.assertEquals("velocity", 1d, result.getMean().getElement(1), 0d);
  }
Ejemplo n.º 4
0
  @Test
  public void testPathStateConvert2() {
    final Path startPath =
        TestUtils.makeTmpPath(graph, false, new Coordinate(0, 0), new Coordinate(10, 0));

    final Matrix covar =
        MatrixFactory.getDefault()
            .copyArray(new double[][] {new double[] {126.56, 8.44}, new double[] {8.44, 0.56}});
    final MultivariateGaussian startBelief =
        new AdjMultivariateGaussian(VectorFactory.getDenseDefault().createVector2D(0d, 1d), covar);
    final PathStateDistribution currentBelief = new PathStateDistribution(startPath, startBelief);

    final Vector groundLoc =
        MotionStateEstimatorPredictor.getOg()
            .times(currentBelief.getGroundDistribution().getMean());
    AssertJUnit.assertEquals("initial state x", 0d, groundLoc.getElement(0), 0d);
    AssertJUnit.assertEquals("initial state y", 0d, groundLoc.getElement(1), 0d);

    final Path newPath =
        TestUtils.makeTmpPath(
            graph,
            true,
            new Coordinate(20, -10),
            new Coordinate(10, -10),
            new Coordinate(10, 0),
            new Coordinate(0, 0));

    final PathStateDistribution result = currentBelief.convertToPath(newPath);

    AssertJUnit.assertEquals("distance", -0d, result.getMean().getElement(0), 0d);
    AssertJUnit.assertEquals("velocity", -1d, result.getMean().getElement(1), 0d);
  }
  private static Matrix getVarianceConstraintMatrix(
      double aVariance, double a0Variance, Double angle) {

    if (angle == null) return MatrixFactory.getDefault().createIdentity(2, 2).scale(aVariance);

    final Matrix rotationMatrix = MatrixFactory.getDefault().createIdentity(2, 2);
    rotationMatrix.setElement(0, 0, Math.cos(angle));
    rotationMatrix.setElement(0, 1, -Math.sin(angle));
    rotationMatrix.setElement(1, 0, Math.sin(angle));
    rotationMatrix.setElement(1, 1, Math.cos(angle));

    final Matrix temp =
        MatrixFactory.getDefault()
            .createDiagonal(
                VectorFactory.getDefault().copyArray(new double[] {a0Variance, aVariance}));
    return rotationMatrix.times(temp).times(rotationMatrix.transpose());
  }
  private static Matrix createStateTransitionMatrix(double timeDiff) {

    final Matrix Gct = MatrixFactory.getDefault().createIdentity(4, 4);
    Gct.setElement(0, 1, timeDiff);
    Gct.setElement(2, 3, timeDiff);

    return Gct;
  }
    /**
     * Computes the pair-wise confidence test results
     *
     * @param data Data from each treatment to consider
     * @param pairwiseTest Confidence test used for pair-wise null-hypothesis tests.
     */
    protected void computePairwiseTestResults(
        final Collection<? extends Collection<? extends Number>> data,
        final NullHypothesisEvaluator<Collection<? extends Number>> pairwiseTest) {

      ArrayList<? extends Collection<? extends Number>> treatments =
          CollectionUtil.asArrayList(data);

      final int K = treatments.size();
      Matrix Z = MatrixFactory.getDefault().createMatrix(K, K);
      Matrix P = MatrixFactory.getDefault().createMatrix(K, K);
      ArrayList<ArrayList<ConfidenceStatistic>> stats =
          new ArrayList<ArrayList<ConfidenceStatistic>>(K);
      for (int i = 0; i < K; i++) {
        ArrayList<ConfidenceStatistic> comp = new ArrayList<ConfidenceStatistic>(K);
        for (int j = 0; j < K; j++) {
          comp.add(null);
        }
        stats.add(comp);
      }

      for (int i = 0; i < K; i++) {
        // Comparisons to ourselves are perfect
        Z.setElement(i, i, 0.0);
        P.setElement(i, i, 1.0);
        Collection<? extends Number> datai = treatments.get(i);
        for (int j = i + 1; j < K; j++) {
          Collection<? extends Number> dataj = treatments.get(j);
          ConfidenceStatistic comparison = pairwiseTest.evaluateNullHypothesis(datai, dataj);
          final double pij = comparison.getNullHypothesisProbability();
          final double zij = comparison.getTestStatistic();
          Z.setElement(i, j, zij);
          Z.setElement(j, i, zij);
          P.setElement(i, j, pij);
          P.setElement(j, i, pij);
          stats.get(i).set(j, comparison);
          stats.get(j).set(i, comparison);
        }
      }

      this.testStatistics = Z;
      this.nullHypothesisProbabilities = P;
      this.pairwiseTestStatistics = stats;
    }
Ejemplo n.º 8
0
 /**
  * Creates a uniform transition-probability Matrix
  *
  * @param numStates Number of states to create the Matrix for
  * @return Uniform probability Matrix.
  */
 protected static Matrix createUniformTransitionProbability(int numStates) {
   Matrix A = MatrixFactory.getDefault().createMatrix(numStates, numStates);
   final double p = 1.0 / numStates;
   for (int i = 0; i < numStates; i++) {
     for (int j = 0; j < numStates; j++) {
       A.setElement(i, j, p);
     }
   }
   return A;
 }
  private static Matrix createStateCovarianceMatrix(
      double timeDiff, double aVariance, double a0Variance, Double angle) {
    final Matrix A_half = MatrixFactory.getDefault().createMatrix(4, 2);
    A_half.setElement(0, 0, Math.pow(timeDiff, 2) / 2d);
    A_half.setElement(1, 0, timeDiff);
    A_half.setElement(2, 1, Math.pow(timeDiff, 2) / 2d);
    A_half.setElement(3, 1, timeDiff);

    final Matrix Q = getVarianceConstraintMatrix(aVariance, a0Variance, angle);
    final Matrix A = A_half.times(Q).times(A_half.transpose());
    return A;
  }
Ejemplo n.º 10
0
  @Test(enabled = false, expectedExceptions = IllegalStateException.class)
  public void testBadConversion() {
    final Path startPath =
        TestUtils.makeTmpPath(graph, true, new Coordinate(20, -10), new Coordinate(10, -10));

    final Matrix covar =
        MatrixFactory.getDefault()
            .copyArray(new double[][] {new double[] {126.56, 8.44}, new double[] {8.44, 0.56}});
    final MultivariateGaussian startBelief =
        new AdjMultivariateGaussian(
            VectorFactory.getDefault().createVector2D(-0d, -5d / 30d), covar);
    final PathStateDistribution currentBelief = new PathStateDistribution(startPath, startBelief);

    final Path newPath =
        TestUtils.makeTmpPath(
            graph,
            true,
            new Coordinate(20, -10),
            new Coordinate(10, -10),
            new Coordinate(10, 0),
            new Coordinate(0, 0));

    PathUtils.checkAndGetConvertedBelief(currentBelief, newPath);
  }
 /** Test of getDefault method, of class MatrixFactory. */
 public void testGetDefault() {
   System.out.println("getDefault");
   MatrixFactory<? extends Matrix> result = MatrixFactory.getDefault();
   assertSame(MatrixFactory.DEFAULT_DENSE_INSTANCE, result);
 }
 static {
   O = MatrixFactory.getDefault().createMatrix(2, 4);
   O.setElement(0, 0, 1);
   O.setElement(1, 2, 1);
 }
Ejemplo n.º 13
0
 public static Matrix vstack(Matrix... matricies) {
   return vstack(MatrixFactory.getDefault(), matricies);
 }