/**
* 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;
}