void CopyCovarianceTransform(EkfSlam slam, int index, Transform2D transform) { var m = index >= 0 ? 2 * index + 3 : 0; var mean = slam.Mean.Take(m, m + 1); var covariance = slam.Covariance.Take(m, m + 1); var eigendecomposition = covariance.Evd(); var eigenvalues = eigendecomposition.EigenValues(); var eigenvectors = eigendecomposition.EigenVectors(); // Transform of a unit circle var translation = new Vector2((float)mean[0], (float)mean[1]); var rotation = (float)Math.Atan2(eigenvectors[1, 0], eigenvectors[0, 0]); var scale = new Vector2((float)eigenvalues[0].Magnitude, (float)eigenvalues[1].Magnitude); transform.Position = translation; transform.Rotation = rotation; transform.Scale = scale; }
public SlamController(Vehicle agent) { slam = new EkfSlam(); slam.MotionNoise = new DenseMatrix(new[, ] { { 0.01, 0, 0 }, { 0, 0.01, 0 }, { 0, 0, 0.02 } }); slam.MeasurementNoise = new DenseMatrix(new[, ] { { 1.0, 0 }, { 0, 1.0 } }); landmarkIndices = new LandmarkMappingCollection(); motion = new DenseVector(2); measurements = Enumerable.Empty <LandmarkMeasurement>(); vehicle = agent; }