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