public void push_test()
{
Generator.Seed = 0;
#region doc_push
// Let's say we have the following sequence of coordinate points:
double[] x = Vector.Range(0.0, 100.0); // z(t) = (t, 4.2*t + noise)
double[] y = x.Apply(x_i => 4.2 * x_i + Generator.Random.NextDouble());
// Create a new Kalman filter
var kf = new KalmanFilter2D();
// Push the points into the filter
for (int i = 0; i < x.Length; i++)
{
kf.Push(x[i], y[i]);
}
// Estimate the points location
double newX = kf.X; // should be 99.004000000912569
double newY = kf.Y; // should be 415.99224675780607
// Estimate the points velocity
double velX = kf.XAxisVelocity; // should be 1.0020000004925984
double velY = kf.YAxisVelocity; // should be 4.1356394152821094
#endregion
Assert.AreEqual(1.0020000004925984, velX);
Assert.AreEqual(4.1356394152821094, velY);
Assert.AreEqual(99.004000000912569, newX);
Assert.AreEqual(415.99224675780607, newY);
}
private void ApplyKalmanFilter(int objectId, ICoordinate result, DateTime time)
{
if (_particles[objectId].GetAcceleration(result, time, 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;
}
}
}
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);
}
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);
}
public AccordKalmanFilterWrapper(double noise)
{
// Create a new Kalman filter
kf = new KalmanFilter2D();
kf.NoiseY = noise;
}
