/**
* 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;
}
/** 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);
}
@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;
}
/**
* 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;
}
@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);
}
public static Matrix vstack(Matrix... matricies) {
return vstack(MatrixFactory.getDefault(), matricies);
}
