def importance_factor(self, bigQ, blob, pseudoblob): ''' Calculate the relative importance of this measurement (weight) to be used as part of resampling Input: np.ndarray bigQ (measurement covariance) Blob blob (recieved measurement) Blob pseudoblob (estimated measurement) ''' v1 = 2.0*math.pi *magnitude(bigQ) v1 = pow(v1, -0.5) delz = blob_to_matrix(blob) - blob_to_matrix(pseudoblob) delzt = delz.T v2 = math.exp(-0.5 * mm(mm(delzt, inverse(bigQ)), delz)) return v1 * v2
def importance_factor(self, bigQ, blob, pseudoblob): ''' Calculate the relative importance of this measurement (weight) to be used as part of resampling Input: np.ndarray bigQ (measurement covariance) Blob blob (recieved measurement) Blob pseudoblob (estimated measurement) ''' v1 = 2.0 * math.pi * magnitude(bigQ) v1 = pow(v1, -0.5) delz = blob_to_matrix(blob) - blob_to_matrix(pseudoblob) delzt = delz.T v2 = math.exp(-0.5 * mm(mm(delzt, inverse(bigQ)), delz)) return v1 * v2
def update_mean(self, kalman_gain, measure, expected_measure): ''' Update the mean of a known feature based on the calculated Kalman gain and the different between the given measurement and the expected measurement Input: np.ndarray kalman_gain np.ndarray measure(ment) np.ndarray expected_measure(ment) Output: None ''' if self.__immutable__: return None delz = blob_to_matrix(measure) - blob_to_matrix(expected_measure) adjust = mm(kalman_gain, delz) self.mean = self.mean + adjust self.update_count += 1