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