public Particle transformParticle(PFAction act, Particle p)
{
//Particle nextParticle = new Particle(p);
Particle nextParticle = new Particle();
//nextParticle.variables = p.variables;
nextParticle.prob = p.prob;
double maxProb = minimumP;
string[] statekeys = p.variables.Keys.ToArray();
foreach (string state in statekeys)
{
double stateP = p.variables[state];
if (stateP < minimumP)
{
stateP = minimumP;
}
foreach (string actv in act.actParms.Keys)
{
double actP = act.actParms[actv];
string key = String.Format("{0}|{1}", state, actv);
if (state_act_tansition.ContainsKey(key))
{
Estimator myEstimate = state_act_tansition[key];
// fill in each facet
foreach (string facet in myEstimate.estimate.Keys)
{
double estP = (myEstimate.estimate[facet] * stateP * actP);
double noise = (estP * 0.01 * rgen.NextDouble()) - (estP * 0.005);
estP = Math.Abs(estP + noise);
//double estP = (myEstimate.estimate[facet] * stateP) ;
if ((!nextParticle.variables.ContainsKey(facet)) || (estP > nextParticle.variables[facet]))
{
nextParticle.variables[facet] = estP;
nextParticle.prob = estP;
}
if (estP > maxProb)
{
maxProb = estP;
}
// Then what ???
}
}
else
{
Console.WriteLine("Not found:'{0}'", key);
}
}
}
if (maxProb < minimumP)
{
maxProb = minimumP;
}
nextParticle.prob = maxProb; // go with the strongest frame facet for now
if (maxProb == 0)
{
Console.WriteLine("maxProb==0");
}
return(nextParticle);
}
public void addStateActTransition(string rule)
{
// "state|act=prob:nextstate|nextstate|..."
string[] parms = rule.Split('=');
string key = parms[0];
string[] keySplit = key.Split('|');
string state = keySplit[0];
string act = keySplit[1];
string strResult = parms[1];
string[] resultSplit = strResult.Split(':');
string strProb = resultSplit[0];
double prob = double.Parse(strProb);
string strNextList = resultSplit[1];
string[] nextList = strNextList.Split('|');
if (!state_act_tansition.ContainsKey(key))
{
state_act_tansition.Add(key, new Estimator());
}
Estimator myEstimate = state_act_tansition[key];
myEstimate.rule = rule;
myEstimate.state = state;
myEstimate.aux = act;
foreach (string nextState in nextList)
{
if (myEstimate.estimate.ContainsKey(nextState))
{
myEstimate.estimate[nextState] = prob;
}
else
{
myEstimate.estimate.Add(nextState, prob);
}
if (!stateList.Contains(nextState))
{
stateList.Add(nextState);
}
}
if (!stateList.Contains(state))
{
stateList.Add(state);
}
if (!actList.Contains(act))
{
actList.Add(act);
}
}
// We're building a frame slot/value system
// where the frame is indexed as "the world I am experiencing now"
// Or like Horswill's role passing system, with the particles as
// "the world I am tracking"
public double probParticleGeneratedObservation(Observation obsrv, Particle p)
{
double prob = 1;
string[] statekeys = p.variables.Keys.ToArray();
foreach (string state in statekeys)
{
double stateP = p.variables[state];
double pgen = 1;
if (stateP < minimumP)
{
stateP = minimumP;
}
foreach (string sense in obsrv.observable.Keys)
{
double senseP = obsrv.observable[sense];
string key = String.Format("{0}|{1}", state, sense);
if (state_sense_prob.ContainsKey(key))
{
Estimator myEstimate = state_sense_prob[key];
foreach (string facet in myEstimate.estimate.Keys)
{
// should we overwrite or just multiply mix in ?
//p.variables[facet] = p.variables[facet]* myEstimate.estimate[facet];
pgen = pgen * (myEstimate.estimate[facet] * senseP);
}
// Then what ???
}
else
{
Console.WriteLine("Not found:'{0}'", key);
}
}
double finP = stateP * pgen;
p.variables[state] = finP;
//Console.WriteLine("obv.state({0}) = {1}", state, finP);
// p.variables[state] = pgen;
}
// Do the final conjunction of particle facets computation ?
prob = minimumP;
foreach (string facet in statekeys)
{
//prob=prob* p.variables[facet];
if (p.variables[facet] > prob)
{
prob = p.variables[facet];
}
}
//Console.WriteLine("fp={0}\n", prob);
return(prob);
}
// Should do XML versions of these
public void addStateSenseProb(string rule)
{
// "state|sense=prob"
string [] parms = rule.Split('=');
string key = parms[0];
string strProb = parms[1];
string [] keySplit = key.Split('|');
string state = keySplit [0];
string aux = keySplit[1];
if (!state_sense_prob.ContainsKey(key))
{
state_sense_prob.Add(key, new Estimator());
}
Estimator myEstimate = state_sense_prob[key];
myEstimate.rule = rule;
myEstimate.state = state;
myEstimate.aux = aux;
if (myEstimate.estimate.ContainsKey(state))
{
myEstimate.estimate[state] = double.Parse(strProb);
}
else
{
myEstimate.estimate.Add(state, double.Parse(strProb));
}
if (!stateList.Contains(state))
{
stateList.Add(state);
}
if (!senseList.Contains(aux))
{
senseList.Add(aux);
}
}
