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();
int distanceCount = distanceArray.Length;
double weightStep = 1 / (double)distanceCount;
var intersectionCenters = Enumerable.ToArray(GetIntersectionCenters(distanceArray));
foreach (PJayParticle pJay in particles)
{
double prob = 1;
var pCoordinate = new Coordinate(pJay.X, pJay.Z, 0);
foreach (ICoordinate intersectionCenter in intersectionCenters)
{
var dist = _euclidean.Distance(new[] { pJay.X, pJay.Z },
new[] { intersectionCenter.X, intersectionCenter.Z });
// prob *= Normal.Gaussian2D(Math.Pow(80,2), dist, 0);
prob *= GaussianProbabilityDistribution(dist, 150, 0);
}
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 (int 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;
var xx = newParticles[i].X +
ParticleFilter.ParticleFilter.Random.Next(-_numberOfCircleDistances,
_numberOfCircleDistances);
var zz = newParticles[i].Z +
ParticleFilter.ParticleFilter.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 id, params IDistance[] distances)
{
// if (CheckIfAnchorsColliding(distances))
// {
// // TODO: Kesişim yoksa ParticleFilter algoritmasının çalışması gerekiyor.
// return null;
// }
double minH = double.MaxValue;
double maxH = 0;
double maxW = 0;
double minW = double.MaxValue;
foreach (IDistance distance in distances)
{
var 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;
List <PJayParticle> particles;
bool doesExist = false;
if (_particles.ContainsKey(id))
{
particles = _particles[id].Particles;
doesExist = true;
}
else
{
particles = new List <PJayParticle>(_numberOfParticles);
for (int i = 0; i < _numberOfParticles; i++)// todo : Create Semiphore ~20 Threads
{
double x = ParticleFilter.ParticleFilter.Random.Next((int)minW,
(int)maxW);
double y = ParticleFilter.ParticleFilter.Random.Next((int)minH,
(int)maxH);
var particle = new PJayParticle(x, y);
particle.SetNoise(_numberOfCircleDistances);
particles.Add(particle);
}
_particles.Add(id, new ParticleHistory(10)
{
Particles = particles,
});
}
var result = GenerateCoordinates2(distances, particles, out IList <PJayParticle> trainedParticles, doesExist);
var averageX = trainedParticles.Average(x => x.X);
var averageY = trainedParticles.Average(x => x.Z);
for (int i = trainedParticles.Count; i < _numberOfParticles; i++)
{
double x = averageX + ParticleFilter.ParticleFilter.Random.Next(
-_numberOfCircleDistances, _numberOfCircleDistances);
double y = averageY + ParticleFilter.ParticleFilter.Random.Next(
-_numberOfCircleDistances, _numberOfCircleDistances);
var particle = new PJayParticle(x, y);
particle.SetNoise(_numberOfCircleDistances);
trainedParticles.Add(particle);
}
_particles[id].Particles = trainedParticles as List <PJayParticle> ?? trainedParticles.ToList();
var now = DateTime.Now;
if (_particles[id].GetAcceleration(result, now, out var acceleration) && !double.IsNaN(acceleration))
{
var std = _particles[id].AccelerationStd;
if (!double.IsNaN(std))
{
var kalmanFilter = new KalmanFilter2D(3, acceleration, _particles[id].AccelerationStd);
foreach (CoordinateHistory coordinateHistory in _particles[id].GeneratedCoordinate)
{
kalmanFilter.Push(coordinateHistory.Coordinate.X, coordinateHistory.Coordinate.Z);
}
result.X = (result.X + kalmanFilter.X) * .5;
result.Z = (result.Z + kalmanFilter.Y) * .5;
}
}
_particles[id].AddHistoric(result, now);
return(result);
}
