Beispiel #1
0
        public void OnRotation(int angle)
        {
            double[] rotationPdf = ProbabilityHelper.NormalDistribution(angle / 90, RotationStd);
            int      offset      = rotationPdf.Length / 2;

            double[] kernel = new double[4];
            for (int i = 0; i < rotationPdf.Length; i++)
            {
                kernel[MathHelper.GetOneTurn((i - offset) * 90) / 90] = rotationPdf[i];
            }

            double[] temp = new double[4];
            for (int index = 0; index < _belief.Length; index += 4)
            {
                if (!IsFree(index))
                {
                    continue;
                }
                MathHelper.TransposedCircularConvolution1d(index, _belief, kernel, temp);
                temp.CopyTo(_belief, index);
            }

            MathHelper.SetLowerBound(_belief, MinProbability);
            LocalizationUpdated.Invoke(this, new LocalizationEventArgs()
            {
                Belief = _belief
            });
        }
Beispiel #2
0
        public void OnElevation(int relativeFloor)
        {
            double[] kernel     = ProbabilityHelper.NormalDistribution(relativeFloor, ElevationStd);
            int      halfKernel = kernel.Length / 2;

            Array.Clear(_tempPdf, 0, _tempPdf.Length);
            int elevatorLabelIndex = Array.IndexOf(_mapLabels, StaticModel.ElevatorClassName);

            for (int index = 0; index < _viewMap.Length; index++)
            {
                if (_viewMap[index] == elevatorLabelIndex)
                {
                    int z = MathHelper.GetMultiDimensionalIndices(index, WorldShape)[2];
                    MathHelper.TransposedConvolution1d(_belief, index, z, _strides[2],
                                                       WorldShape[2], kernel, _tempPdf);
                }
            }

            _tempPdf.CopyTo(_belief, 0);
            MathHelper.SetLowerBound(_belief, MinProbability);
            LocalizationUpdated.Invoke(this, new LocalizationEventArgs()
            {
                Belief = _belief
            });
        }
Beispiel #3
0
 public void OnMeasurementCompleted(int count)
 {
     MathHelper.Divide(_tempPdf, count);
     ProbabilityHelper.BayesTheorem(_belief, _tempPdf, MinProbability, _belief);
     LocalizationUpdated.Invoke(this, new LocalizationEventArgs()
     {
         Belief = _belief
     });
 }
Beispiel #4
0
        public int ComputeProbabilisticDestinationFloor(int currentFloor, int floorBound)
        {
            double[] distrib = new double[floorBound + 1];
            ProbabilityHelper.SetUniformDistribution(distrib);
            int expected = currentFloor + RelativeFloor;

            if (expected <= floorBound && expected >= 0)
            {
                distrib[expected] = SuccessProb;
                ProbabilityHelper.NormalizePdf(distrib, 0);
            }
            ProbabilityHelper.PdfToCdf(distrib);
            return(ProbabilityHelper.Sample(distrib));
        }
Beispiel #5
0
        /// <summary>
        /// Получить вероятностный индекс класса по распределению из матрицы ошибок
        /// </summary>
        /// <param name="trueClassName">Истинное название класса</param>
        /// <returns></returns>
        public int GetProbabalisticClassIndex(string trueClassName)
        {
            int trueClassIndex = Array.IndexOf(KnownClasses, trueClassName);

            if (trueClassIndex == -1)
            {
                trueClassIndex = Array.IndexOf(KnownClasses, ModelBase.UnknownClassName);
            }

            double[] distrib = new double[KnownClasses.Length];
            for (int i = 0; i < KnownClasses.Length; i++)
            {
                distrib[i] = ConfusionMatrix[trueClassIndex, i];
            }
            ProbabilityHelper.NormalizePdf(distrib, 0);
            ProbabilityHelper.PdfToCdf(distrib);
            return(ProbabilityHelper.Sample(distrib));
        }
Beispiel #6
0
        public Dictionary <string, double> Measure(ModelBase model, int orientation)
        {
            double[] distrib = new double[KnownClasses.Length];

            if (new Random().NextDouble() < SuccessProb)
            {
                //Нормальная работа камеры
                var   obj            = _world.Find(model);
                int   absOrientation = MathHelper.GetOneTurn(obj.Orientation + orientation);
                Point point          = obj.Point + new Point(absOrientation);

                string seenClass = _world.FindOrDefault(point)?.Model.Class;
                if (seenClass == null)
                {
                    seenClass = ModelBase.EmptyClassName;
                }

                //Превращение точного прогноза в вероятностное
                int probClassIndex = GetProbabalisticClassIndex(seenClass);

                //Составление распределения вероятности по предполагаемому классу
                for (int i = 0; i < KnownClasses.Length; i++)
                {
                    distrib[i] = ConfusionMatrix[i, probClassIndex];
                }
                ProbabilityHelper.NormalizePdf(distrib, 0);
            }
            else
            {
                //Сбой камеры - вероятность распределяется случайно
                ProbabilityHelper.SetRandomDistribution(distrib);
            }

            //Запись предсказания в словарь
            var prediction = new Dictionary <string, double>();

            for (int i = 0; i < KnownClasses.Length; i++)
            {
                prediction.Add(KnownClasses[i], distrib[i]);
            }
            return(prediction);
        }
Beispiel #7
0
        public NavigationSystem(World world)
        {
            Point upper = world.UpperBound;

            WorldShape = new int[] { upper.X + 1, upper.Y + 1, upper.Z + 1, 4 };
            _strides   = MathHelper.GetIndexStrides(WorldShape);

            int length = WorldShape[0] * WorldShape[1] * WorldShape[2] * WorldShape[3];

            _belief       = new double[length];
            _viewMap      = new int[length];
            _occupancyMap = new int[length / 4];

            _tempPdf        = new double[length];
            _gaussianKernel = ProbabilityHelper.NormalDistribution(0, _translationStd);

            //Инициализация убеждения равномерным распределением
            ProbabilityHelper.SetUniformDistribution(_belief);
            FillMaps(world);
        }
Beispiel #8
0
        public int ComputeProbabilisticAngle(int wetness)
        {
            double std = Std * (1 + Math.Min(WetnessFactor * wetness + Math.Abs(Angle / 90) * AngleFactor, 1));

            return(ProbabilityHelper.SampleFromNormal(Angle / 90, std) * 90);
        }
Beispiel #9
0
        public int ComputeProbabilisticDistance(int wetness)
        {
            double std = Std * (1 + Math.Min(WetnessFactor * wetness + Distance * DistanceFactor, 1));

            return(ProbabilityHelper.SampleFromNormal(Distance, std));
        }