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;
}
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());
}
@Override
public void measure(MultivariateGaussian belief, Vector observation) {
final Matrix C = this.model.getC();
// Figure out what the model says the observation should be
final Vector xpred = belief.getMean();
final Vector ypred = C.times(xpred);
// Update step... compute the difference between the observation
// and what the model says.
// Then compute the Kalman gain, which essentially indicates
// how much to believe the observation, and how much to believe model
final Vector innovation = observation.minus(ypred);
this.computeMeasurementBelief(belief, innovation, C);
// XXX covariance was set in the previous call
// if (!checkPosDef((DenseMatrix)belief.getCovariance()))
// return;
}