private ICoordinate GenerateCoordinates(IEnumerable <IDistance> distances, IList <PJayParticle> particles,
out IList <PJayParticle> trainedParticles, bool secondTime)
{
var weights = new List <double>(_numberOfParticles);
var maxWeight = 0d;
distances = distances.OrderBy(x => x.Distance);
IDistance[] distanceArray = distances.ToArray();
double distanceSum = distanceArray.Sum(x => x.Distance);
foreach (PJayParticle pJay in particles)
{
double prob = 1;
foreach (IDistance distance in distanceArray)
{
IAnchor anchor = _anchors[distance.FromAnchorId];
double dist = Math.Sqrt(Math.Pow(pJay.X - anchor.X, 2) + Math.Pow(pJay.Z - anchor.Z, 2));
prob *= GaussianProbabilityDistribution(dist, pJay.NumberOfCircleDistances, distance.Distance)
// *(1 - (distance.Distance / distanceSum))
;
}
weights.Add(prob);
if (maxWeight < prob)
{
maxWeight = prob;
}
}
var newParticles = new List <PJayParticle>(_numberOfParticles);
double beta = (secondTime ? 0.9 : 0.7) * maxWeight;
var resultPosition = new Coordinate(0, 0, 0);
var weightTotal = 0d;
for (var i = 0; i < _numberOfParticles; i++)
{
if (beta < weights[i])
{
newParticles.Add(particles[i]);
resultPosition.X += particles[i].X * weights[i];
resultPosition.Z += particles[i].Z * weights[i];
weightTotal += weights[i];
}
}
resultPosition.X /= weightTotal;
resultPosition.Z /= weightTotal;
if (!secondTime)
{
var i = 0;
int currentParticleNumber = newParticles.Count;
while (_numberOfParticles > newParticles.Count)
{
i %= currentParticleNumber;
double xx = newParticles[i].X + Random.Next(-_numberOfCircleDistances, _numberOfCircleDistances);
double zz = newParticles[i].Z + Random.Next(-_numberOfCircleDistances, _numberOfCircleDistances);
var particle = new PJayParticle(xx, zz);
particle.SetNoise(_numberOfCircleDistances);
newParticles.Add(particle);
i++;
}
}
particles = newParticles;
trainedParticles = newParticles;
if (!secondTime)
{
return(GenerateCoordinates(distanceArray, particles, out trainedParticles, true));
}
return(resultPosition);
}
public ICoordinate GetResult(int objectId, params IDistance[] distances)
{
double minH = double.MaxValue;
double maxH = 0;
double maxW = 0;
double minW = double.MaxValue;
foreach (IDistance distance in distances)
{
IAnchor anchor = _anchors[distance.FromAnchorId];
if (maxH < anchor.Z + distance.Distance)
{
maxH = anchor.Z + distance.Distance;
}
if (minH > anchor.Z - distance.Distance)
{
minH = anchor.Z - distance.Distance;
}
if (maxW < anchor.X + distance.Distance)
{
maxW = anchor.X + distance.Distance;
}
if (minW > anchor.X - distance.Distance)
{
minW = anchor.X - distance.Distance;
}
}
maxH += 50;
minH -= 50;
maxW += 50;
minW -= 50;
var worldSizeInfo = new WorldSizeInfo();
worldSizeInfo.WorldSizeWidthMin = minW;
worldSizeInfo.WorldSizeWidthMax = maxW;
worldSizeInfo.WorldSizeHeightMin = minH;
worldSizeInfo.WorldSizeHeightMax = maxH;
List <PJayParticle> particles;
bool doesExist = false;
if (_particles.ContainsKey(objectId))
{
particles = _particles[objectId].Particles;
doesExist = true;
}
else
{
particles = new List <PJayParticle>(_numberOfParticles);
for (int i = 0; i < _numberOfParticles; i++)
{
double x = Random.Next((int)worldSizeInfo.WorldSizeWidthMin,
(int)worldSizeInfo.WorldSizeWidthMax);
double z = Random.Next((int)worldSizeInfo.WorldSizeHeightMin,
(int)worldSizeInfo.WorldSizeHeightMax);
var particle = new PJayParticle(x, z);
particle.SetNoise(_numberOfCircleDistances);
particles.Add(particle);
}
_particles.Add(objectId, new ParticleHistory(10)
{
Particles = particles,
});
}
// particles = new List<PJayParticle>(_numberOfParticles);
ICoordinate result = GenerateCoordinates(distances, particles, out IList <PJayParticle> trainedParticles, doesExist);
double averageX = trainedParticles.Average(x => x.X);
double averageZ = trainedParticles.Average(x => x.Z);
for (int i = trainedParticles.Count; i < _numberOfParticles; i++)
{
double x = averageX + Random.Next(-_numberOfCircleDistances, _numberOfCircleDistances);
double z = averageZ + Random.Next(-_numberOfCircleDistances, _numberOfCircleDistances);
var particle = new PJayParticle(x, z);
particle.SetNoise(_numberOfCircleDistances);
trainedParticles.Add(particle);
}
_particles[objectId].Particles = trainedParticles as List <PJayParticle> ?? trainedParticles.ToList();
DateTime now = DateTime.Now;
if (_particles[objectId].GetAcceleration(result, now, out double acceleration) && !double.IsNaN(acceleration))
{
double std = _particles[objectId].AccelerationStd;
if (!double.IsNaN(std))
{
var kalmanFilter = new KalmanFilter2D(3, acceleration, _particles[objectId].AccelerationStd);
foreach (CoordinateHistory coordinateHistory in _particles[objectId].GeneratedCoordinate)
{
kalmanFilter.Push(coordinateHistory.Coordinate.X, coordinateHistory.Coordinate.Z);
}
result.X = (result.X + kalmanFilter.X) * .5;
result.Z = (result.Z + kalmanFilter.Y) * .5;
}
}
_particles[objectId].AddHistoric(result, now);
return(result);
}
