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 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 OnTranslation(int direction, int distance)
{
int kernelSize = (distance + 1) * 3;
if (kernelSize % 2 == 0)
{
kernelSize++;
}
double[] tempKernel = new double[kernelSize];
double[] kernel = new double[kernelSize];
Array.Clear(_tempPdf, 0, _tempPdf.Length);
for (int index = 0; index < _belief.Length; index += 4)
{
if (!IsFree(index))
{
continue;
}
int[] indices = MathHelper.GetMultiDimensionalIndices(index, WorldShape);
for (int j = 0; j < 2; j++)
{
ComputeTranslationKernel(index + j, distance, tempKernel, kernel);
TransposedConvolutionTillOccupancy(index + j, j, indices[j], 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
});
}
private static void OnLocalizationUpdated(
object sender, LocalizationEventArgs e)
{
LocalizationUpdated?.Invoke(
sender, e);
}