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