internal double GetDiscreteDistResult(UnivariateDiscreteDistribution discreteDistribution, ListToPoint <double> function)
{
var inputList = new List <double>();
for (int i = 0; i < 1000; i++)
{
inputList.Add(discreteDistribution.Generate());
}
return(function(inputList));
}
/// <summary>
/// Obtain several measurements from the hidden state.
/// Ladmarks may be misdetected.
/// </summary>
/// <param name="time">Provides a snapshot of timing values.</param>
/// <returns>Pixel-range measurements.</returns>
public override List <MeasurementT> Measure(GameTime time)
{
List <MeasurementT> measurements = new List <MeasurementT>();
Map visible = new Map(3);
DataAssociation = new List <int>();
double[][] diraccov = new double[3][] { new double[3] {
0.001, 0, 0
},
new double[3] {
0, 0.001, 0
},
new double[3] {
0, 0, 0.001
} };
// add every measurement with probability = DetectionProbability
for (int i = 0; i < Landmarks.Count; i++)
{
double pd = DetectionProbability(Landmarks[i]);
if (pd > 0)
{
if (Util.Uniform.Next() < pd)
{
measurements.Add(MeasureDetected(Landmarks[i]));
DataAssociation.Add(i);
visible.Add(new Gaussian(Landmarks[i], diraccov, 1.0));
}
else
{
visible.Add(new Gaussian(Landmarks[i], diraccov, 0.0));
// weight indicates visible but not detected
}
}
}
// poisson distributed clutter measurement count
// a cap of 10 lambda is enforced because technically
// the number is unbounded so it could freeze the system
int nclutter;
try {
// the poisson generator may underflow if lambda is too small
nclutter = Math.Min(clutterGen.Generate(), (int)(ClutterCount * 10));
}
catch (ArithmeticException)
{
nclutter = 0;
}
for (int i = 0; i < nclutter; i++)
{
measurements.Add(Measurer.RandomMeasure());
DataAssociation.Add(int.MinValue);
}
MappedMeasurements.Clear();
foreach (MeasurementT z in measurements)
{
MappedMeasurements.Add(Measurer.MeasureToMap(Pose, z));
}
WayVisibleMaps.Add(Tuple.Create(time.TotalGameTime.TotalSeconds, visible));
return(measurements);
}
