def update_svs(self, svs_obs):
OVERWRITE_DEPTH = True
if OVERWRITE_DEPTH:
self.states[:, 2] = svs_obs
else:
pred_svs_obs = self.states[:, 2].copy()
# Fix the shape so it is two-dimensional
pred_svs_obs.shape += (1,)
loglikelihood = np.log(misctools.mvnpdf(np.array([[svs_obs]]),
pred_svs_obs,
np.array([[[0.2]]])) +
np.finfo(np.double).eps)
loglikelihood -= max(loglikelihood)
likelihood = np.exp(loglikelihood)
self.weights *= likelihood
self.weights /= self.weights.sum()
def update_gps(self, gps_obs):
USE_KF = True
if USE_KF == True:
obs_matrix = np.array([self.parameters.state_obs_fn.parameters.gpsH])
obs_noise = np.array([self.parameters.state_likelihood_fn.parameters.gps_obs_noise])
upd_weights, upd_states, upd_covs = \
g500_kf_update(self.weights, self.states, self.covariances,
obs_matrix, obs_noise, gps_obs)
self.weights = upd_weights
self.states = upd_states
self.covariances = upd_covs
else:
pred_gps_obs = np.array(self.states[:, 0:2])
likelihood = misctools.mvnpdf(np.array([gps_obs]), pred_gps_obs,
np.array([self.parameters.state_likelihood_fn.parameters.gps_obs_noise]))
self.weights *= likelihood
self.weights /= self.weights.sum()
def update_dvl(self, dvl_obs):
USE_KF = True
if USE_KF:
obs_matrix = np.array([self.parameters.state_obs_fn.parameters.dvlH])
obs_noise = np.array([self.parameters.state_likelihood_fn.parameters.dvl_obs_noise])
upd_weights, upd_states, upd_covs = \
g500_kf_update(self.weights, self.states, self.covariances,
obs_matrix, obs_noise, dvl_obs)
self.weights = upd_weights
self.states = upd_states
self.covariances = upd_covs
else:
pred_dvl_obs = np.array(self.states[:, 3:])
npa_dvl_obs = np.array([dvl_obs])
cov = np.array([self.parameters.state_likelihood_fn.parameters.dvl_obs_noise])
loglikelihood = np.log(misctools.mvnpdf(npa_dvl_obs, pred_dvl_obs, cov) +
np.finfo(np.double).eps)
loglikelihood -= max(loglikelihood)
likelihood = np.exp(loglikelihood)
self.weights *= likelihood
self.weights /= self.weights.sum()
def update(self, observations, observation_noise):
self.flags.ESTIMATE_IS_VALID = False
# Container for slam parent update
slam_info = STRUCT()
slam_info.likelihood = 1
num_observations, z_dim = (observations.shape + (3,))[0:2]
if not self.weights.shape[0]:
return slam_info
detection_probability = self.camera_pd(self.parent_ned,
self.parent_rpy, self.states)
#detection_probability[detection_probability<0.1] = 0
clutter_pdf = self.camera_clutter(observations)
clutter_intensity = self.vars.clutter_intensity*clutter_pdf
self.flags.LOCK.acquire()
try:
# Account for missed detection
prev_weights = self.weights.copy()
prev_states = self.states.copy()
prev_covs = self.covs.copy()
updated = STRUCT()
updated.weights = [self.weights*(1-detection_probability)]
updated.states = [self.states]
updated.covs = [self.covs]
# Do the update only for detected landmarks
detected_indices = detection_probability >= 0
detected = STRUCT()
detected.weights = ( prev_weights[detected_indices]*
detection_probability[detected_indices] )
detected.states = prev_states[detected_indices]
detected.covs = prev_covs[detected_indices]
#ZZ SLAM, step 1:
slam_info.exp_sum__pd_predwt = np.exp(-detected.weights.sum())
# SLAM, prep for step 2:
slam_info.sum__clutter_with_pd_updwt = np.zeros(num_observations)
if detected.weights.shape[0]:
# Covariance update part of the Kalman update is common to all
# observation-updates
if observations.shape[0]:
h_mat = featuredetector.tf.relative_rot_mat(self.parent_rpy)
# Predicted observation from the current states
pred_z = featuredetector.tf.relative(self.parent_ned,
self.parent_rpy, detected.states)
observation_noise = observation_noise[0]
detected.covs, kalman_info = kf_update_cov(detected.covs,
h_mat,
observation_noise,
INPLACE=True)
# We need to update the states and find the updated weights
for (_observation_, obs_count) in zip(observations,
range(num_observations)):
#new_x = copy.deepcopy(x)
# Apply the Kalman update to get the new state -
# update in-place and return the residuals
upd_states, residuals = kf_update_x(detected.states, pred_z,
_observation_,
kalman_info.kalman_gain,
INPLACE=False)
# Calculate the weight of the Gaussians for this observation
# Calculate term in the exponent
#code.interact(local=locals())
#x_pdf = np.exp(-0.5*np.power(
# blas.dgemv(kalman_info.inv_sqrt_S,
# residuals, TRANSPOSE_A=True), 2).sum(axis=1))/ \
# np.sqrt(kalman_info.det_S*(2*np.pi)**z_dim)
x_pdf = misctools.mvnpdf(_observation_, pred_z, kalman_info.S)
upd_weights = detected.weights*x_pdf
# Normalise the weights
normalisation_factor = ( clutter_intensity[obs_count] +
#self.vars.birth_intensity +
upd_weights.sum() )
upd_weights /= normalisation_factor
#print "Obs Index: ", str(obs_count+1)
#print upd_weights.sum()
# SLAM, step 2:
slam_info.sum__clutter_with_pd_updwt[obs_count] = \
normalisation_factor
# Create new state with new_x and P to add to _states_
updated.weights += [upd_weights.copy()]
updated.states += [upd_states.copy()]
updated.covs += [detected.covs.copy()]
#print upd_weights.sum()
else:
slam_info.sum__clutter_with_pd_updwt = np.array(clutter_intensity)
self.weights = np.concatenate(updated.weights)
self.states = np.concatenate(updated.states)
self.covs = np.concatenate(updated.covs)
# SLAM, finalise:
slam_info.likelihood = (slam_info.exp_sum__pd_predwt *
slam_info.sum__clutter_with_pd_updwt.prod())
assert self.weights.shape[0] == self.states.shape[0] == self.covs.shape[0], "Lost states!!"
finally:
self.flags.LOCK.release()
return slam_info
