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