public void TestInterpolationOfParabola()
{
List<Point> points = new List<Point>()
{
new Point(3,2),
new Point(2,4),
new Point(8,1)
};
Parabola result = Parabola.fit(points, points.Count);
Parabola correct = new Parabola(1.5, -9.5, 16.0);
Assert.AreEqual(result.a, correct.a, delta, "coefficient 'a' is wrong.");
Assert.AreEqual(result.b, correct.b, delta, "coefficient 'b' is wrong.");
Assert.AreEqual(result.c, correct.c, delta, "coefficient 'c' is wrong.");
}
public void TestFittingLine()
{
List<Point> points = new List<Point>()
{
new Point(32,30),
new Point(22,20),
new Point(5,3)
};
Parabola result = Parabola.fit(points, points.Count);
Parabola correct = new Parabola(0.0, 1.0, 2.0);
Assert.AreEqual(result.a, correct.a, delta, "coefficient 'a' is wrong.");
Assert.AreEqual(result.b, correct.b, delta, "coefficient 'b' is wrong.");
Assert.AreEqual(result.c, correct.c, delta, "coefficient 'c' is wrong.");
}
public void TestFittingRandomData()
{
List<Point> points = new List<Point>()
{
new Point(0, 31),
new Point(-1, 7),
new Point(-3, 3),
new Point(-5, 9),
new Point(-7, 5),
new Point(-10, 1),
new Point(-15, 7),
new Point(-25, 3)
};
Parabola result = Parabola.fit(points, points.Count);
Parabola correct = new Parabola(-0.02011, 1.08419, -14.2176);
Assert.AreEqual(result.a, correct.a, delta, "coefficient 'a' is wrong.");
Assert.AreEqual(result.b, correct.b, delta, "coefficient 'b' is wrong.");
Assert.AreEqual(result.c, correct.c, delta, "coefficient 'c' is wrong.");
}
public void TestFittingRandomData2()
{
List<Point> points = new List<Point>()
{
new Point(0, 720),
new Point(320, 245),
new Point(54, 245),
new Point(32, 689),
new Point(754, 530),
new Point(24, 732),
new Point(240, 323)
};
Parabola result = Parabola.fit(points, points.Count);
Parabola correct = new Parabola(-0.010085, 9.47823, -1583.3);
Assert.AreEqual(result.a, correct.a, delta, "coefficient 'a' is wrong.");
Assert.AreEqual(result.b, correct.b, delta, "coefficient 'b' is wrong.");
Assert.AreEqual(result.c, correct.c, delta, "coefficient 'c' is wrong.");
}
public Parabola FeedParabola(Parabola parabola)
{
if (variablesCount != 3)
throw new InvalidVariablesCountException();
if (!isInitialized)
{
kalman.CorrectedState[0, 0] = (float)parabola.a;
kalman.CorrectedState[1, 0] = (float)parabola.b;
kalman.CorrectedState[2, 0] = (float)parabola.c;
isInitialized = true;
return parabola;
}
Matrix<float> meas = new Matrix<float>(3, 1);
meas[0, 0] = (float)parabola.a;
meas[1, 0] = (float)parabola.b;
meas[2, 0] = (float)parabola.c;
kalman.Predict();
Matrix<float> estimation = kalman.Correct(meas);
return new Parabola(estimation[0, 0], estimation[1, 0], estimation[2, 0]);
}
public static Parabola Moved(Parabola old, double offset)
{
Parabola b = new Parabola(old.a, old.b, old.c);
b.moveHorizontal(offset);
return b;
}
public static Parabola merge(Parabola a, Parabola b, double t = 0.5)
{
return new Parabola((a.a + b.a) * t,
(a.b + b.b) * t,
(a.c + b.c) * t);
}
/// <summary>
/// Returns average distance sampled from 4 points
/// </summary>
/// <param name="a">first parabola</param>
/// <param name="b">second parabola</param>
/// <returns>average distance</returns>
public static double distance(Parabola a, Parabola b)
{
double sum = 0;
for (double x = 10; x <= 90; x += 20)
sum += a.at(x) - b.at(x);
return sum * 0.25;
}
public Parabola(Parabola p)
{
a = p.a;
b = p.b;
c = p.c;
}
