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)