/// <summary>
/// Obtain a measurement from the hidden state.
/// This method always detects the landmark (misdetection
/// probability is ignored).
/// </summary>
/// <param name="landmark">Landmark 3d location against which the measurement is performed.</param>
/// <returns>Pixel-range measurements.</returns>
public MeasurementT MeasureDetected(double[] landmark)
{
MeasurementT measurement = Measurer.MeasurePerfect(Pose, landmark);
double[] mlinear = measurement.ToLinear();
double[] noise = Util.RandomGaussianVector(new double[mlinear.Length], MeasurementCovariance);
return(measurement.FromLinear(mlinear.Add(noise)));
}
/// <summary>
/// Get the probability of detection of a particular landmark.
/// It is modelled as a constant if it's on the FOV and zero if not.
/// </summary>
/// <param name="landmark">Queried landmark.</param>
/// <returns></returns>
public double DetectionProbability(double[] landmark)
{
return(Measurer.FuzzyVisibleM(Measurer.MeasurePerfect(Pose, landmark)) * detectionProbability);
}
/// <summary>
/// Find if a given ladmark is visible from the current pose of the vehicle.
/// </summary>
/// <param name="landmark">Queried landmark.</param>
/// <returns>True if the landmark is visible; false otherwise.</returns>
public bool Visible(double[] landmark)
{
return(Measurer.VisibleM(Measurer.MeasurePerfect(Pose, landmark)));
}