Пример #1
0
        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];
        }
Пример #2
0
        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);
        }
Пример #3
0
 public void RestoreControllerState(SlamControllerState state)
 {
     memento = state;
 }