public void UpdateEstimate()
{
if (memento != null)
{
var covarianceOrder = (int)Math.Sqrt(memento.Covariance.Length);
var motionNoiseOrder = (int)Math.Sqrt(memento.MotionNoise.Length);
var measurementNoiseOrder = (int)Math.Sqrt(memento.MeasurementNoise.Length);
slam.Mean = new DenseVector(memento.Mean);
slam.Covariance = new DenseMatrix(covarianceOrder, covarianceOrder, memento.Covariance);
slam.MotionNoise = new DenseMatrix(motionNoiseOrder, motionNoiseOrder, memento.MotionNoise);
slam.MeasurementNoise = new DenseMatrix(measurementNoiseOrder, measurementNoiseOrder, memento.MeasurementNoise);
landmarkIndices.Clear();
foreach (var mapping in memento.LandmarkIndices)
{
landmarkIndices.Add(mapping);
}
memento = null;
}
slam.Update(motion, measurements);
// Wrap angle
slam.Mean[2] = MathHelper.WrapAngle((float)slam.Mean[2]);
vehicle.Transform.Position = new Vector2((float)slam.Mean[0], (float)slam.Mean[1]);
vehicle.Transform.Rotation = (float)slam.Mean[2];
}
public SlamControllerState StoreControllerState()
{
var state = new SlamControllerState();
state.Mean = slam.Mean.ToArray();
state.Covariance = slam.Covariance.ToColumnWiseArray();
state.MotionNoise = slam.MotionNoise.ToColumnWiseArray();
state.MeasurementNoise = slam.MeasurementNoise.ToColumnWiseArray();
foreach (var mapping in landmarkIndices)
{
state.LandmarkIndices.Add(mapping);
}
return(state);
}
public void RestoreControllerState(SlamControllerState state)
{
memento = state;
}