/**
* * Move the robot and update the distribution with the action model
*
* @param distance
* @param _heading
* @param _sensorModel
*/
private void move(
float distance, Heading _heading, ActionModel _actionModel, SensorModel _sensorModel) {
// move robot
m_robot.translate(m_translationAmount);
// now update estimate of position using the action model
m_distribution = _actionModel.updateAfterMove(m_distribution, _heading);
// if visualising, update the shown distribution
if (m_mapVis != null) {
m_mapVis.setDistribution(m_distribution);
}
// A short delay so we can see what's going on
Delay.msDelay(1000);
m_distribution =
_sensorModel.updateAfterSensing(m_distribution, _heading, m_robot.getRangeValues());
// if visualising, update the shown distribution
if (m_mapVis != null) {
m_mapVis.setDistribution(m_distribution);
}
// A short delay so we can see what's going on
Delay.msDelay(1000);
}
public RandomVariable perceptionUpdate(RandomVariable aBelief, String perception) {
RandomVariable newBelief = aBelief.duplicate();
// one way - use matrices
Matrix beliefMatrix = aBelief.asMatrix();
Matrix o_matrix = sensorModel.asMatrix(perception);
Matrix updated = o_matrix.times(beliefMatrix);
newBelief.updateFrom(updated);
newBelief.normalize();
return newBelief;
// alternate way of doing this. clearer in intent.
// for (String state : aBelief.states()){
// double probabilityOfPerception= sensorModel.get(state,perception);
// newBelief.setProbabilityOf(state,probabilityOfPerception *
// aBelief.getProbabilityOf(state));
// }
}
public RandomVariable calculate_next_backward_message(
RandomVariable forwardBelief, RandomVariable present_backward_message, String perception) {
RandomVariable result = present_backward_message.duplicate();
// System.out.println("fb :-calculating new backward message");
// System.out.println("fb :-diagonal matrix from sens model = ");
Matrix oMatrix = sensorModel.asMatrix(perception);
// System.out.println(oMatrix);
Matrix transitionMatrix = transitionModel.asMatrix(); // action
// should
// be
// passed
// in
// here?
// System.out.println("fb :-present backward message = "
// +present_backward_message);
Matrix backwardMatrix =
transitionMatrix.times(oMatrix.times(present_backward_message.asMatrix()));
Matrix resultMatrix = backwardMatrix.arrayTimes(forwardBelief.asMatrix());
result.updateFrom(resultMatrix);
result.normalize();
// System.out.println("fb :-normalized new backward message = "
// +result);
return result;
}
