public void testForwardMessagingWorksForFiltering()
{
RandomVariable afterOneStep = robotHmm.forward(robotHmm.prior(),
HmmConstants.DO_NOTHING, HmmConstants.SEE_DOOR_OPEN);
Assert.AreEqual(0.75, afterOneStep
.getProbabilityOf(HmmConstants.DOOR_OPEN), TOLERANCE);
Assert.AreEqual(0.25, afterOneStep
.getProbabilityOf(HmmConstants.DOOR_CLOSED), TOLERANCE);
RandomVariable afterTwoSteps = robotHmm.forward(afterOneStep,
HmmConstants.PUSH_DOOR, HmmConstants.SEE_DOOR_OPEN);
Assert.AreEqual(0.983, afterTwoSteps
.getProbabilityOf(HmmConstants.DOOR_OPEN), TOLERANCE);
Assert.AreEqual(0.017, afterTwoSteps
.getProbabilityOf(HmmConstants.DOOR_CLOSED), TOLERANCE);
}
public RandomVariable smooth(String perception)
{
evidenceFromSmoothedStepToPresent.Add(perception);
Matrix O_t = hmm.sensorModel().asMatrix(perception);
Matrix transitionMatrix = hmm.transitionModel().asMatrix();
if (time > timelag)
{
forwardMessage = hmm.forward(forwardMessage, perception); // This
// seems
// WRONG
// I think this should be
// forwardMessage = hmm.forward(forwardMessage,
// evidenceFromSmoothedStepToPresent.get(0));
// this the perception at t-d. the book's algorithm
// uses the latest perception.
evidenceFromSmoothedStepToPresent.RemoveAt(0);
Matrix O_t_minus_d = hmm.sensorModel().asMatrix(
evidenceFromSmoothedStepToPresent[0]);
B = O_t_minus_d.inverse().times(
transitionMatrix.inverse().times(
B.times(transitionMatrix.times(O_t))));
}
else
{
B = B.times(transitionMatrix.times(O_t));
}
time += 1;
if (time > timelag)
{
Matrix one = hmm.prior().createUnitBelief().asMatrix();
Matrix forwardMatrix = forwardMessage.asMatrix();
RandomVariable result = hmm.prior().duplicate();
Matrix backwardMessage = (B.times(one));
result.updateFrom(forwardMatrix.arrayTimes(backwardMessage));
result.normalize();
return(result);
}
else
{
return(null);
}
}
