final int xDim = posteriorState.getInputDimensionality();
final Matrix Ij = MatrixFactory.getDefault().createIdentity(xDim, xDim);
final Matrix H = MatrixFactory.getDefault().createMatrix(xDim, xDim * 2);
H.setSubMatrix(0, 0, Ij);
H.setSubMatrix(0, xDim, MatrixFactory.getDefault().createDiagonal(predState.getStateSample()));
final Vector postStateSample = posteriorState.sample(this.rng);
final MultivariateGaussian priorPhi = predState.getPsiSS().get(predState.getClassId());
final Vector phiPriorSmpl = priorPhi.sample(this.rng);
final Vector xHdiff = postStateSample.minus(H.times(phiPriorSmpl));
final double newN = invScaleSS.getShape() + 1d;
final double d = invScaleSS.getScale() + xHdiff.dotProduct(xHdiff);
invScaleSS.setScale(d);
invScaleSS.setShape(newN);
// FIXME TODO: crappy sampler
final double newInvScaleSmpl = invScaleSS.sample(this.rng);
/*
* Update state and measurement covariances, which
* have a strict dependency in this model (equality).
*/
kf.setMeasurementCovariance(MatrixFactory.getDefault().createDiagonal(
VectorFactory.getDefault().createVector(kf.getModel().getOutputDimensionality(),
newInvScaleSmpl)));
kf.setModelCovariance(MatrixFactory.getDefault().createDiagonal(
VectorFactory.getDefault().createVector(kf.getModel().getStateDimensionality(),
newInvScaleSmpl)));
/*
* Update offset and AR(1) prior(s).
* Note that we divide out the previous inv scale param, since
* we want to update A alone.
*/
final Matrix priorAInv = priorPhi.getCovariance().scale(1d/predState.getInvScaleSample()).inverse();
/*
* TODO FIXME: we don't have a generalized outer product, so we're only
* supporting the 1d case for now.
*/
final Vector Hv = H.convertToVector();
final Matrix postAInv = priorAInv.plus(Hv.outerProduct(Hv)).inverse();
// TODO FIXME: ewww. inverse.
final Vector postPhiMean = postAInv.times(priorAInv.times(phiPriorSmpl).plus(
H.transpose().times(postStateSample)));
final MultivariateGaussian postPhi = systemOffsetsSS.get(predState.getClassId());
postPhi.setMean(postPhiMean);
postPhi.setCovariance(postAInv.scale(newInvScaleSmpl));
final Vector postPhiSmpl = postPhi.sample(this.rng);
final Matrix smplArTerms = MatrixFactory.getDefault().createDiagonal(
postPhiSmpl.subVector(
postPhiSmpl.getDimensionality()/2,
postPhiSmpl.getDimensionality() - 1));
kf.getModel().setA(smplArTerms);
final Vector smplOffsetTerm = postPhiSmpl.subVector(0,
postPhiSmpl.getDimensionality()/2 - 1);
kf.getModel().setState(smplOffsetTerm);
kf.setCurrentInput(smplOffsetTerm);
final GaussianArHpTransitionState postState =