def phdUpdate(self, observation_set):
# Container for slam parent update
slam_info = PARAMETERS()
num_observations = observation_set.shape[0]
if num_observations:
z_dim = observation_set.shape[1]
else:
z_dim = 0
if not self.weights.shape[0]:
self._states_ = self.states.copy()
self._weights_ = self.weights.copy()
return
detection_probability = self.parameters.pd_fn.handle(self.states,
self.parameters.pd_fn.parameters)
#clutter_pdf = [self.clutter_fn.handle(_observation_,
# self.clutter_fn.parameters) \
# for _observation_ in observation_set]
clutter_pdf = self.parameters.clutter_fn.handle(observation_set,
self.parameters.clutter_fn.parameters)
# Account for missed detection
self._states_ = self.states.copy()
self._weights_ = [self.weights*(1-detection_probability)]
# SLAM, step 1:
slam_info.exp_sum__pd_predwt = np.exp(-self.weights.sum())
# Split x and P out from the combined state vector
detected_indices = detection_probability > 0.1
detected_states = self.states[detected_indices]
x = detected_states.state
P = detected_states.covariance
# Scale the weights by detection probability
weights = self.weights[detected_indices]*detection_probability[detected_indices]
# SLAM, prep for step 2:
slam_info.sum__clutter_with_pd_updwt = np.zeros(num_observations)
if x.shape[0]:
# Part of the Kalman update is common to all observation-updates
x, P, kalman_info = kf_update(x, P,
np.array([self.parameters.obs_fn.parameters.H]),
np.array([self.parameters.obs_fn.parameters.R]),
None, INPLACE=True)#USE_NP=0)
# Container for the updated states
new_gmstate = self.states.__class__(0)
# Predicted observation from the current states
pred_z = featuredetector.tf.relative(self.parameters.obs_fn.parameters.parent_state_xyz,
self.parameters.obs_fn.parameters.parent_state_rpy,
x)
#print "PREDICTED Z:"
#print pred_z
# We need to update the states and find the updated weights
for (_observation_, obs_count) in zip(observation_set,
range(num_observations)):
#new_x = copy.deepcopy(x)
# Apply the Kalman update to get the new state - update in-place
# and return the residuals
new_x, residuals = kf_update_x(x, pred_z,
_observation_, kalman_info.kalman_gain,
INPLACE=False)
code.interact(local=locals())
# Calculate the weight of the Gaussians for this observation
# Calculate term in the exponent
x_pdf = np.exp(-0.5*np.power(
blas.dgemv(kalman_info.inv_sqrt_S, residuals), 2).sum(axis=1))/ \
np.sqrt(kalman_info.det_S*(2*np.pi)**z_dim)
new_weight = weights*x_pdf
# Normalise the weights
normalisation_factor = clutter_pdf[obs_count] + new_weight.sum()
new_weight /= normalisation_factor
# 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_
new_gmstate.set(new_x, P)
self._states_.append(new_gmstate)
self._weights_ += [new_weight]
else:
slam_info.sum__clutter_with_pd_updwt = np.array(clutter_pdf)
self._weights_ = np.concatenate(self._weights_)
# SLAM, finalise:
slam_info.likelihood = (slam_info.exp_sum__pd_predwt *
slam_info.sum__clutter_with_pd_updwt.prod())
return slam_info
def g500_slam_fn_defs():
# Vehicle state prediction
state_markov_predict_fn_handle = None
state_markov_predict_fn_parameters = PARAMETERS()
state_markov_predict_fn = fn_params(state_markov_predict_fn_handle,
state_markov_predict_fn_parameters)
# Vehicle state to observation space
state_obs_fn_handle = None
state_obs_fn_parameters = PARAMETERS()
state_obs_fn_parameters.gpsH = np.hstack(( np.eye(2), np.zeros((2,4)) ))
state_obs_fn_parameters.dvlH = np.hstack(( np.zeros((3,3)), np.eye(3) ))
state_obs_fn = fn_params(state_obs_fn_handle, state_obs_fn_parameters)
# Likelihood function for importance sampling
state_likelihood_fn_handle = None
state_likelihood_fn_parameters = PARAMETERS()
state_likelihood_fn = fn_params(state_likelihood_fn_handle,
state_likelihood_fn_parameters)
# Update function for state
state__state_update_fn_handle = None
state__state_update_fn_parameters = PARAMETERS()
state__state_update_fn = fn_params(state__state_update_fn_handle,
state__state_update_fn_parameters)
# State estimation from particles
state_estimate_fn_handle = None #misctools.sample_mn_cv
state_estimate_fn_parameters = PARAMETERS()
state_estimate_fn = fn_params(state_estimate_fn_handle,
state_estimate_fn_parameters)
# Parameters for the filter
# Roll, pitch, yaw + velocities is common to all particles - we assume
# that the information from the imu is perfect
# We only need to estimate x,y,z. The roll, pitch and yaw must be fed
# externally
state_parameters = {"nparticles":13,
"ndims":6,
"resample_threshold":-1}
# Parameters for the PHD filter
feature_parameters = {"max_terms":100,
"elim_threshold":1e-4,
"merge_threshold":3,
"ndims":3}
ndims = feature_parameters["ndims"]
# Landmark state-prediction
feature_markov_predict_fn_handle = gmphdfilter.markov_predict
feature_markov_predict_fn_parameters = PARAMETERS()
feature_markov_predict_fn_parameters.F = np.eye(3)
feature_markov_predict_fn_parameters.Q = np.zeros(ndims)
feature_markov_predict_fn = fn_params(feature_markov_predict_fn_handle,
feature_markov_predict_fn_parameters)
# Landmark state-to-observation function
feature_obs_fn_handle = None
feature_obs_fn_parameters = PARAMETERS()
feature_obs_fn_parameters.H = np.eye(ndims)
feature_obs_fn_parameters.R = 0.1*np.eye(ndims)
feature_obs_fn_parameters.parent_state_xyz = np.zeros(3)
feature_obs_fn_parameters.parent_state_rpy = np.zeros(3)
feature_obs_fn = fn_params(feature_obs_fn_handle,
feature_obs_fn_parameters)
# Likelihood function - not used for the GM PHD filter
feature_likelihood_fn = fn_params()
# Landmark state update function - not used
feature__state_update_fn = fn_params()
# Clutter function
clutter_fn_handle = None#gmphdfilter.uniform_clutter
clutter_fn_parameters = PARAMETERS()
clutter_fn_parameters.intensity = 0.01
# Range should be the field of view of the sensor
clutter_fn_parameters.range = [[-1, 1], [-1, 1], [-1, 1]]
clutter_fn = fn_params(clutter_fn_handle, clutter_fn_parameters)
# Birth function
birth_fn_handle = camera_birth
birth_fn_parameters = PARAMETERS()
birth_fn_parameters.intensity = 0.001
birth_fn_parameters.obs2state = lambda x: np.array(x)
birth_fn_parameters.parent_state_xyz = np.zeros(3)
birth_fn_parameters.parent_state_rpy = np.zeros(3)
birth_fn_parameters.R = 0.1*np.eye(3)
birth_fn = fn_params(birth_fn_handle, birth_fn_parameters)
# Survival/detection probability
ps_fn_handle = gmphdfilter.constant_survival
ps_fn_parameters = PARAMETERS()
ps_fn_parameters.ps = 1
ps_fn = fn_params(ps_fn_handle, ps_fn_parameters)
pd_fn_handle = None
pd_fn_parameters = PARAMETERS()
pd_fn_parameters.width = 2.0
pd_fn_parameters.depth = 3.0
pd_fn_parameters.height = 1.0
pd_fn_parameters.pd = 0.98
pd_fn_parameters.parent_state_xyz = np.zeros(3)
pd_fn_parameters.parent_state_rpy = np.zeros(3)
pd_fn = fn_params(pd_fn_handle, pd_fn_parameters)
# Use default estimator
feature_estimate_fn = fn_params()
return SLAM_FN_DEFS(state_markov_predict_fn, state_obs_fn,
state_likelihood_fn, state__state_update_fn,
state_estimate_fn, state_parameters,
feature_markov_predict_fn, feature_obs_fn,
feature_likelihood_fn, feature__state_update_fn,
clutter_fn, birth_fn, ps_fn, pd_fn,
feature_estimate_fn, feature_parameters)
