// Does the posterior evidence at time t agree with the internal state at time t?
private bool Ut(PosteriorEvidence et, int t)
{
if (et.LaneProbabilities.Count != 0)
{
double with = 0.0;
double against = 0.0;
// get probabilities
foreach (KeyValuePair<ArbiterLane, double> est in et.LaneProbabilities)
{
if (est.Key.LaneId.Equals(InternalState[t].Initial) || est.Key.LaneId.Equals(InternalState[t].Target))
{
with += est.Value;
}
else
{
against += est.Value;
}
}
// make prob
Probability agreement = (new Probability(with, against, true)).Normalize();
// consistency
bool ct = this.Ct(this.PosteriorEvidence);
CoreCommon.CurrentInformation.LAConsistent = ct.ToString();
// check if this is greater than, say 40% in agreement
if (ct)
return agreement.T > 0.2 ? true : false;
else
return true;
}
else
{
return true;
}
}
/// <summary>
/// Updates evidence
/// </summary>
/// <param name="observations"></param>
public void UpdateEvidence(List<AreaEstimate> observations)
{
// dictionary mapping lanes to values
Dictionary<ArbiterLane, double> areaEstimates = new Dictionary<ArbiterLane, double>();
// loop over area possibilities
foreach (AreaEstimate ae in observations)
{
// do nothing for now and return first for nav tests
if (ae.AreaType == StateAreaType.Lane)
{
// get lane
ArbiterLane al = this.GetLane(ae.AreaId);
// assign
if (areaEstimates.ContainsKey(al))
{
// update
areaEstimates[al] = areaEstimates[al] + ae.Probability;
}
else
{
// add new
areaEstimates.Add(al, ae.Probability);
}
}
}
// output
if(this.InternalState.Count > 0)
{
foreach(KeyValuePair<ArbiterLane, double> kvp in areaEstimates)
{
if (kvp.Key.LaneId.Equals(this.InternalState[this.InternalState.Count - 1].Initial))
{
CoreCommon.CurrentInformation.LAPosteriorProbInitial = kvp.Value.ToString("F6");
}
if (kvp.Key.LaneId.Equals(this.InternalState[this.InternalState.Count - 1].Target))
{
CoreCommon.CurrentInformation.LAPosteriorProbTarget = kvp.Value.ToString("F6");
}
}
}
if (this.FilteredEstimate.Count > 0)
{
Probability p = this.FilteredEstimate[this.FilteredEstimate.Count - 1];
CoreCommon.CurrentInformation.LAProbabilityCorrect = p.T.ToString("F6");
}
// new estimate
PosteriorEvidence pe = new PosteriorEvidence(areaEstimates);
// add
PosteriorEvidence.Add(pe);
}
