slam_08_d_density_error_ellipse.py 文件源码

def probability_of_measurement(self, measurement, predicted_measurement):
        """Given a measurement and a predicted measurement, computes
           probability."""
        # Compute differences to real measurements.
        d_true, alpha_true = measurement
        d_pred, alpha_pred = predicted_measurement

        # --->>> Compute difference in distance and bearing angle.
        d_diff = abs(d_pred - d_true)
        alpha_diff = (alpha_pred - alpha_true + pi) % (2 * pi) - pi

        # Important: make sure the angle difference works correctly and does
        # not return values offset by 2 pi or - 2 pi.
        # You may use the following Gaussian PDF function:
        # scipy.stats.norm.pdf(x, mu, sigma). With the import in the header,
        # this is normal_dist.pdf(x, mu, sigma).
        p_d = normal_dist.pdf(d_diff, 0, self.measurement_distance_stddev)
        p_alpha = normal_dist.pdf(alpha_diff, 0, self.measurement_angle_stddev)

        # Note that the two parameters sigma_d and sigma_alpha discussed
        # in the lecture are self.measurement_distance_stddev and
        # self.measurement_angle_stddev.
        return p_d * p_alpha