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); }