@Override
public GaussianArHpWfParticle update(
GaussianArHpWfParticle predState) {
final MultivariateGaussian posteriorState = predState.getState().clone();
final KalmanFilter kf = predState.getFilter().clone();
kf.update(posteriorState, predState.getObservation().getObservedValue());
/*
* The following are the parameter learning updates;
* they can be done off-line, but we'll do them now.
* TODO FIXME check that the input/offset thing is working!
*/
final InverseGammaDistribution scaleSS = predState.getSigma2SS().clone();
final MultivariateGaussian systemOffsetsSS = predState.getPsiSS().clone();
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();
final Vector phiPriorSmpl = priorPhi.sample(this.rng);
final Vector xHdiff = postStateSample.minus(H.times(phiPriorSmpl));
final double newN = scaleSS.getShape() + 1d;
final double d = scaleSS.getScale() + xHdiff.dotProduct(xHdiff);
scaleSS.setScale(d);
scaleSS.setShape(newN);
// FIXME TODO: crappy sampler
final double newScaleSmpl = scaleSS.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(),
newScaleSmpl)));
kf.setModelCovariance(MatrixFactory.getDefault().createDiagonal(
VectorFactory.getDefault().createVector(kf.getModel().getStateDimensionality(),
newScaleSmpl)));
/*
* Update offset and AR(1) prior(s).
* Note that we divide out the previous scale param, since
* we want to update A alone.
*/
final Matrix priorAInv = priorPhi.getCovariance().scale(1d/predState.getSigma2Sample()).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;
postPhi.setMean(postPhiMean);
postPhi.setCovariance(postAInv.scale(newScaleSmpl));
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);