def kalman_gain(self, feature_id, bigH, Qinv):
'''
Calculate the kalman gain for the update of the feature based on the
existing covariance of the feature, bigH, and the inverse of bigQ
Input:
int feature_id
np.ndarray bigH (Jacobian of the measurement model)
np.ndarray Qinv (inverse of Q, the measurement covariance)
Output:
numpy.ndarray
'''
old_covar = self.get_feature_by_id(feature_id).covar
return Matrix(mm(mm(old_covar, bigH.T), Qinv))
def kalman_gain(self, feature_id, bigH, Qinv):
'''
Calculate the kalman gain for the update of the feature based on the
existing covariance of the feature, bigH, and the inverse of bigQ
Input:
int feature_id
np.ndarray bigH (Jacobian of the measurement model)
np.ndarray Qinv (inverse of Q, the measurement covariance)
Output:
numpy.ndarray
'''
old_covar = self.get_feature_by_id(feature_id).covar
return Matrix(mm(mm(old_covar, bigH.T), Qinv))
def update_covar(self, kalman_gain, bigH):
'''
Update the covariance of a known feature based on the calculated Kalman
gain and the Jacobian of the measurement model (bigH)
Input:
np.ndarray kalman_gain
np.ndarray bigH
Output:
None
'''
if self.__immutable__:
return None
adjust = msubtract(self.identity, mm(kalman_gain, bigH))
self.covar = mm(adjust, self.covar)
self.update_count += 1
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_covar(self, kalman_gain, bigH):
'''
Update the covariance of a known feature based on the calculated Kalman
gain and the Jacobian of the measurement model (bigH)
Input:
np.ndarray kalman_gain
np.ndarray bigH
Output:
None
'''
if self.__immutable__:
return None
adjust = msubtract(self.identity, mm(kalman_gain, bigH))
self.covar = mm(adjust, self.covar)
self.update_count += 1
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 measurement_covariance(self, bigH, feature_id, Qt):
'''
Calculate the covarance of the measurement with respect to the given
Jacobian bigH, the feature's covariance and Qt (measurement noise?)
Input:
np.ndarray bigH (Jacobian of the measurement model)
int feature_id
np.ndarray Qt (measurement noise)
Output:
numpy.ndarray
'''
old_covar = self.get_feature_by_id(feature_id).covar
other = mm(mm(bigH, old_covar), bigH.T)
compilation = madd(other, Qt)
return Matrix(compilation)
def measurement_covariance(self, bigH, feature_id, Qt):
'''
Calculate the covarance of the measurement with respect to the given
Jacobian bigH, the feature's covariance and Qt (measurement noise?)
Input:
np.ndarray bigH (Jacobian of the measurement model)
int feature_id
np.ndarray Qt (measurement noise)
Output:
numpy.ndarray
'''
old_covar = self.get_feature_by_id(feature_id).covar
other = mm(mm(bigH, old_covar), bigH.T)
compilation = madd(other, Qt)
return Matrix(compilation)
def cam_observation_update(self, cam_obs):
'''Single bearing-color observation'''
zt = Matrix([
cam_obs.bearing, cam_obs.color.r, cam_obs.color.g, cam_obs.color.b
])
self.motion_update(self.last_twist)
for particle in self.robot_particles:
j = particle.get_feature_id(zt)
if j < 0: # not seen before
# note, this will automagically add a new feature if possible
particle.weight = self.add_hypothesis(particle.state, zt)
else: # j seen before
feature = particle.get_feature_by_id(j)
# pylint: disable=line-too-long
# naming explains functionality
z_hat = particle.measurement_prediction(
feature.mean, particle.state)
H = self.jacobian_of_motion_model(particle.state, feature.mean)
Q = mm(mm(H, feature.covar), transpose(H)) + self.Qt
Q_inverse = inverse(Q)
K = mm(mm(feature.covar, transpose(H)), Q_inverse)
new_mean = feature.mean + mm(K, zt - z_hat)
new_covar = mm(identity(5) - mm(K, H), feature.covar)
particle.replace_feature_ekf(j, new_mean, new_covar)
particle.weight = pow(
2 * math.pi * magnitude(Q), -1 / 2) * math.exp(
-0.5 * (transpose(zt - z_hat) * Q_inverse *
(zt - z_hat)))
# endif
# for all other features...do nothing
# end for
temp_particle_list = []
sum_ = 0
for particle in self.robot_particles:
sum_ = sum_ + particle.weight
chosen = random() * sum_
for _ in range(0, len(self.robot_particles)):
for particle in self.robot_particles:
chosen = chosen - particle.weight
if chosen < 0:
# choose this particle
temp_particle_list.append(particle.deep_copy())
self.robot_particles = temp_particle_list
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
def cam_observation_update(self, cam_obs):
'''Single bearing-color observation'''
zt = Matrix([cam_obs.bearing,
cam_obs.color.r, cam_obs.color.g, cam_obs.color.b])
self.motion_update(self.last_twist)
for particle in self.robot_particles:
j = particle.get_feature_id(zt)
if j < 0: # not seen before
# note, this will automagically add a new feature if possible
particle.weight = self.add_hypothesis(particle.state, zt)
else: # j seen before
feature = particle.get_feature_by_id(j)
# pylint: disable=line-too-long
# naming explains functionality
z_hat = particle.measurement_prediction(feature.mean, particle.state)
H = self.jacobian_of_motion_model(particle.state, feature.mean)
Q = mm(mm(H, feature.covar), transpose(H)) + self.Qt
Q_inverse = inverse(Q)
K = mm(mm(feature.covar, transpose(H)), Q_inverse)
new_mean = feature.mean + mm(K, zt - z_hat)
new_covar = mm(identity(5) - mm(K, H), feature.covar)
particle.replace_feature_ekf(j, new_mean, new_covar)
particle.weight = pow(2*math.pi*magnitude(Q), -1/2) * math.exp(-0.5 * (transpose(zt - z_hat)*Q_inverse*(zt - z_hat)))
# endif
# for all other features...do nothing
# end for
temp_particle_list = []
sum_ = 0
for particle in self.robot_particles:
sum_ = sum_ + particle.weight
chosen = random()*sum_
for _ in range(0, len(self.robot_particles)):
for particle in self.robot_particles:
chosen = chosen - particle.weight
if chosen < 0:
# choose this particle
temp_particle_list.append(particle.deep_copy())
self.robot_particles = temp_particle_list
