示例#1
0
 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()
示例#2
0
 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()
示例#3
0
 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()
示例#4
0
 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