public void OneRayPointPrecisionMatrixTest()
{
var center = new Point(1000, 0);
var angleStandardDeviation = Math.Atan(0.25 / 1000);
const double expectedYVariance = 0.25 * 0.25;
const double expectedXVariance = 1.0;
var graph = new Graph();
var point2DState = new Point2DState(new Point(1001.7, 0.1));
graph.AddObservation(new VectorFromOriginObservation(new Point(0, 0), 0, point2DState, angleStandardDeviation));
graph.AddObservation(new Point2DObservation(center, new[] { 1.0, 0.0, 0.0, .001 }, point2DState));
graph.Optimize(20);
var precisionMatrix = point2DState.GetPrecisionMatrix();
Assert.AreEqual(point2DState.Estimate.X, center.X, 1e-12);
Assert.AreEqual(point2DState.Estimate.Y, center.Y, 1e-12);
Assert.AreEqual(0, precisionMatrix[1], 1e-12);
Assert.AreEqual(0, precisionMatrix[2], 1e-12);
Assert.AreEqual(1 / expectedXVariance, precisionMatrix[0], 1e-4);
Assert.AreEqual(1 / expectedYVariance + 0.001, precisionMatrix[3], 1e-6);
}
public void TwoRaysPrecisionMatrixTest()
{
var center = new Point(1000, 1000);
var angleStandardDeviation = Math.Atan(0.25 / 1000);
const double expectedVariance = 0.25 * 0.25;
var graph = new Graph();
var point2DState = new Point2DState(center + new Vector(2.3, -3.4));
graph.AddObservation(new VectorFromOriginObservation(new Point(1000, 0), 0.5 * Math.PI, point2DState, angleStandardDeviation));
graph.AddObservation(new VectorFromOriginObservation(new Point(0, 1000), 0, point2DState, angleStandardDeviation));
graph.Optimize(20);
Assert.AreEqual(point2DState.Estimate.X, center.X, 1e-12);
Assert.AreEqual(point2DState.Estimate.Y, center.Y, 1e-12);
var precisionMatrix = point2DState.GetPrecisionMatrix();
Assert.AreEqual(0, precisionMatrix[1], 1e-12);
Assert.AreEqual(0, precisionMatrix[2], 1e-12);
Assert.AreEqual(1 / expectedVariance, precisionMatrix[0], 1e-3);
Assert.AreEqual(1 / expectedVariance, precisionMatrix[3], 1e-3);
}
public void ThreeRayPrecisionMatrixTest()
{
var center = new Point(0, 0);
var angleStandardDeviation = Math.Atan(0.25 / 1000);
const double expectedVariance = 0.25 * 0.25;
var graph = new Graph();
var point2DState = new Point2DState(center + new Vector(2.3, -3.4));
graph.AddObservation(new VectorFromOriginObservation(new Point(1000 / Math.Sqrt(2), 1000 / Math.Sqrt(2)), -0.75 * Math.PI, point2DState, angleStandardDeviation));
graph.AddObservation(new VectorFromOriginObservation(new Point(-1000 / Math.Sqrt(2), 1000 / Math.Sqrt(2)), -0.25 * Math.PI, point2DState, angleStandardDeviation));
graph.AddObservation(new VectorFromOriginObservation(new Point(-1000 / Math.Sqrt(2), -1000 / Math.Sqrt(2)), 0.25 * Math.PI, point2DState, angleStandardDeviation));
graph.Optimize(20);
var precisionMatrix = point2DState.GetPrecisionMatrix();
var rotationAngle = 0.25 * Math.PI;
double cosA = Math.Cos(rotationAngle);
double sinA = Math.Sin(rotationAngle);
var r = new[] { cosA, -sinA, cosA, sinA };
var covariance = Inverse2By2(precisionMatrix);
var rotatedCovariance = Multiply2By2(Multiply2By2(r, covariance), Transpose2By2(r));
var rotatedPrecisionMatrix = Inverse2By2(rotatedCovariance);
Assert.AreEqual(point2DState.Estimate.X, center.X, 1e-12);
Assert.AreEqual(point2DState.Estimate.Y, center.Y, 1e-12);
Assert.AreEqual(0, rotatedPrecisionMatrix[1], 1e-3);
Assert.AreEqual(0, rotatedPrecisionMatrix[2], 1e-3);
Assert.AreEqual(2 / expectedVariance, rotatedPrecisionMatrix[0], 1e-2);
Assert.AreEqual(1 / expectedVariance, rotatedPrecisionMatrix[3], 1e-2);
}