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 }); }
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 }); }
public void OnMeasurementCompleted(int count) { MathHelper.Divide(_tempPdf, count); ProbabilityHelper.BayesTheorem(_belief, _tempPdf, MinProbability, _belief); LocalizationUpdated.Invoke(this, new LocalizationEventArgs() { Belief = _belief }); }
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)); }
/// <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)); }
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); }
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); }
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); }
public int ComputeProbabilisticDistance(int wetness) { double std = Std * (1 + Math.Min(WetnessFactor * wetness + Distance * DistanceFactor, 1)); return(ProbabilityHelper.SampleFromNormal(Distance, std)); }