public MonteCarloLocalization(AutonomyConfiguration configuration)
{
// Assign fields
this.configuration = configuration;
// Initialize random number generator
MathHelper.random = new Random((int)DateTime.Now.Ticks);
// Initialize field
field = new Field(FieldWidth, FieldHeight, DigX, ObstacleX, configuration.WallMargin);
}
public Particle(Field field, double x, double y, double angle)
{
// Assign fields
this.field = field;
this.x = x;
this.y = y;
this.angle = angle;
// Create rangefinders
rangeFinderFront = new RangeFinderModel(field, this, 0.0, 46.99);
rangeFinderRear = new RangeFinderModel(field, this, 180.0, -18*2.54);
}
public RangeFinderModel(Field field, MovingObject entity, double mountAngle, double offset)
{
// Initialize fields
this.field = field;
this.entity = entity;
this.mountAngle = mountAngle;
this.offset = offset;
// Calculate angle delta
this.angleDelta = (EndSweep - BeginSweep) / ((double)MeasurementCount - 1.0);
// Calculate starting angle
startingAngle = mountAngle + entity.Angle - ((EndSweep - BeginSweep) / 2.0);
// Standard deviation
standardDeviation = Math.Sqrt(Math.Sqrt(Variance));
}
