public ParticleSet perceptionUpdate(String perception, Randomizer r) { // compute Particle Weight foreach (Particle p in particles) { double particleWeight = hmm.sensorModel().get(p.getState(), perception); p.setWeight(particleWeight); } // weighted sample to create new ParticleSet ParticleSet result = new ParticleSet(hmm); while (result.size() != size()) { foreach (Particle p in particles) { double probability = r.nextDouble(); if (probability <= p.getWeight()) { if (result.size() < size()) { result.add(new Particle(p.getState(), p.getWeight())); } } } } return(result); }
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); } }