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); } }