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
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
